util.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

#include <fcntl.h> /* low-level i/o */
#include <malloc.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <cassert>
#include <cerrno>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>

#include <immintrin.h>
#include <x86intrin.h>

namespace apollo {
namespace drivers {
namespace camera {

void yuyv2rgb_avx(unsigned char *YUV, unsigned char *RGB, int NumPixels);

#define SIMD_INLINE inline __attribute__((always_inline))

void print_m256(const __m256i a);
void print_m256_i32(const __m256i a);
void print_m256_i16(const __m256i a);

template <class T>
SIMD_INLINE char GetChar(T value, size_t index) {
  return (reinterpret_cast<char *>(&value))[index];
}

#define SIMD_CHAR_AS_LONGLONG(a) (((int64_t)a) & 0xFF)

#define SIMD_SHORT_AS_LONGLONG(a) (((int64_t)a) & 0xFFFF)

#define SIMD_INT_AS_LONGLONG(a) (((int64_t)a) & 0xFFFFFFFF)

#define SIMD_LL_SET1_EPI8(a)                                                \
  SIMD_CHAR_AS_LONGLONG(a) | (SIMD_CHAR_AS_LONGLONG(a) << 8) |              \
      (SIMD_CHAR_AS_LONGLONG(a) << 16) | (SIMD_CHAR_AS_LONGLONG(a) << 24) | \
      (SIMD_CHAR_AS_LONGLONG(a) << 32) | (SIMD_CHAR_AS_LONGLONG(a) << 40) | \
      (SIMD_CHAR_AS_LONGLONG(a) << 48) | (SIMD_CHAR_AS_LONGLONG(a) << 56)

#define SIMD_LL_SET2_EPI8(a, b)                                             \
  SIMD_CHAR_AS_LONGLONG(a) | (SIMD_CHAR_AS_LONGLONG(b) << 8) |              \
      (SIMD_CHAR_AS_LONGLONG(a) << 16) | (SIMD_CHAR_AS_LONGLONG(b) << 24) | \
      (SIMD_CHAR_AS_LONGLONG(a) << 32) | (SIMD_CHAR_AS_LONGLONG(b) << 40) | \
      (SIMD_CHAR_AS_LONGLONG(a) << 48) | (SIMD_CHAR_AS_LONGLONG(b) << 56)

#define SIMD_LL_SETR_EPI8(a, b, c, d, e, f, g, h)                           \
  SIMD_CHAR_AS_LONGLONG(a) | (SIMD_CHAR_AS_LONGLONG(b) << 8) |              \
      (SIMD_CHAR_AS_LONGLONG(c) << 16) | (SIMD_CHAR_AS_LONGLONG(d) << 24) | \
      (SIMD_CHAR_AS_LONGLONG(e) << 32) | (SIMD_CHAR_AS_LONGLONG(f) << 40) | \
      (SIMD_CHAR_AS_LONGLONG(g) << 48) | (SIMD_CHAR_AS_LONGLONG(h) << 56)

#define SIMD_LL_SET1_EPI16(a)                                     \
  SIMD_SHORT_AS_LONGLONG(a) | (SIMD_SHORT_AS_LONGLONG(a) << 16) | \
      (SIMD_SHORT_AS_LONGLONG(a) << 32) | (SIMD_SHORT_AS_LONGLONG(a) << 48)

#define SIMD_LL_SET2_EPI16(a, b)                                  \
  SIMD_SHORT_AS_LONGLONG(a) | (SIMD_SHORT_AS_LONGLONG(b) << 16) | \
      (SIMD_SHORT_AS_LONGLONG(a) << 32) | (SIMD_SHORT_AS_LONGLONG(b) << 48)

#define SIMD_LL_SETR_EPI16(a, b, c, d)                            \
  SIMD_SHORT_AS_LONGLONG(a) | (SIMD_SHORT_AS_LONGLONG(b) << 16) | \
      (SIMD_SHORT_AS_LONGLONG(c) << 32) | (SIMD_SHORT_AS_LONGLONG(d) << 48)

#define SIMD_LL_SET1_EPI32(a) \
  SIMD_INT_AS_LONGLONG(a) | (SIMD_INT_AS_LONGLONG(a) << 32)

#define SIMD_LL_SET2_EPI32(a, b) \
  SIMD_INT_AS_LONGLONG(a) | (SIMD_INT_AS_LONGLONG(b) << 32)

#define SIMD_MM256_SET1_EPI8(a)                                        \
  {                                                                    \
    SIMD_LL_SET1_EPI8(a)                                               \
    , SIMD_LL_SET1_EPI8(a), SIMD_LL_SET1_EPI8(a), SIMD_LL_SET1_EPI8(a) \
  }

#define SIMD_MM256_SET2_EPI8(a0, a1)                        \
  {                                                         \
    SIMD_LL_SET2_EPI8(a0, a1)                               \
    , SIMD_LL_SET2_EPI8(a0, a1), SIMD_LL_SET2_EPI8(a0, a1), \
        SIMD_LL_SET2_EPI8(a0, a1)                           \
  }

#define SIMD_MM256_SETR_EPI8(a0, a1, a2, a3, a4, a5, a6, a7, a8, a9, aa, ab, \
                             ac, ad, ae, af, b0, b1, b2, b3, b4, b5, b6, b7, \
                             b8, b9, ba, bb, bc, bd, be, bf)                 \
  {                                                                          \
    SIMD_LL_SETR_EPI8(a0, a1, a2, a3, a4, a5, a6, a7)                        \
    , SIMD_LL_SETR_EPI8(a8, a9, aa, ab, ac, ad, ae, af),                     \
        SIMD_LL_SETR_EPI8(b0, b1, b2, b3, b4, b5, b6, b7),                   \
        SIMD_LL_SETR_EPI8(b8, b9, ba, bb, bc, bd, be, bf)                    \
  }

#define SIMD_MM256_SET1_EPI16(a)                                          \
  {                                                                       \
    SIMD_LL_SET1_EPI16(a)                                                 \
    , SIMD_LL_SET1_EPI16(a), SIMD_LL_SET1_EPI16(a), SIMD_LL_SET1_EPI16(a) \
  }

#define SIMD_MM256_SET2_EPI16(a0, a1)                         \
  {                                                           \
    SIMD_LL_SET2_EPI16(a0, a1)                                \
    , SIMD_LL_SET2_EPI16(a0, a1), SIMD_LL_SET2_EPI16(a0, a1), \
        SIMD_LL_SET2_EPI16(a0, a1)                            \
  }

#define SIMD_MM256_SETR_EPI16(a0, a1, a2, a3, a4, a5, a6, a7, a8, a9, aa, ab, \
                              ac, ad, ae, af)                                 \
  {                                                                           \
    SIMD_LL_SETR_EPI16(a0, a1, a2, a3)                                        \
    , SIMD_LL_SETR_EPI16(a4, a5, a6, a7), SIMD_LL_SETR_EPI16(a8, a9, aa, ab), \
        SIMD_LL_SETR_EPI16(ac, ad, ae, af)                                    \
  }

#define SIMD_MM256_SET1_EPI32(a)                                          \
  {                                                                       \
    SIMD_LL_SET1_EPI32(a)                                                 \
    , SIMD_LL_SET1_EPI32(a), SIMD_LL_SET1_EPI32(a), SIMD_LL_SET1_EPI32(a) \
  }

#define SIMD_MM256_SET2_EPI32(a0, a1)                         \
  {                                                           \
    SIMD_LL_SET2_EPI32(a0, a1)                                \
    , SIMD_LL_SET2_EPI32(a0, a1), SIMD_LL_SET2_EPI32(a0, a1), \
        SIMD_LL_SET2_EPI32(a0, a1)                            \
  }

#define SIMD_MM256_SETR_EPI32(a0, a1, a2, a3, a4, a5, a6, a7) \
  {                                                           \
    SIMD_LL_SET2_EPI32(a0, a1)                                \
    , SIMD_LL_SET2_EPI32(a2, a3), SIMD_LL_SET2_EPI32(a4, a5), \
        SIMD_LL_SET2_EPI32(a6, a7)                            \
  }

const size_t A = sizeof(__m256i);
const size_t DA = 2 * A;
const size_t QA = 4 * A;
const size_t OA = 8 * A;
const size_t HA = A / 2;

const __m256i K_ZERO = SIMD_MM256_SET1_EPI8(0);
const __m256i K_INV_ZERO = SIMD_MM256_SET1_EPI8(0xFF);

const __m256i K8_01 = SIMD_MM256_SET1_EPI8(0x01);
const __m256i K8_02 = SIMD_MM256_SET1_EPI8(0x02);
const __m256i K8_04 = SIMD_MM256_SET1_EPI8(0x04);
const __m256i K8_08 = SIMD_MM256_SET1_EPI8(0x08);
const __m256i K8_10 = SIMD_MM256_SET1_EPI8(0x10);
const __m256i K8_20 = SIMD_MM256_SET1_EPI8(0x20);
const __m256i K8_40 = SIMD_MM256_SET1_EPI8(0x40);
const __m256i K8_80 = SIMD_MM256_SET1_EPI8(0x80);

const __m256i K8_01_FF = SIMD_MM256_SET2_EPI8(0x01, 0xFF);

const __m256i K16_0001 = SIMD_MM256_SET1_EPI16(0x0001);
const __m256i K16_0002 = SIMD_MM256_SET1_EPI16(0x0002);
const __m256i K16_0003 = SIMD_MM256_SET1_EPI16(0x0003);
const __m256i K16_0004 = SIMD_MM256_SET1_EPI16(0x0004);
const __m256i K16_0005 = SIMD_MM256_SET1_EPI16(0x0005);
const __m256i K16_0006 = SIMD_MM256_SET1_EPI16(0x0006);
const __m256i K16_0008 = SIMD_MM256_SET1_EPI16(0x0008);
const __m256i K16_0010 = SIMD_MM256_SET1_EPI16(0x0010);
const __m256i K16_0018 = SIMD_MM256_SET1_EPI16(0x0018);
const __m256i K16_0020 = SIMD_MM256_SET1_EPI16(0x0020);
const __m256i K16_0080 = SIMD_MM256_SET1_EPI16(0x0080);
const __m256i K16_00FF = SIMD_MM256_SET1_EPI16(0x00FF);
const __m256i K16_FF00 = SIMD_MM256_SET1_EPI16(0xFF00);

const __m256i K32_00000001 = SIMD_MM256_SET1_EPI32(0x00000001);
const __m256i K32_00000002 = SIMD_MM256_SET1_EPI32(0x00000002);
const __m256i K32_00000004 = SIMD_MM256_SET1_EPI32(0x00000004);
const __m256i K32_00000008 = SIMD_MM256_SET1_EPI32(0x00000008);
const __m256i K32_000000FF = SIMD_MM256_SET1_EPI32(0x000000FF);
const __m256i K32_0000FFFF = SIMD_MM256_SET1_EPI32(0x0000FFFF);
const __m256i K32_00010000 = SIMD_MM256_SET1_EPI32(0x00010000);
const __m256i K32_01000000 = SIMD_MM256_SET1_EPI32(0x01000000);
const __m256i K32_FFFFFF00 = SIMD_MM256_SET1_EPI32(0xFFFFFF00);

const __m256i K8_SHUFFLE_BGR0_TO_BLUE = SIMD_MM256_SETR_EPI8(
    0x0, 0x3, 0x6, 0x9, 0xC, 0xF, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, -1, -1, 0x2, 0x5, 0x8, 0xB, 0xE, -1, -1, -1, -1, -1);
const __m256i K8_SHUFFLE_BGR1_TO_BLUE = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x1, 0x4, 0x7, 0xA, 0xD, 0x0,
    0x3, 0x6, 0x9, 0xC, 0xF, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i K8_SHUFFLE_BGR2_TO_BLUE = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, 0x2, 0x5, 0x8, 0xB, 0xE, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x1, 0x4, 0x7, 0xA, 0xD);

