/*

 * Copyright (c) 2018, Alliance for Open Media. All rights reserved.

 *

 * This source code is subject to the terms of the BSD 2 Clause License and

 * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License

 * was not distributed with this source code in the LICENSE file, you can

 * obtain it at www.aomedia.org/license/software. If the Alliance for Open

 * Media Patent License 1.0 was not distributed with this source code in the

 * PATENTS file, you can obtain it at www.aomedia.org/license/patent.

 */

#include <stdbool.h>

#include <assert.h>

#include "config/av1_rtcd.h"

#include "av1/encoder/ml.h"

#include "av1/encoder/x86/ml_sse3.h"

// In order to avoid the high-latency of swapping between FPU and SIMD

// operations, we keep the result in a 128-bit register even though we only

// care about a single value.

static void nn_propagate_8to1(const float *const inputs,

                              const float *const weights,

                              __m128 *const output) {

  const __m128 inputs_h = _mm_loadu_ps(&inputs[4]);

  const __m128 inputs_l = _mm_loadu_ps(inputs);

  const __m128 weights_h = _mm_loadu_ps(&weights[4]);

  const __m128 weights_l = _mm_loadu_ps(weights);

  const __m128 mul_h = _mm_mul_ps(inputs_h, weights_h);

  const __m128 mul_l = _mm_mul_ps(inputs_l, weights_l);

  // [7 6 5 4] [3 2 1 0] (weight and input indices)

  const __m128 vadd = _mm_add_ps(mul_l, mul_h);

  // [7+3 6+2 5+1 4+0]

  const __m128 hadd1 = _mm_hadd_ps(vadd, vadd);

  // [7+6+3+2 5+4+1+0 7+6+3+2 5+4+1+0]

  const __m128 hadd2 = _mm_hadd_ps(hadd1, hadd1);

  // [7+6+5+4+3+2+1+0 7+6+5+4+3+2+1+0 7+6+5+4+3+2+1+0 7+6+5+4+3+2+1+0]

  *output = _mm_add_ps(*output, hadd2);

}

void av1_nn_propagate_4to1_sse3(const float *const inputs,

                                const float *const weights,

                                __m128 *const output) {

  const __m128 inputs128 = _mm_loadu_ps(inputs);

  const __m128 weights128 = _mm_loadu_ps(weights);

  const __m128 mul = _mm_mul_ps(inputs128, weights128);

  // [3 2 1 0] (weight and input indices)

  const __m128 hadd1 = _mm_hadd_ps(mul, mul);

  // [3+2 1+0 3+2 1+0]

  const __m128 hadd2 = _mm_hadd_ps(hadd1, hadd1);

  // [3+2+1+0 3+2+1+0 3+2+1+0 3+2+1+0]

  *output = _mm_add_ps(*output, hadd2);

}

void av1_nn_propagate_4to4_sse3(const float *const inputs,

                                const float *const weights,

                                __m128 *const outputs, const int num_inputs) {

  const __m128 inputs128 = _mm_loadu_ps(inputs);

  __m128 hadd[2];

  for (int i = 0; i < 2; i++) {  // For each pair of outputs

    const __m128 weight0 = _mm_loadu_ps(&weights[2 * i * num_inputs]);

    const __m128 mul0 = _mm_mul_ps(weight0, inputs128);

    const __m128 weight1 = _mm_loadu_ps(&weights[(2 * i + 1) * num_inputs]);

    const __m128 mul1 = _mm_mul_ps(weight1, inputs128);

    hadd[i] = _mm_hadd_ps(mul0, mul1);

  }

  // hadd[0] = [7+6 5+4 3+2 1+0] (weight indices)

  // hadd[1] = [15+14 13+12 11+10 9+8]

  const __m128 hh = _mm_hadd_ps(hadd[0], hadd[1]);

  // [15+14+13+12 11+10+9+8 7+6+5+4 3+2+1+0]

  *outputs = _mm_add_ps(*outputs, hh);

}