const __m256i Y_SHUFFLE0 = SIMD_MM256_SETR_EPI8(
    0x0, 0x2, 0x4, 0x6, 0x8, 0xa, 0xc, 0xe, -1, -1, -1, -1, -1, -1, -1, -1, 0x0,
    0x2, 0x4, 0x6, 0x8, 0xa, 0xc, 0xe, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i Y_SHUFFLE1 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, 0x0, 0x2, 0x4, 0x6, 0x8, 0xa, 0xc, 0xe, -1,
    -1, -1, -1, -1, -1, -1, -1, 0x0, 0x2, 0x4, 0x6, 0x8, 0xa, 0xc, 0xe);

const __m256i U_SHUFFLE0 = SIMD_MM256_SETR_EPI8(
    0x1, 0x5, 0x9, 0xd, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x1,
    0x5, 0x9, 0xd, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i U_SHUFFLE1 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, 0x1, 0x5, 0x9, 0xd, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, 0x1, 0x5, 0x9, 0xd, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i U_SHUFFLE2 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, 0x1, 0x5, 0x9, 0xd, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, -1, -1, -1, 0x1, 0x5, 0x9, 0xd, -1, -1, -1, -1);

const __m256i U_SHUFFLE3 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x1, 0x5, 0x9, 0xd, -1, -1,
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x1, 0x5, 0x9, 0xd);
const __m256i U_SHUFFLE4 =
    SIMD_MM256_SETR_EPI8(0x0, 0x0, 0x0, 0x0, 0x4, 0x0, 0x0, 0x0, 0x1, 0x0, 0x0,
                         0x0, 0x5, 0x0, 0x0, 0x0, 0x2, 0x0, 0x0, 0x0, 0x6, 0x0,
                         0x0, 0x0, 0x3, 0x0, 0x0, 0x0, 0x7, 0x0, 0x0, 0x0);

const __m256i V_SHUFFLE0 = SIMD_MM256_SETR_EPI8(
    0x3, 0x7, 0xb, 0xf, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x3,
    0x7, 0xb, 0xf, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i V_SHUFFLE1 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, 0x3, 0x7, 0xb, 0xf, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, 0x3, 0x7, 0xb, 0xf, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i V_SHUFFLE2 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, 0x3, 0x7, 0xb, 0xf, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, -1, -1, -1, 0x3, 0x7, 0xb, 0xf, -1, -1, -1, -1);

const __m256i V_SHUFFLE3 = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x3, 0x7, 0xb, 0xf, -1, -1,
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x3, 0x7, 0xb, 0xf);

const __m256i K8_SHUFFLE_PERMUTED_BLUE_TO_BGR0 = SIMD_MM256_SETR_EPI8(
    0x0, -1, -1, 0x1, -1, -1, 0x2, -1, -1, 0x3, -1, -1, 0x4, -1, -1, 0x5, -1,
    -1, 0x6, -1, -1, 0x7, -1, -1, 0x8, -1, -1, 0x9, -1, -1, 0xA, -1);
const __m256i K8_SHUFFLE_PERMUTED_BLUE_TO_BGR1 = SIMD_MM256_SETR_EPI8(
    -1, 0x3, -1, -1, 0x4, -1, -1, 0x5, -1, -1, 0x6, -1, -1, 0x7, -1, -1, 0x8,
    -1, -1, 0x9, -1, -1, 0xA, -1, -1, 0xB, -1, -1, 0xC, -1, -1, 0xD);