void av1_nn_propagate_4to8_sse3(const float *const inputs,

                                const float *const weights, __m128 *const out_h,

                                __m128 *const out_l, const int num_inputs) {

  const __m128 inputs128 = _mm_loadu_ps(inputs);

  __m128 hadd[4];

  for (int i = 0; i < 4; i++) {  // For each pair of outputs

    const __m128 weight0 = _mm_loadu_ps(&weights[2 * i * num_inputs]);

    const __m128 weight1 = _mm_loadu_ps(&weights[(2 * i + 1) * num_inputs]);

    const __m128 mul0 = _mm_mul_ps(inputs128, weight0);

    const __m128 mul1 = _mm_mul_ps(inputs128, weight1);

    hadd[i] = _mm_hadd_ps(mul0, mul1);

  }

  // hadd[0] = [7+6 5+4 3+2 1+0] (weight indices)

  // hadd[1] = [15+14 13+12 11+10 9+8]

  // hadd[2] = [23+22 21+20 19+18 17+16]

  // hadd[3] = [31+30 29+28 27+26 25+24]

  const __m128 hh0 = _mm_hadd_ps(hadd[0], hadd[1]);

  // [15+14+13+12 11+10+9+8 7+6+5+4 3+2+1+0]

  const __m128 hh1 = _mm_hadd_ps(hadd[2], hadd[3]);

  // [31+30+29+28 27+26+25+24 23+22+21+20 19+18+17+16]

  *out_h = _mm_add_ps(*out_h, hh1);

  *out_l = _mm_add_ps(*out_l, hh0);

}

static void nn_propagate_8to4(const float *const inputs,

                              const float *const weights, __m128 *const outputs,

                              const int num_inputs) {

  const __m128 inputs_h = _mm_loadu_ps(inputs + 4);

  const __m128 inputs_l = _mm_loadu_ps(inputs);

  // [7 6 5 4] [3 2 1 0] (input indices)

  __m128 add[4];

  for (int i = 0; i < 4; i++) {  // For each output:

    const __m128 weight_h = _mm_loadu_ps(&weights[i * num_inputs + 4]);

    const __m128 weight_l = _mm_loadu_ps(&weights[i * num_inputs]);

    const __m128 mul_h = _mm_mul_ps(inputs_h, weight_h);

    const __m128 mul_l = _mm_mul_ps(inputs_l, weight_l);

    add[i] = _mm_add_ps(mul_l, mul_h);

  }

  // add[0] = [7+3 6+2 5+1 4+0]

  // add[1] = [15+11 14+10 13+9 12+8]

  // add[2] = [23+19 22+18 21+17 20+16]

  // add[3] = [31+27 30+26 29+25 28+24]

  const __m128 hadd_h = _mm_hadd_ps(add[2], add[3]);

  // [31+30+27+26 29+28+25+24 23+22+19+18 21+20+17+16]

  const __m128 hadd_l = _mm_hadd_ps(add[0], add[1]);

  // [15+14+11+10 13+12+9+8 7+6+3+2 5+4+1+0]

  const __m128 haddhadd = _mm_hadd_ps(hadd_l, hadd_h);

  // [31+30+29+28+27+26+25+24 23+22+21+20+19+18+17+16

  //  15+14+13+12+11+10+9+8 7+6+5+4+3+2+1+0]

  *outputs = _mm_add_ps(*outputs, haddhadd);

}

static void nn_activate8(__m128 *out_h, __m128 *out_l) {

  const __m128 zero = _mm_setzero_ps();

  *out_h = _mm_max_ps(*out_h, zero);

  *out_l = _mm_max_ps(*out_l, zero);

}

static void nn_activate4(__m128 *x) { *x = _mm_max_ps(*x, _mm_setzero_ps()); }

// Calculate prediction based on the given input features and neural net config.

// Assume there are no more than NN_MAX_NODES_PER_LAYER nodes in each hidden

// layer.