const __m256i K8_SHUFFLE_PERMUTED_BLUE_TO_BGR2 = SIMD_MM256_SETR_EPI8(
    -1, -1, 0x6, -1, -1, 0x7, -1, -1, 0x8, -1, -1, 0x9, -1, -1, 0xA, -1, -1,
    0xB, -1, -1, 0xC, -1, -1, 0xD, -1, -1, 0xE, -1, -1, 0xF, -1, -1);

const __m256i K8_SHUFFLE_PERMUTED_GREEN_TO_BGR0 = SIMD_MM256_SETR_EPI8(
    -1, 0x0, -1, -1, 0x1, -1, -1, 0x2, -1, -1, 0x3, -1, -1, 0x4, -1, -1, 0x5,
    -1, -1, 0x6, -1, -1, 0x7, -1, -1, 0x8, -1, -1, 0x9, -1, -1, 0xA);
const __m256i K8_SHUFFLE_PERMUTED_GREEN_TO_BGR1 = SIMD_MM256_SETR_EPI8(
    -1, -1, 0x3, -1, -1, 0x4, -1, -1, 0x5, -1, -1, 0x6, -1, -1, 0x7, -1, -1,
    0x8, -1, -1, 0x9, -1, -1, 0xA, -1, -1, 0xB, -1, -1, 0xC, -1, -1);
const __m256i K8_SHUFFLE_PERMUTED_GREEN_TO_BGR2 = SIMD_MM256_SETR_EPI8(
    0x5, -1, -1, 0x6, -1, -1, 0x7, -1, -1, 0x8, -1, -1, 0x9, -1, -1, 0xA, -1,
    -1, 0xB, -1, -1, 0xC, -1, -1, 0xD, -1, -1, 0xE, -1, -1, 0xF, -1);

const __m256i K8_SHUFFLE_PERMUTED_RED_TO_BGR0 = SIMD_MM256_SETR_EPI8(
    -1, -1, 0x0, -1, -1, 0x1, -1, -1, 0x2, -1, -1, 0x3, -1, -1, 0x4, -1, -1,
    0x5, -1, -1, 0x6, -1, -1, 0x7, -1, -1, 0x8, -1, -1, 0x9, -1, -1);
const __m256i K8_SHUFFLE_PERMUTED_RED_TO_BGR1 = SIMD_MM256_SETR_EPI8(
    0x2, -1, -1, 0x3, -1, -1, 0x4, -1, -1, 0x5, -1, -1, 0x6, -1, -1, 0x7, -1,
    -1, 0x8, -1, -1, 0x9, -1, -1, 0xA, -1, -1, 0xB, -1, -1, 0xC, -1);
const __m256i K8_SHUFFLE_PERMUTED_RED_TO_BGR2 = SIMD_MM256_SETR_EPI8(
    -1, 0x5, -1, -1, 0x6, -1, -1, 0x7, -1, -1, 0x8, -1, -1, 0x9, -1, -1, 0xA,
    -1, -1, 0xB, -1, -1, 0xC, -1, -1, 0xD, -1, -1, 0xE, -1, -1, 0xF);

const __m256i K8_SHUFFLE_BGR0_TO_GREEN = SIMD_MM256_SETR_EPI8(
    0x1, 0x4, 0x7, 0xA, 0xD, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, 0x0, 0x3, 0x6, 0x9, 0xC, 0xF, -1, -1, -1, -1, -1);
const __m256i K8_SHUFFLE_BGR1_TO_GREEN = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x2, 0x5, 0x8, 0xB, 0xE, 0x1,
    0x4, 0x7, 0xA, 0xD, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i K8_SHUFFLE_BGR2_TO_GREEN = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, 0x0, 0x3, 0x6, 0x9, 0xC, 0xF, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x2, 0x5, 0x8, 0xB, 0xE);

const __m256i K8_SHUFFLE_BGR0_TO_RED = SIMD_MM256_SETR_EPI8(
    0x2, 0x5, 0x8, 0xB, 0xE, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, 0x1, 0x4, 0x7, 0xA, 0xD, -1, -1, -1, -1, -1, -1);
const __m256i K8_SHUFFLE_BGR1_TO_RED = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0x0, 0x3, 0x6, 0x9, 0xC, 0xF, 0x2,
    0x5, 0x8, 0xB, 0xE, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1);
const __m256i K8_SHUFFLE_BGR2_TO_RED = SIMD_MM256_SETR_EPI8(
    -1, -1, -1, -1, -1, 0x1, 0x4, 0x7, 0xA, 0xD, -1, -1, -1, -1, -1, -1, -1, -1,
    -1, -1, -1, -1, -1, -1, -1, -1, 0x0, 0x3, 0x6, 0x9, 0xC, 0xF);

const __m256i K16_Y_ADJUST = SIMD_MM256_SET1_EPI16(0);
const __m256i K16_UV_ADJUST = SIMD_MM256_SET1_EPI16(128);
const int Y_ADJUST = 0;
const int UV_ADJUST = 128;
const int YUV_TO_BGR_AVERAGING_SHIFT = 13;
const int YUV_TO_BGR_ROUND_TERM = 0;  // 1 << (YUV_TO_BGR_AVERAGING_SHIFT);
const int Y_TO_RGB_WEIGHT =
    static_cast<int>((((1 << YUV_TO_BGR_AVERAGING_SHIFT))));