void av1_nn_predict_sse3(const float *input_nodes,

                         const NN_CONFIG *const nn_config, int reduce_prec,

                         float *const output) {

  float buf[2][NN_MAX_NODES_PER_LAYER];

  int buf_index = 0;

  int num_inputs = nn_config->num_inputs;

  // Hidden layers, except the final iteration is the output layer.

  for (int layer = 0; layer <= nn_config->num_hidden_layers; layer++) {

    const float *layer_weights = nn_config->weights[layer];

    const float *layer_bias = nn_config->bias[layer];

    bool output_layer = (layer == nn_config->num_hidden_layers);

    float *const output_nodes = output_layer ? output : &buf[buf_index][0];

    const int num_outputs = output_layer ? nn_config->num_outputs

                                         : nn_config->num_hidden_nodes[layer];

    if (num_inputs % 4 == 0 && num_outputs % 8 == 0) {

      for (int out = 0; out < num_outputs; out += 8) {

        __m128 out_h = _mm_loadu_ps(&layer_bias[out + 4]);

        __m128 out_l = _mm_loadu_ps(&layer_bias[out]);

        for (int in = 0; in < num_inputs; in += 4) {

          av1_nn_propagate_4to8_sse3(&input_nodes[in],

                                     &layer_weights[out * num_inputs + in],

                                     &out_h, &out_l, num_inputs);

        }

        if (!output_layer) nn_activate8(&out_h, &out_l);

        _mm_storeu_ps(&output_nodes[out + 4], out_h);

        _mm_storeu_ps(&output_nodes[out], out_l);

      }

    } else if (num_inputs % 8 == 0 && num_outputs % 4 == 0) {

      for (int out = 0; out < num_outputs; out += 4) {

        __m128 outputs = _mm_loadu_ps(&layer_bias[out]);

        for (int in = 0; in < num_inputs; in += 8) {

          nn_propagate_8to4(&input_nodes[in],

                            &layer_weights[out * num_inputs + in], &outputs,

                            num_inputs);

        }

        if (!output_layer) nn_activate4(&outputs);

        _mm_storeu_ps(&output_nodes[out], outputs);

      }

    } else if (num_inputs % 4 == 0 && num_outputs % 4 == 0) {

      for (int out = 0; out < num_outputs; out += 4) {

        __m128 outputs = _mm_loadu_ps(&layer_bias[out]);

        for (int in = 0; in < num_inputs; in += 4) {

          av1_nn_propagate_4to4_sse3(&input_nodes[in],

                                     &layer_weights[out * num_inputs + in],

                                     &outputs, num_inputs);

        }

        if (!output_layer) nn_activate4(&outputs);

        _mm_storeu_ps(&output_nodes[out], outputs);

      }

    } else if (num_inputs % 8 == 0) {

      for (int out = 0; out < num_outputs; out++) {

        __m128 total = _mm_load1_ps(&layer_bias[out]);

        for (int in = 0; in < num_inputs; in += 8) {

          nn_propagate_8to1(&input_nodes[in],

                            &layer_weights[out * num_inputs + in], &total);

        }

        if (!output_layer) nn_activate4(&total);

        output_nodes[out] = _mm_cvtss_f32(total);

      }

    } else if (num_inputs % 4 == 0) {

      for (int out = 0; out < num_outputs; out++) {

        __m128 total = _mm_load1_ps(&layer_bias[out]);

        for (int in = 0; in < num_inputs; in += 4) {

          av1_nn_propagate_4to1_sse3(

              &input_nodes[in], &layer_weights[out * num_inputs + in], &total);

        }

        if (!output_layer) nn_activate4(&total);

        output_nodes[out] = _mm_cvtss_f32(total);

      }

    } else {

      // Use SSE instructions for scalar operations to avoid the latency of

      // swapping between SIMD and FPU modes.

      for (int out = 0; out < num_outputs; out++) {

        __m128 total = _mm_load1_ps(&layer_bias[out]);

        for (int in_node = 0; in_node < num_inputs; in_node++) {

          __m128 input = _mm_load1_ps(&input_nodes[in_node]);

          __m128 weight =

              _mm_load1_ps(&layer_weights[num_inputs * out + in_node]);

          total = _mm_add_ps(total, _mm_mul_ps(input, weight));

        }

        if (!output_layer) nn_activate4(&total);

        output_nodes[out] = _mm_cvtss_f32(total);

      }

    }

    input_nodes = output_nodes;

    num_inputs = num_outputs;

    buf_index = 1 - buf_index;

  }

  if (reduce_prec) av1_nn_output_prec_reduce(output, nn_config->num_outputs);

}