const int U_TO_BLUE_WEIGHT =
    static_cast<int>((2.041 * (1 << YUV_TO_BGR_AVERAGING_SHIFT)));
const int U_TO_GREEN_WEIGHT =
    -static_cast<int>((0.3455 * (1 << YUV_TO_BGR_AVERAGING_SHIFT)));
const int V_TO_GREEN_WEIGHT =
    -static_cast<int>((0.7169 * (1 << YUV_TO_BGR_AVERAGING_SHIFT)));
const int V_TO_RED_WEIGHT =
    static_cast<int>((1.4065 * (1 << YUV_TO_BGR_AVERAGING_SHIFT)));

const __m256i K16_YRGB_RT =
    SIMD_MM256_SET2_EPI16(Y_TO_RGB_WEIGHT, YUV_TO_BGR_ROUND_TERM);
const __m256i K16_VR_0 = SIMD_MM256_SET2_EPI16(V_TO_RED_WEIGHT, 0);
const __m256i K16_UG_VG =
    SIMD_MM256_SET2_EPI16(U_TO_GREEN_WEIGHT, V_TO_GREEN_WEIGHT);
const __m256i K16_UB_0 = SIMD_MM256_SET2_EPI16(U_TO_BLUE_WEIGHT, 0);

template <bool align>
SIMD_INLINE __m256i Load(const __m256i *p);

template <>
SIMD_INLINE __m256i Load<false>(const __m256i *p) {
  return _mm256_loadu_si256(p);
}

template <>
SIMD_INLINE __m256i Load<true>(const __m256i *p) {
  return _mm256_load_si256(p);
}

SIMD_INLINE void *AlignLo(const void *ptr, size_t align) {
  return reinterpret_cast<void *>(((size_t)ptr) & ~(align - 1));
}

SIMD_INLINE bool Aligned(const void *ptr, size_t align = sizeof(__m256)) {
  return ptr == AlignLo(ptr, align);
}

template <bool align>
SIMD_INLINE void Store(__m256i *p, __m256i a);

template <>
SIMD_INLINE void Store<false>(__m256i *p, __m256i a) {
  _mm256_storeu_si256(p, a);
}

template <>
SIMD_INLINE void Store<true>(__m256i *p, __m256i a) {
  _mm256_store_si256(p, a);
}

SIMD_INLINE __m256i SaturateI16ToU8(__m256i value) {
  return _mm256_min_epi16(K16_00FF, _mm256_max_epi16(value, K_ZERO));
}

SIMD_INLINE __m256i AdjustY16(__m256i y16) {
  return _mm256_subs_epi16(y16, K16_Y_ADJUST);
}

SIMD_INLINE __m256i AdjustUV16(__m256i uv16) {
  return _mm256_subs_epi16(uv16, K16_UV_ADJUST);
}

SIMD_INLINE __m256i AdjustedYuvToRed32(__m256i y16_1, __m256i v16_0) {
  // print_m256_i16(y16_1);
  // print_m256_i16(v16_0);
  // print_m256_i32(_mm256_madd_epi16(y16_1, K16_YRGB_RT));
  // print_m256_i32(_mm256_madd_epi16(v16_0, K16_VR_0));
  return _mm256_srai_epi32(
      _mm256_add_epi32(_mm256_madd_epi16(y16_1, K16_YRGB_RT),
                       _mm256_madd_epi16(v16_0, K16_VR_0)),
      YUV_TO_BGR_AVERAGING_SHIFT);
}

SIMD_INLINE __m256i AdjustedYuvToRed16(__m256i y16, __m256i v16) {
  // print_m256_i32(AdjustedYuvToRed32(_mm256_unpacklo_epi16(y16, K16_0001),
  // _mm256_unpacklo_epi16(v16, K_ZERO)));
  // print_m256_i16(_mm256_unpacklo_epi16(y16, K16_0001));
  return SaturateI16ToU8(_mm256_packs_epi32(
      AdjustedYuvToRed32(_mm256_unpacklo_epi16(y16, K16_0001),
                         _mm256_unpacklo_epi16(v16, K_ZERO)),
      AdjustedYuvToRed32(_mm256_unpackhi_epi16(y16, K16_0001),
                         _mm256_unpackhi_epi16(v16, K_ZERO))));
}