// Based on N. N. Schraudolph. A Fast, Compact Approximation of the Exponential

// Function. Neural Computation, 11(4):853–862, 1999.

static inline __m128 approx_exp(__m128 y) {

#define A ((1 << 23) / 0.69314718056f)  // (1 << 23) / ln(2)

#define B \

  127  // Offset for the exponent according to IEEE floating point standard.

#define C 60801  // Magic number controls the accuracy of approximation

  const __m128 multiplier = _mm_set1_ps(A);

  const __m128i offset = _mm_set1_epi32(B * (1 << 23) - C);

  y = _mm_mul_ps(y, multiplier);

  y = _mm_castsi128_ps(_mm_add_epi32(_mm_cvtps_epi32(y), offset));

  return y;

#undef A

#undef B

#undef C

}

static inline __m128 reduce_max(__m128 reg) {

  __m128 tmp_reg;

  tmp_reg = _mm_shuffle_ps(reg, reg, 0x4e);  // 01 00 11 10

  reg = _mm_max_ps(reg, tmp_reg);

  tmp_reg = _mm_shuffle_ps(reg, reg, 0xb1);  // 10 11 00 01

  reg = _mm_max_ps(reg, tmp_reg);

  return reg;

}

static inline __m128 reduce_sum(__m128 reg) {

  __m128 tmp_reg;

  tmp_reg = _mm_shuffle_ps(reg, reg, 0x4e);  // 01 00 11 10

  reg = _mm_add_ps(reg, tmp_reg);

  tmp_reg = _mm_shuffle_ps(reg, reg, 0xb1);  // 10 11 00 01

  reg = _mm_add_ps(reg, tmp_reg);

  return reg;

}

void av1_nn_fast_softmax_16_sse3(const float *input, float *output) {

  // Clips at -10 to avoid underflowing

  const __m128 clipper = _mm_set1_ps(-10.0f);

  // Load in 16 values

  __m128 in_0 = _mm_loadu_ps(&input[0]);

  __m128 in_1 = _mm_loadu_ps(&input[4]);

  __m128 in_2 = _mm_loadu_ps(&input[8]);

  __m128 in_3 = _mm_loadu_ps(&input[12]);

  // Get the max

  __m128 max_0 = _mm_max_ps(in_0, in_1);

  __m128 max_1 = _mm_max_ps(in_2, in_3);

  max_0 = _mm_max_ps(max_0, max_1);

  max_0 = reduce_max(max_0);

  // Subtract the max off and clip

  in_0 = _mm_sub_ps(in_0, max_0);

  in_1 = _mm_sub_ps(in_1, max_0);

  in_2 = _mm_sub_ps(in_2, max_0);

  in_3 = _mm_sub_ps(in_3, max_0);

  in_0 = _mm_max_ps(in_0, clipper);

  in_1 = _mm_max_ps(in_1, clipper);

  in_2 = _mm_max_ps(in_2, clipper);

  in_3 = _mm_max_ps(in_3, clipper);

  // Exponentiate and compute the denominator

  __m128 sum = in_0 = approx_exp(in_0);

  in_1 = approx_exp(in_1);

  sum = _mm_add_ps(sum, in_1);

  in_2 = approx_exp(in_2);

  sum = _mm_add_ps(sum, in_2);

  in_3 = approx_exp(in_3);

  sum = _mm_add_ps(sum, in_3);

  sum = reduce_sum(sum);

  // Divide to get the probability

  in_0 = _mm_div_ps(in_0, sum);

  in_1 = _mm_div_ps(in_1, sum);

  in_2 = _mm_div_ps(in_2, sum);

  in_3 = _mm_div_ps(in_3, sum);

  _mm_storeu_ps(&output[0], in_0);

  _mm_storeu_ps(&output[4], in_1);

  _mm_storeu_ps(&output[8], in_2);

  _mm_storeu_ps(&output[12], in_3);

}