SIMD_INLINE __m256i AdjustedYuvToGreen32(__m256i y16_1, __m256i u16_v16) {
  return _mm256_srai_epi32(
      _mm256_add_epi32(_mm256_madd_epi16(y16_1, K16_YRGB_RT),
                       _mm256_madd_epi16(u16_v16, K16_UG_VG)),
      YUV_TO_BGR_AVERAGING_SHIFT);
}

SIMD_INLINE __m256i AdjustedYuvToGreen16(__m256i y16, __m256i u16,
                                         __m256i v16) {
  return SaturateI16ToU8(_mm256_packs_epi32(
      AdjustedYuvToGreen32(_mm256_unpacklo_epi16(y16, K16_0001),
                           _mm256_unpacklo_epi16(u16, v16)),
      AdjustedYuvToGreen32(_mm256_unpackhi_epi16(y16, K16_0001),
                           _mm256_unpackhi_epi16(u16, v16))));
}

SIMD_INLINE __m256i AdjustedYuvToBlue32(__m256i y16_1, __m256i u16_0) {
  return _mm256_srai_epi32(
      _mm256_add_epi32(_mm256_madd_epi16(y16_1, K16_YRGB_RT),
                       _mm256_madd_epi16(u16_0, K16_UB_0)),
      YUV_TO_BGR_AVERAGING_SHIFT);
}

SIMD_INLINE __m256i AdjustedYuvToBlue16(__m256i y16, __m256i u16) {
  return SaturateI16ToU8(_mm256_packs_epi32(
      AdjustedYuvToBlue32(_mm256_unpacklo_epi16(y16, K16_0001),
                          _mm256_unpacklo_epi16(u16, K_ZERO)),
      AdjustedYuvToBlue32(_mm256_unpackhi_epi16(y16, K16_0001),
                          _mm256_unpackhi_epi16(u16, K_ZERO))));
}

SIMD_INLINE __m256i YuvToRed(__m256i y, __m256i v) {
  __m256i lo = AdjustedYuvToRed16(_mm256_unpacklo_epi8(y, K_ZERO),
                                  AdjustUV16(_mm256_unpacklo_epi8(v, K_ZERO)));
  __m256i hi = AdjustedYuvToRed16((_mm256_unpackhi_epi8(y, K_ZERO)),
                                  AdjustUV16(_mm256_unpackhi_epi8(v, K_ZERO)));

  // print_m256_i16(lo);
  // print_m256_i16(hi);
  return _mm256_packus_epi16(lo, hi);
}

SIMD_INLINE __m256i YuvToGreen(__m256i y, __m256i u, __m256i v) {
  __m256i lo =
      AdjustedYuvToGreen16((_mm256_unpacklo_epi8(y, K_ZERO)),
                           AdjustUV16(_mm256_unpacklo_epi8(u, K_ZERO)),
                           AdjustUV16(_mm256_unpacklo_epi8(v, K_ZERO)));
  __m256i hi =
      AdjustedYuvToGreen16((_mm256_unpackhi_epi8(y, K_ZERO)),
                           AdjustUV16(_mm256_unpackhi_epi8(u, K_ZERO)),
                           AdjustUV16(_mm256_unpackhi_epi8(v, K_ZERO)));
  return _mm256_packus_epi16(lo, hi);
}

SIMD_INLINE __m256i YuvToBlue(__m256i y, __m256i u) {
  __m256i lo = AdjustedYuvToBlue16((_mm256_unpacklo_epi8(y, K_ZERO)),
                                   AdjustUV16(_mm256_unpacklo_epi8(u, K_ZERO)));
  __m256i hi = AdjustedYuvToBlue16((_mm256_unpackhi_epi8(y, K_ZERO)),
                                   AdjustUV16(_mm256_unpackhi_epi8(u, K_ZERO)));
  return _mm256_packus_epi16(lo, hi);
}

template <int index>
__m256i InterleaveBgr(__m256i blue, __m256i green, __m256i red);

template <>
SIMD_INLINE __m256i InterleaveBgr<0>(__m256i blue, __m256i green, __m256i red) {
  return _mm256_or_si256(
      _mm256_shuffle_epi8(_mm256_permute4x64_epi64(blue, 0x44),
                          K8_SHUFFLE_PERMUTED_BLUE_TO_BGR0),
      _mm256_or_si256(_mm256_shuffle_epi8(_mm256_permute4x64_epi64(green, 0x44),
                                          K8_SHUFFLE_PERMUTED_GREEN_TO_BGR0),
                      _mm256_shuffle_epi8(_mm256_permute4x64_epi64(red, 0x44),
                                          K8_SHUFFLE_PERMUTED_RED_TO_BGR0)));
}

template <>
SIMD_INLINE __m256i InterleaveBgr<1>(__m256i blue, __m256i green, __m256i red) {
  return _mm256_or_si256(
      _mm256_shuffle_epi8(_mm256_permute4x64_epi64(blue, 0x99),
                          K8_SHUFFLE_PERMUTED_BLUE_TO_BGR1),
      _mm256_or_si256(_mm256_shuffle_epi8(_mm256_permute4x64_epi64(green, 0x99),
                                          K8_SHUFFLE_PERMUTED_GREEN_TO_BGR1),
                      _mm256_shuffle_epi8(_mm256_permute4x64_epi64(red, 0x99),
                                          K8_SHUFFLE_PERMUTED_RED_TO_BGR1)));
}

template <>
SIMD_INLINE __m256i InterleaveBgr<2>(__m256i blue, __m256i green, __m256i red) {
  return _mm256_or_si256(
      _mm256_shuffle_epi8(_mm256_permute4x64_epi64(blue, 0xEE),
                          K8_SHUFFLE_PERMUTED_BLUE_TO_BGR2),
      _mm256_or_si256(_mm256_shuffle_epi8(_mm256_permute4x64_epi64(green, 0xEE),
                                          K8_SHUFFLE_PERMUTED_GREEN_TO_BGR2),
                      _mm256_shuffle_epi8(_mm256_permute4x64_epi64(red, 0xEE),
                                          K8_SHUFFLE_PERMUTED_RED_TO_BGR2)));
}

SIMD_INLINE __m256i BgrToBlue(__m256i bgr[3]) {
  __m256i b0 = _mm256_shuffle_epi8(bgr[0], K8_SHUFFLE_BGR0_TO_BLUE);
  __m256i b2 = _mm256_shuffle_epi8(bgr[2], K8_SHUFFLE_BGR2_TO_BLUE);
  return _mm256_or_si256(
      _mm256_permute2x128_si256(b0, b2, 0x20),
      _mm256_or_si256(_mm256_shuffle_epi8(bgr[1], K8_SHUFFLE_BGR1_TO_BLUE),
                      _mm256_permute2x128_si256(b0, b2, 0x31)));
}

SIMD_INLINE __m256i BgrToGreen(__m256i bgr[3]) {
  __m256i g0 = _mm256_shuffle_epi8(bgr[0], K8_SHUFFLE_BGR0_TO_GREEN);
  __m256i g2 = _mm256_shuffle_epi8(bgr[2], K8_SHUFFLE_BGR2_TO_GREEN);
  return _mm256_or_si256(
      _mm256_permute2x128_si256(g0, g2, 0x20),
      _mm256_or_si256(_mm256_shuffle_epi8(bgr[1], K8_SHUFFLE_BGR1_TO_GREEN),
                      _mm256_permute2x128_si256(g0, g2, 0x31)));
}

SIMD_INLINE __m256i BgrToRed(__m256i bgr[3]) {
  __m256i r0 = _mm256_shuffle_epi8(bgr[0], K8_SHUFFLE_BGR0_TO_RED);
  __m256i r2 = _mm256_shuffle_epi8(bgr[2], K8_SHUFFLE_BGR2_TO_RED);
  return _mm256_or_si256(
      _mm256_permute2x128_si256(r0, r2, 0x20),
      _mm256_or_si256(_mm256_shuffle_epi8(bgr[1], K8_SHUFFLE_BGR1_TO_RED),
                      _mm256_permute2x128_si256(r0, r2, 0x31)));
}

template <bool align>
SIMD_INLINE __m256i LoadPermuted(const __m256i *p) {
  return _mm256_permute4x64_epi64(Load<align>(p), 0xD8);
}

}  // namespace camera
}  // namespace drivers
}  // namespace apollo