#include <cmath>
#include <iostream>

#include <boost/python/list.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/extract.hpp>

#include <scitbx/matrix/eigensystem.h>
#include <mmtbx/tls/utils.h>
#include <mmtbx/tls/optimise_amplitudes.h>

namespace mmtbx { namespace tls { namespace optimise {

template <typename T>
T find_max(const af::shared<T> &array)
{
  T max_val = array[0];
  for (size_t i=1; i<array.size(); i++)
  {
    if (max_val < array[i]) { max_val = array[i]; }
  }
  return max_val;
};

bool is_zero(const sym s, double tol=1e-12)
{
  if (
      (std::abs(s[0])<tol) &&
      (std::abs(s[1])<tol) &&
      (std::abs(s[2])<tol) &&
      (std::abs(s[3])<tol) &&
      (std::abs(s[4])<tol) &&
      (std::abs(s[5])<tol)
      )
  {
    return true;
  }
  return false;
}

//! Main constructor
MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator(
    const symArrNd &target_uijs,
    const dblArrNd &target_weights,
    const dblArr1d &base_amplitudes,
    const bp::list &base_uijs,
    const bp::list &base_atom_indices,
    const selArr1d &dataset_hash,
    const symArr1d &residual_uijs ) :
  target_uijs(target_uijs),
  target_weights(target_weights),
  dataset_hash(dataset_hash),
  residual_uijs(residual_uijs),
  n_dst(target_uijs.accessor().all()[0]),
  n_atm(target_uijs.accessor().all()[1]),
  n_base(base_amplitudes.size()),
  n_total(base_amplitudes.size() + residual_uijs.size()),
  residual_mask_total(0),
  n_call(0)
{
  std::ostringstream errMsg;
  // Check target uijs
  if (target_uijs.accessor().nd() != 2) {
    errMsg << "invalid target_uijs: must be 2-dimensional flex array (currently " << target_uijs.accessor().nd() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  // Check weights
  if (target_uijs.accessor().nd() != target_weights.accessor().nd())
  {
    errMsg << "invalid dimension of target_weights (dimension " << target_weights.accessor().nd() << "): must be same dimension as target_uijs (dimension " << target_uijs.accessor().nd() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  for (size_t i=0; i<target_uijs.accessor().nd(); i++)
  {
    if (target_uijs.accessor().all()[i] != target_weights.accessor().all()[i])
    {
      errMsg << "incompatible dimension of target_weights (axis " << i << "): must be same size as target_uijs (" << target_weights.accessor().all()[i] << " != " << target_uijs.accessor().all()[i] <<")";
      throw std::invalid_argument( errMsg.str() );
    }
  }
  // Check base_amplitudes, base_uijs and base_atom_indices (common error message)
  errMsg << "invalid input base components. "
    << "base_amplitudes (length " << base_amplitudes.size()
    << "), base_uijs (length " << bp::len(base_uijs)
    << ") and base_atom_indices (length " << bp::len(base_atom_indices)
    << ") must all be the same length";
  if (base_amplitudes.size() != n_base) { throw std::invalid_argument( errMsg.str() ); }
  if (bp::len(base_uijs) != n_base) { throw std::invalid_argument( errMsg.str() ); }
  if (bp::len(base_atom_indices) != n_base) { throw std::invalid_argument( errMsg.str() ); }
  errMsg.str(""); // clear the previous error message
  // Unpack base_uijs and base_atom_indicess
  base_u.reserve(bp::len(base_uijs));
  base_i.reserve(bp::len(base_atom_indices));
  for (std::size_t i = 0; i < n_base; ++i)
  {
    symArr1d* atm_u = new symArr1d(bp::extract<symArr1d>(base_uijs[i]));
    selArr1d* atm_i = new selArr1d(bp::extract<selArr1d>(base_atom_indices[i]));

    if (atm_u->size() != atm_i->size())
    {
      errMsg << "incompatible pair (element " << i << ") in base_uijs/base_atom_indices: pairwise elements must be the same length (" << atm_u->size() << " and " << atm_i->size() << ")";
      throw std::invalid_argument( errMsg.str() );
    }
    if (find_max(*atm_i) >= n_atm)
    {
      errMsg << "invalid selection in base_atom_indices (" << find_max(*atm_i) << "): attempting to select atom outside of array (size " << n_atm << ")";
      throw std::invalid_argument( errMsg.str() );
    }

    base_u.push_back(atm_u);
    base_i.push_back(atm_i);
  }
  // Check dataset hash
  if (dataset_hash.size() != n_base) {
    errMsg << "invalid dataset_hash (length " << dataset_hash.size() << "): must be same length as base_amplitudes, base_uijs & base_atom_indices (length " << base_amplitudes.size() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  if (find_max(dataset_hash) >= n_dst)
  {
    errMsg << "invalid value in dataset_hash (" << find_max(dataset_hash) << "): attempts to select element outside range of target_uijs (size " << n_dst << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  for (size_t i_dst=0; i_dst < n_dst; i_dst++)
  {
    bool found = false;
    for (size_t i_base=0; i_base < n_base; i_base++)
    {
      if (dataset_hash[i_base] == i_dst)
      {
        found = true;
        break;
      }
    }
    if (found == false)
    {
      errMsg << "Dataset index " << i_dst << " is not present in dataset_hash -- this dataset has no base elements associated with it.";
      throw std::invalid_argument( errMsg.str() );
    }
  }
  // Check residual uijs
  if (residual_uijs.size() != n_atm)
  {
    errMsg << "invalid size of residual_uijs (" << residual_uijs.size() << "): must match 2nd dimension of target_uijs (" << n_atm << ")";
    throw std::invalid_argument( errMsg.str() );
  }

  // ==========================
  // Assign class members
  // ==========================

  // Initialise residual amplitude array
  dblArr1d res_amplitudes(n_atm, 1.0);

  // Concatenate amplitude arrays
  initial_amplitudes.reserve(n_total);
  std::copy(base_amplitudes.begin(), base_amplitudes.end(), std::back_inserter(initial_amplitudes));
  std::copy(res_amplitudes.begin(), res_amplitudes.end(), std::back_inserter(initial_amplitudes));
  // Copy to current values
  current_amplitudes = dblArr1d(initial_amplitudes);

  // Total Uijs (summed over levels) (datasets * atoms)
  total_uijs = symArrNd(af::flex_grid<>(n_dst, n_atm), sym(0.,0.,0.,0.,0.,0.));

  // Initialise blank residual mask
  setResidualMask(blnArr1d(n_dst, true));
}

dblArr1d MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::getCurrentAmplitudes()
{
  return current_amplitudes;
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::setCurrentAmplitudes(const dblArr1d &values)
{
  if (values.size() != current_amplitudes.size()) {
    std::ostringstream errMsg;
    errMsg << "Input array (size " << values.size() << ") must be the same length as current_amplitudes (size " << current_amplitudes.size() << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  for (size_t i=0; i<current_amplitudes.size(); i++)
  {
    current_amplitudes[i] = values[i];
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::printCurrentAmplitudes()
{
  for (size_t i=0; i<current_amplitudes.size(); i++)
  {
    std::cout << i << " - " << current_amplitudes[i] << std::endl;
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::setResidualMask(const blnArr1d &mask)
{
  if (mask.size() != n_dst) {
    std::ostringstream errMsg;
    errMsg << "Input array (size " << mask.size() << ") must be the same length as number of datasets (" << n_dst << ")";
    throw std::invalid_argument( errMsg.str() );
  }
  residual_mask = blnArr1d(mask);
  residual_mask_total = 0;
  for (size_t i=0; i<residual_mask.size(); i++)
  {
    residual_mask_total += (int)residual_mask[i];
  }
}

bp::tuple MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::computeFunctionalAndGradients()
{
  // Reset everything
  zero();

  n_call++;

  // Reset negative amplitudes
  //
  sanitise_current_amplitudes();

  // Apply amplitudes to base uijs
  //
  calculate_total_uijs();

  // Calculate least-squares component of target function
  //
  calculate_f_g_least_squares();

  // Return as python tuple for optimiser
  //
  return bp::make_tuple(functional, gradients);
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::zero()
{
  // Reset functional and gradients
  functional = 0.0;
  gradients = dblArr1d(n_total, 0.0);

  // Zero-out the level uijs
  memset(&total_uijs[0], 0.0, sizeof(sym) * total_uijs.size());
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::sanitise_current_amplitudes()
{
  // Set negative amplitudes to zero
  for (size_t i_opt=0; i_opt<current_amplitudes.size(); i_opt++)
  {
    if (current_amplitudes[i_opt] < 0.0)
    {
      // Constrain the value to be zero (and thereby negate any benefit to the functional that could have been gained)
      current_amplitudes[i_opt] = 0.0;
    }
  }
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculate_total_uijs()
{
  // ==========================================================
  // Sum over amplitudes to generate totals - !!! BASE TERMS !!!
  //
  for (size_t i_base=0; i_base<n_base; i_base++)
  {
    symArr1d &base_u_atom = *(base_u[i_base]);
    selArr1d &base_i_atom = *(base_i[i_base]);
    size_t i_dst = dataset_hash[i_base];
    size_t i_opt = i_base;

    // Iterate through atoms associated with this base element
    for (size_t i_atm_x=0; i_atm_x<base_u_atom.size(); i_atm_x++)
    {
      // apply multipliers
      sym m = current_amplitudes[i_opt] * base_u_atom[i_atm_x];
      // Skip if null
      if (is_zero(m))
      {
        continue;
      }
      // Add to total uijs
      total_uijs(i_dst, base_i_atom[i_atm_x]) += m;
    }
  }
  //
  // Sum over amplitudes to generate totals - !!! RESIDUAL TERMS !!!
  //
  for (size_t i_atm=0; i_atm<n_atm; i_atm++)
  {
    size_t i_opt = n_base + i_atm;
    sym m = current_amplitudes[i_opt] * residual_uijs[i_atm];
    for (size_t i_dst=0; i_dst<n_dst; i_dst++)
    {
      total_uijs(i_dst, i_atm) += m;
    }
  }
  // ==========================================================
}

void MultiGroupMultiDatasetUijAmplitudeFunctionalAndGradientCalculator::calculate_f_g_least_squares()
{
  // Normalisation term (number of datasets)
  double norm_all = 1. / (double)(n_dst * n_atm);

  // Normalisation term for residual level
  double norm_res = 0.0; // if unchanged, residual level will not be optimised
  if (residual_mask_total > 0)
  {
    // Upweights by term n_all/n_calc to simulate being calculated over all datasets
    norm_res = norm_all * (double)(n_dst) / (double)(residual_mask_total);
  }

  // ==========================================================
  // Calculate functional and gradients
  for (size_t i_dst=0; i_dst<n_dst; i_dst++)
  {
    // Extract weights for this dataset
    dblArr1d d_wgts(&target_weights(i_dst,0), &target_weights(i_dst, n_atm));
    // Extract total uijs for this dataset
    symArr1d d_total(&total_uijs(i_dst,0), &total_uijs(i_dst, n_atm));

    // Calculate difference to target
    symArr1d d_diffs(n_atm); // all elements will be populated
    for (size_t i_atm=0; i_atm<n_atm; i_atm++)
    {
      sym d_diff_i = target_uijs(i_dst,i_atm) - total_uijs(i_dst,i_atm);
      d_diffs[i_atm] = d_diff_i;

      // Calculate functional -- least-squares component
      for (size_t i_elem=0; i_elem<6; i_elem++)
      {
        functional += (
            d_diff_i[i_elem] *
            d_diff_i[i_elem] *
            d_wgts[i_atm] *
            norm_all
            );
      }
    }

    // Calculate gradients -- BASE TERMS
    for (size_t i_base=0; i_base<n_base; i_base++)
    {
      // Only calculte if this base term is related to this dataset
      if (i_dst != dataset_hash[i_base])
      {
        continue;
      }

      // i_ relative to full length of amplitudes
      size_t i_opt = i_base;

      // Extract the base elements
      symArr1d &base_u_atom = *(base_u[i_base]);
      selArr1d &base_i_atom = *(base_i[i_base]);

      // Iterate through the atoms in the base element
      for (size_t i_atm_x=0; i_atm_x<base_i_atom.size(); i_atm_x++)
      {
        // i_atm relative to n_atm
        size_t i_atm = base_i_atom[i_atm_x];

        // Extract the uij for this base element
        sym base_sym = base_u_atom[i_atm_x];

        // Skip this atom if the base_uij is zero at this position
        if (is_zero(base_sym))
        {
          continue;
        }

        // Corresponding u_diff
        sym diff_sym = d_diffs[i_atm];

        // Gradient from least-squares
        // -2*base*diffs*wgts
        for (size_t i_elem=0; i_elem<6; i_elem++)
        {
          gradients[i_opt] += (
              -2.0 *
              base_sym[i_elem] *
              diff_sym[i_elem] *
              d_wgts[i_atm] *
              norm_all
              );
        }
      }
    }

    // Calculate gradients -- RESIDUAL TERMS
    if (residual_mask[i_dst])
    {
      for (size_t i_atm=0; i_atm<n_atm; i_atm++)
      {
        size_t i_opt = n_base + i_atm;

        // Extract the uij for this base element
        sym base_sym = residual_uijs[i_atm];

        // Skip this atom if the base_uij is zero at this position
        if (is_zero(base_sym))
        {
          continue;
        }

        // Corresponding u_diff
        sym diff_sym = d_diffs[i_atm];

        // Gradient from least-squares
        // -2*base*diffs*wgts
        for (size_t i_elem=0; i_elem<6; i_elem++)
        {
          gradients[i_opt] += (
              -2.0 *
              base_sym[i_elem] *
              diff_sym[i_elem] *
              d_wgts[i_atm] *
              norm_res
              );
        }
      }
    }
  }
}

} } } // close namepsace mmtbx/tls/optimise


//============================================================================
// Name        : test_rng.cpp
// Author      : Mikail Sheikh
// Date        : 28/11/2013
// Copyright   : Distributed under the MIT License (MIT)
// Description : A QuantLib wrapper for Boost random number generators
//============================================================================

#include "boost_rng_bindings.hpp"

#include <ql/time/calendar.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>
#include <ql/option.hpp>
#include <ql/exercise.hpp>
#include <ql/quote.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/instruments/payoffs.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <ql/pricingengines/vanilla/mceuropeanengine.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/settings.hpp>
#include <ql/handle.hpp>
#include <ql/utilities/dataformatters.hpp>

#include <boost/timer/timer.hpp>
#include <boost/shared_ptr.hpp>

#include <iostream>
#include <iomanip>
#include <ctime>

int main()
{

  try
  {
    // set up dates
    Calendar calendar = TARGET();
    Date todaysDate(15, May, 1998);
    Date settlementDate(17, May, 1998);
    Settings::instance().evaluationDate() = todaysDate;

    // our options
    Option::Type type(Option::Put);
    Real underlying = 36;
    Real strike = 40;
    Spread dividendYield = 0.00;
    Rate riskFreeRate = 0.06;
    Volatility volatility = 0.20;
    Date maturity(17, May, 1999);
    DayCounter dayCounter = Actual365Fixed();

    std::cout << "Option type = " << type << std::endl;
    std::cout << "Maturity = " << maturity << std::endl;
    std::cout << "Underlying price = " << underlying << std::endl;
    std::cout << "Strike = " << strike << std::endl;
    std::cout << "Risk-free interest rate = " << io::rate(riskFreeRate)
        << std::endl;
    std::cout << "Dividend yield = " << io::rate(dividendYield) << std::endl;
    std::cout << "Volatility = " << io::volatility(volatility) << std::endl;
    std::cout << std::endl;
    std::string method;
    std::cout << std::endl;

    // write column headings
    Size widths[] =
      { 35, 14 };
    std::cout << std::setw(widths[0]) << std::left << "Method"
        << std::setw(widths[1]) << std::left << "European" << std::endl;

    boost::shared_ptr<Exercise> europeanExercise(
        new EuropeanExercise(maturity));

    Handle<Quote> underlyingH(
        boost::shared_ptr<Quote>(new SimpleQuote(underlying)));

    // bootstrap the yield/dividend/vol curves
    Handle<YieldTermStructure> flatTermStructure(
        boost::shared_ptr<YieldTermStructure>(
            new FlatForward(settlementDate, riskFreeRate, dayCounter)));
    Handle<YieldTermStructure> flatDividendTS(
        boost::shared_ptr<YieldTermStructure>(
            new FlatForward(settlementDate, dividendYield, dayCounter)));
    Handle<BlackVolTermStructure> flatVolTS(
        boost::shared_ptr<BlackVolTermStructure>(
            new BlackConstantVol(settlementDate, calendar, volatility,
                dayCounter)));
    boost::shared_ptr<StrikedTypePayoff> payoff(
        new PlainVanillaPayoff(type, strike));
    boost::shared_ptr<BlackScholesMertonProcess> bsmProcess(
        new BlackScholesMertonProcess(underlyingH, flatDividendTS,
            flatTermStructure, flatVolTS));

    // options
    VanillaOption europeanOption(payoff, europeanExercise);

    Real npv;
    Size timeSteps;
    Size mcSeed = 42;

    // Monte Carlo Method: MC (crude)
    timeSteps = 1;
    method = "MC (crude)";
    {
      boost::timer::auto_cpu_timer timer;
      boost::shared_ptr<PricingEngine> mcengine1;
      mcengine1 = MakeMCEuropeanEngine<PseudoRandom>(bsmProcess).withSteps(
          timeSteps).withAbsoluteTolerance(0.002).withSeed(mcSeed);
      europeanOption.setPricingEngine(mcengine1);
      npv = europeanOption.NPV();
    }
    // Real errorEstimate = europeanOption.errorEstimate();
    std::cout << std::setw(widths[0]) << std::left << method << std::fixed
        << std::setw(widths[1]) << std::left << npv << std::endl;

    timeSteps = 1;
    method = "MC (crude MT11213b)";
    {
      boost::timer::auto_cpu_timer timer;
      boost::shared_ptr<PricingEngine> mcengine2;
      mcengine2 =
          MakeMCEuropeanEngine<PseudoRandomBoostMT11213b>(bsmProcess).withSteps(
              timeSteps).withAbsoluteTolerance(0.002).withSeed(mcSeed);
      europeanOption.setPricingEngine(mcengine2);
      npv = europeanOption.NPV();
    }
    // Real errorEstimate = europeanOption.errorEstimate();
    std::cout << std::setw(widths[0]) << std::left << method << std::fixed
        << std::setw(widths[1]) << std::left << npv << std::endl;

    timeSteps = 1;
    method = "MC (crude MT19937)";
    {
      boost::timer::auto_cpu_timer timer;
      boost::shared_ptr<PricingEngine> mcengine3;
      mcengine3 =
          MakeMCEuropeanEngine<PseudoRandomBoostMT19937>(bsmProcess).withSteps(
              timeSteps).withAbsoluteTolerance(0.002).withSeed(mcSeed);
      europeanOption.setPricingEngine(mcengine3);
      npv = europeanOption.NPV();
    }

    std::cout << std::setw(widths[0]) << std::left << method << std::fixed
        << std::setw(widths[1]) << std::left << npv << std::endl;
  }
  catch (std::exception& e)
  {
    std::cerr << e.what() << std::endl;
    return 1;
  }
  catch (...)
  {
    std::cerr << "unknown error" << std::endl;
    return 1;
  }
}


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {
//math_function 定义了caffe 中用到的一些矩阵操作和数值计算的一些函数

/*
 *功能： C=alpha*A*B+beta*C
 *A,B,C 是输入矩阵（一维数组格式）
 *CblasRowMajor :数据是行主序的（二维数据也是用一维数组储存的）
 *TransA, TransB：是否要对A和B做转置操作（CblasTrans CblasNoTrans）
 *M： A、C 的行数
 *N： B、C 的列数
 *K： A 的列数， B 的行数
 *lda ： A的列数（不做转置）行数（做转置）
 *ldb： B的列数（不做转置）行数（做转置）
*/
template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

/*
功能： y=alpha*A*x+beta*y
其中X和Y是向量，A 是矩阵
M：A 的行数
N：A 的列数
cblas_sgemv 中的 参数1 表示对X和Y的每个元素都进行操作
*/
template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

/*
功能： Y=alpha*X+Y
N：为X和Y中element的个数
*/
template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

/*
功能：用常数 alpha 对 Y 进行初始化
函数 void *memset(void *buffer, char c, unsigned count) 一般为新申请的内存做初始化，
功能是将buffer所指向内存中的每个字节的内容全部设置为c指定的ASCII值, count为块的大小,
使用memset函数来初始化数组或者结构体比其他初始化方法更快一点
*/
template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

//功能： 给 Y 的每个 element 加上常数 alpha
template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

/*
函数 void *memcpy(void *dest, void *src, unsigned int count) 把src所指向
的内存区域 copy到dest所指向的内存区域, count为块的大小
表头文件: #include <string.h>
定义函数: void *memcpy(void *dest, const void *src, size_t n)
函数说明: memcpy()用来拷贝src所指的内存内容前n个字节到dest所指的内存地址上。与strcpy()不同的是,memcpy()会完整的复制n个字节,不会因为遇到字符串结束'\0'而结束
返回值:   返回指向dest的指针
*/
template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}

template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

/*
功能：X = alpha*X
N： X中element的个数
*/
template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

// axpby Y=alpha * X +beta*Y
template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

/*
功能：这四个函数分别实现element-wise的加减乘除（y[i] = a[i] + - * \ b[i]）
*/
template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

/*
功能：返回一个随机数
*/
unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

/*
功能 ： 返回 b 最大方向上可以表示的最接近的数值。
*/
template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

/*
功能： 返回 vector X 和 vector Y 的内积。
incx， incy ： 步长，即每隔incx 或 incy 个element 进行操作。
*/
template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


#include <benchmark/benchmark.h>
#include <Eigen/Eigen>
#include <nabo/nabo.h>

#include "nanoflanntesttype.hpp"

static void LIBNABO_NN_BUILD(benchmark::State &state) {
    Eigen::MatrixXf dataset = Eigen::MatrixXf::Random(3, state.range(0));
    Nabo::NNSearchF* nns;

    for (auto _ : state) {
       nns = Nabo::NNSearchF::createKDTreeLinearHeap(dataset);
    }
    delete nns;
}
BENCHMARK(LIBNABO_NN_BUILD)->RangeMultiplier(10)->Range(10, 10000);

static void NANOFLANN_NN_BUILD(benchmark::State &state) {
    wave::FeatureKDTree<float> data;
    data.points = Eigen::MatrixXf::Random(3, state.range(0));

    wave::kd_tree_t<float> kdtree(3, data, nanoflann::KDTreeSingleIndexAdaptorParams(8));

    for (auto _ : state) {
        kdtree.buildIndex();
    }
}
BENCHMARK(NANOFLANN_NN_BUILD)->RangeMultiplier(10)->Range(10, 10000);

static void LIBNABO_EXACT_NN_SEARCH(benchmark::State &state) {
    Eigen::MatrixXf dataset = Eigen::MatrixXf::Random(3, 10000);
    Eigen::MatrixXf query = Eigen::MatrixXf::Random(3, state.range(0));

    Nabo::NNSearchF* nns = Nabo::NNSearchF::createKDTreeLinearHeap(dataset);

    Eigen::MatrixXi indices;
    Eigen::MatrixXf dists2;

    // Look for the range nearest neighbours of each query point,
    // We do not want approximations but we want to sort by the distance,
    indices.resize(state.range(1), query.cols());
    dists2.resize(state.range(1), query.cols());
    for (auto _ : state) {
        nns->knn(query, indices, dists2, state.range(1), 0, Nabo::NNSearchF::SORT_RESULTS);
    }
    delete nns;
}
BENCHMARK(LIBNABO_EXACT_NN_SEARCH)->RangeMultiplier(10)->Ranges({{10, 10000}, {1, 10}});

/// nanoflann seems to crap out higher than 10000 point

static void NANOFLANN_EXACT_NN_SEARCH(benchmark::State &state) {
    wave::FeatureKDTree<float> data;
    data.points = Eigen::MatrixXf::Random(3, 10000);
    Eigen::MatrixXf query = Eigen::MatrixXf::Random(3, state.range(0));

    wave::kd_tree_t<float> kdtree(3, data, nanoflann::KDTreeSingleIndexAdaptorParams(8));

    const size_t num_results = state.range(1);
    std::vector<size_t>   ret_indexes(num_results);
    std::vector<float> out_dists_sqr(num_results);

    nanoflann::KNNResultSet<float> resultSet(num_results);

    resultSet.init(&ret_indexes[0], &out_dists_sqr[0]);

    kdtree.buildIndex();
    for (auto _ : state) {
        auto ptr = query.data();
        for (long i = 0; i < state.range(0); i++) {
            kdtree.knnSearch(ptr, state.range(1), &ret_indexes[0], &out_dists_sqr[0]);
            ptr = ptr + 3;
        }
    }
}
BENCHMARK(NANOFLANN_EXACT_NN_SEARCH)->RangeMultiplier(10)->Ranges({{10, 10000}, {1, 10}});

static void LIBNABO_APPROX_NN_SEARCH(benchmark::State &state) {
    Eigen::MatrixXf dataset = Eigen::MatrixXf::Random(3, 6400);
    Eigen::MatrixXf query = Eigen::MatrixXf::Random(3, 6400);

    Nabo::NNSearchF* nns = Nabo::NNSearchF::createKDTreeLinearHeap(dataset);

    Eigen::MatrixXi indices;
    Eigen::MatrixXf dists2;

    // Look for the 10 nearest neighbours
    indices.resize(10, query.cols());
    dists2.resize(10, query.cols());

    float approx = 0.01f * static_cast<float>(state.range(0));

    for (auto _ : state) {
        nns->knn(query, indices, dists2, 10, approx, Nabo::NNSearchF::SORT_RESULTS);
    }
    delete nns;
}
BENCHMARK(LIBNABO_APPROX_NN_SEARCH)->RangeMultiplier(2)->Range(1, 16);

BENCHMARK_MAIN();


#include <vector>

#include "caffe/filler.hpp"
#include "caffe/layers/inner_product_mtl_layer.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/calculate_SVD.hpp"
//#include <Eigen/Eigen>
//using namespace Eigen;

namespace caffe {

template <typename Dtype>

void InnerProductMtlLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
  const int num_output = this->layer_param_.inner_product_mtl_param().num_output();
  bias_term_ = this->layer_param_.inner_product_mtl_param().bias_term();
  transpose_ = this->layer_param_.inner_product_mtl_param().transpose();
  N_ = num_output;
  k_value_ = this->layer_param_.inner_product_mtl_param().k_value();
  const int axis = bottom[0]->CanonicalAxisIndex(
      this->layer_param_.inner_product_mtl_param().axis());
  // Dimensions starting from "axis" are "flattened" into a single
  // length K_ vector. For example, if bottom[0]'s shape is (N, C, H, W),
  // and axis == 1, N inner products with dimension CHW are performed.
  K_ = bottom[0]->count(axis);
  // Check if we need to set up the weights
  if (this->blobs_.size() > 0) {
    LOG(INFO) << "Skipping parameter initialization";
  } else {
    if (bias_term_) {
      this->blobs_.resize(2);
    } else {
      this->blobs_.resize(1);
    }
    // Initialize the weights
    vector<int> weight_shape(2);
    if (transpose_) {
      weight_shape[0] = K_;
      weight_shape[1] = N_;
    } else {
      weight_shape[0] = N_;
      weight_shape[1] = K_;
    }
    this->blobs_[0].reset(new Blob<Dtype>(weight_shape));
    // fill the weights
    shared_ptr<Filler<Dtype> > weight_filler(GetFiller<Dtype>(
        this->layer_param_.inner_product_mtl_param().weight_filler()));
    weight_filler->Fill(this->blobs_[0].get());
    // If necessary, intiialize and fill the bias term
    if (bias_term_) {
      vector<int> bias_shape(1, N_);
      this->blobs_[1].reset(new Blob<Dtype>(bias_shape));
      shared_ptr<Filler<Dtype> > bias_filler(GetFiller<Dtype>(
          this->layer_param_.inner_product_mtl_param().bias_filler()));
      bias_filler->Fill(this->blobs_[1].get());
    }
  }  // parameter initialization
  this->param_propagate_down_.resize(this->blobs_.size(), true);
}

template <typename Dtype>
void InnerProductMtlLayer<Dtype>::Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top) {
  // Figure out the dimensions
  const int axis = bottom[0]->CanonicalAxisIndex(
      this->layer_param_.inner_product_mtl_param().axis());
  const int new_K = bottom[0]->count(axis);
  CHECK_EQ(K_, new_K)
      << "Input size incompatible with inner product parameters.";
  // The first "axis" dimensions are independent inner products; the total
  // number of these is M_, the product over these dimensions.
  M_ = bottom[0]->count(0, axis);
  // The top shape will be the bottom shape with the flattened axes dropped,
  // and replaced by a single axis with dimension num_output (N_).
  vector<int> top_shape = bottom[0]->shape();
  top_shape.resize(axis + 1);
  top_shape[axis] = N_;
  top[0]->Reshape(top_shape);
  // Set up the bias multiplier
  if (bias_term_) {
    vector<int> bias_shape(1, M_);
    bias_multiplier_.Reshape(bias_shape);
    caffe_set(M_, Dtype(1), bias_multiplier_.mutable_cpu_data());
  }
}

template <typename Dtype>
void InnerProductMtlLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype>*>& bottom,
    const vector<Blob<Dtype>*>& top) {
  const Dtype* bottom_data = bottom[0]->cpu_data();
  Dtype* top_data = top[0]->mutable_cpu_data();
  const Dtype* weight = this->blobs_[0]->cpu_data();

  caffe_cpu_gemm<Dtype>(CblasNoTrans, transpose_ ? CblasNoTrans : CblasTrans,
      M_, N_, K_, (Dtype)1.,
      bottom_data, weight, (Dtype)0., top_data);
  if (bias_term_) {
    caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasNoTrans, M_, N_, 1, (Dtype)1.,
        bias_multiplier_.cpu_data(),
        this->blobs_[1]->cpu_data(), (Dtype)1., top_data);
  }
}

template <typename Dtype>
void InnerProductMtlLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
	const vector<bool>& propagate_down,
	const vector<Blob<Dtype>*>& bottom) {
	if (this->param_propagate_down_[0]) {
		const Dtype* top_diff = top[0]->cpu_diff();
		const Dtype* bottom_data = bottom[0]->cpu_data();
		// Gradient with respect to weight
		#ifdef _DEBUG
			const Dtype* weight = this->blobs_[0]->cpu_data();
			const Dtype* weight_diff_mut = this->blobs_[0]->mutable_cpu_diff();
			const Dtype* data_diff = bottom[0]->cpu_diff();
			const Dtype* weight_diff = this->blobs_[0]->cpu_diff();
			for (int i = 0; i < 10; i++){
				cout << " " << weight[i] << " ";
			}
			cout << endl;
			cout << endl;

			for (int i = 0; i < 10; i++){
				cout << " " << weight_diff_mut[i] << " ";
			}
			cout << endl;
			cout << endl;

			for (int i = 0; i < 10; i++){
				cout << " " << weight_diff[i] << " ";
			}
			cout << endl;
			cout << endl;

			for (int i = 0; i < 10; i++){
				cout << " " << data_diff[i] << " ";
			}
			cout << endl;
			cout << endl;

		#endif

		if (transpose_) {
			caffe_cpu_gemm<Dtype>(CblasTrans, CblasNoTrans,
				K_, N_, M_,
				(Dtype)1., bottom_data, top_diff,
				(Dtype)1., this->blobs_[0]->mutable_cpu_diff());
		}
		else {
			caffe_cpu_gemm<Dtype>(CblasTrans, CblasNoTrans,
				N_, K_, M_,
				(Dtype)1., top_diff, bottom_data,
				(Dtype)1., this->blobs_[0]->mutable_cpu_diff());
		}

	}
	if (bias_term_ && this->param_propagate_down_[1]) {
		const Dtype* top_diff = top[0]->cpu_diff();
		// Gradient with respect to bias
		caffe_cpu_gemv<Dtype>(CblasTrans, M_, N_, (Dtype)1., top_diff,
			bias_multiplier_.cpu_data(), (Dtype)1.,
			this->blobs_[1]->mutable_cpu_diff());
	}

	//FM: SVD
	const Dtype* weight = this->blobs_[0]->cpu_data(); // nomizw pos to weight mporei na figei teleiws kai na douleuw mono me to weight_mut
	Dtype* weight_mut = this->blobs_[0]->mutable_cpu_data();

	int num_neurons = this->blobs_[0]->num(); // T tasks
	int dim_features = this->blobs_[0]->count() / num_neurons; // feature dimension
	CalculateSVD(weight, weight_mut, num_neurons, dim_features, k_value_);
	// FM: End of SVD

	if (propagate_down[0]) {
		const Dtype* top_diff = top[0]->cpu_diff();
		// Gradient with respect to bottom data
		if (transpose_) {
			caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasTrans,
				M_, K_, N_,
				(Dtype)1., top_diff, weight,
				(Dtype)0., bottom[0]->mutable_cpu_diff());
		}
		else {
			caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasNoTrans,
				M_, K_, N_,
				(Dtype)1., top_diff, weight,
				(Dtype)0., bottom[0]->mutable_cpu_diff());
		}
	}

	#ifdef _DEBUG
		const Dtype* weight_diff_mut = this->blobs_[0]->mutable_cpu_diff();
		const Dtype* data_diff = bottom[0]->cpu_diff();
		const Dtype* weight_diff = this->blobs_[0]->cpu_diff();
		for (int i = 0; i < 10; i++){
			cout << " " << weight[i] << " ";
		}
		cout << endl;
		cout << endl;

		for (int i = 0; i < 10; i++){
			cout << " " << weight_diff_mut[i] << " ";
		}
		cout << endl;
		cout << endl;

		for (int i = 0; i < 10; i++){
			cout << " " << weight_diff[i] << " ";
		}
		cout << endl;
		cout << endl;

		for (int i = 0; i < 10; i++){
			cout << " " << data_diff[i] << " ";
		}
		cout << endl;
		cout << endl;

	#endif

}

//template <typename Dtype>
//void InnerProductMtlLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
//    const vector<bool>& propagate_down,
//    const vector<Blob<Dtype>*>& bottom) {
//  if (this->param_propagate_down_[0]) {
//    const Dtype* top_diff = top[0]->cpu_diff();
//    const Dtype* bottom_data = bottom[0]->cpu_data();
//    // Gradient with respect to weight
//	#ifdef _DEBUG
//		const Dtype* weight = this->blobs_[0]->cpu_data();
//		const Dtype* weight_diff = this->blobs_[0]->mutable_cpu_diff();
//		for (int i = 0; i < 10; i++){
//			cout << " " << weight[i] << " ";
//		}
//		cout << endl;
//		cout << endl;
//
//		for (int i = 0; i < 10; i++){
//			cout << " " << weight_diff[i] << " ";
//		}
//		cout << endl;
//		cout << endl;
//
//	#endif
//
//    if (transpose_) {
//      caffe_cpu_gemm<Dtype>(CblasTrans, CblasNoTrans,
//          K_, N_, M_,
//          (Dtype)1., bottom_data, top_diff,
//          (Dtype)1., this->blobs_[0]->mutable_cpu_diff());
//    } else {
//      caffe_cpu_gemm<Dtype>(CblasTrans, CblasNoTrans,
//          N_, K_, M_,
//          (Dtype)1., top_diff, bottom_data,
//          (Dtype)1., this->blobs_[0]->mutable_cpu_diff());
//    }
//
//  }
//  if (bias_term_ && this->param_propagate_down_[1]) {
//    const Dtype* top_diff = top[0]->cpu_diff();
//    // Gradient with respect to bias
//    caffe_cpu_gemv<Dtype>(CblasTrans, M_, N_, (Dtype)1., top_diff,
//        bias_multiplier_.cpu_data(), (Dtype)1.,
//        this->blobs_[1]->mutable_cpu_diff());
//  }
//
//  //FM: SVD
//  const Dtype* weight = this->blobs_[0]->cpu_data();
//  Dtype* weight_mut = this->blobs_[0]->mutable_cpu_data();
//
//#ifdef _DEBUG
//  //initial weights
//  for (int i = 0; i < 10; i++){
//	  cout << " " << weight[i] << " ";
//  }
//  cout << endl;
//  cout << endl;
//#endif
//
//  int num_neurons = this->blobs_[0]->num(); // T tasks
//  int dim_features = this->blobs_[0]->count() / num_neurons; // feature dimension
//  //Dtype* weight2 = new Dtype[num_neurons*dim_features];
//
//  //Instantiate the weight matrix
//  MatrixXf m = MatrixXf::Random(num_neurons, dim_features);
//  int counter = 0;
//  for (int i = 0; i < num_neurons; ++i) {
//	  for (int j = 0; j < dim_features; ++j) {
//		  m(i, j) = weight[counter];
//		  counter++;
//	  }
//
//  }
//
//
//  //Perform SVD
//  // cout << "Here is the matrix m:" << endl << m << endl;
//  JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);
//
//  // Set with the new values
//  counter = 0;
//  MatrixXf m_inner = svd.matrixU() * (svd.singularValues().asDiagonal() * svd.matrixV().transpose());
//
//  for (int i = 0; i < num_neurons; ++i) {
//	  for (int j = 0; j < dim_features; ++j) {
//		  weight_mut[counter] = m_inner(i, j); // j,i because the matrix now has features in the columns (each column corresponds to one neuron)
//		  counter++;
//	  }
//
//  }
//
//#ifdef _DEBUG
//  MatrixXf diff = m_inner - m;
//  cout << "diff:\n" << diff.array().abs().sum() << "\n";
//  //cout << "Its singular values are:" << endl << svd.singularValues() << endl;
//  //cout << "Its left singular vectors are the columns of the thin L-U matrix:" << endl << svd.matrixU() << endl;
//  //cout << "Its right singular vectors are the columns of the thin S-V matrix:" << endl << svd.matrixV() << endl;
//
//  for (int i = 0; i < 10; i++){
//	  cout << " " << weight[i] << " ";
//  }
//  cout << endl;
//  cout << endl;
//
//  for (int i = 0; i < 10; i++){
//	  cout << " " << weight_mut[i] << " ";
//  }
//  cout << endl;
//  cout << endl;
//
//#endif
//
//  // FM: End of SVD
//
//  if (propagate_down[0]) {
//    const Dtype* top_diff = top[0]->cpu_diff();
//    // Gradient with respect to bottom data
//    if (transpose_) {
//      caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasTrans,
//          M_, K_, N_,
//		  (Dtype)1., top_diff, weight,
//          (Dtype)0., bottom[0]->mutable_cpu_diff());
//    } else {
//      caffe_cpu_gemm<Dtype>(CblasNoTrans, CblasNoTrans,
//          M_, K_, N_,
//		  (Dtype)1., top_diff, weight,
//          (Dtype)0., bottom[0]->mutable_cpu_diff());
//    }
//  }
//}

#ifdef CPU_ONLY
STUB_GPU(InnerProductMtlLayer);
#endif

INSTANTIATE_CLASS(InnerProductMtlLayer);
REGISTER_LAYER_CLASS(InnerProductMtl);

}  // namespace caffe


#include <math.h>
#include <uWS/uWS.h>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "Eigen-3.3/Eigen/Core"
#include "Eigen-3.3/Eigen/QR"
#include "MPC.h"
#include "json.hpp"

//#include "matplotlibcpp.h"

//#include <boost/thread.hpp>
// for convenience
using json = nlohmann::json;

const bool show_graph = false;

//namespace plt = matplotlibcpp;

// For converting back and forth between radians and degrees.
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }
const double Lf = 2.67;

int iters= 1;

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
// else the empty string "" will be returned.
string hasData(string s) {
  auto found_null = s.find("null");
  auto b1 = s.find_first_of("[");
  auto b2 = s.rfind("}]");
  if (found_null != string::npos) {
    return "";
  } else if (b1 != string::npos && b2 != string::npos) {
    return s.substr(b1, b2 - b1 + 2);
  }
  return "";
}

// Evaluate a polynomial.
double polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (unsigned int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals,
                        int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);
  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (unsigned int i = 0; i < xvals.size(); i++) {
    A(i, 0) = 1.0;
  }

  for (unsigned int j = 0; j < xvals.size(); j++) {
    for ( int i = 0; i < order; i++) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);
  return result;
}

void peigen (Eigen::VectorXd vect ) // send a vector to console
{

          for (unsigned int i = 0; i < vect.size() ; i++)
          {
            cout << vect[i] << endl;
          }
          cout << endl ;

}

void pvector (vector<double> vect ) // send a vector to console
{

          for (unsigned int i = 0; i < vect.size() ; i++)
          {
            cout << vect[i] << endl;
          }
          cout << endl ;

}

// https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector

Eigen::VectorXd stdvector2eigen(vector<double> v1)
{
          //from v1 to an eignen vector
          //double* ptr_data = &v1[0];
          Eigen::VectorXd v2 = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(v1.data(), v1.size());
          return v2;

}

vector<double> eigen2stdvector(Eigen::VectorXd v2)
{
    //from the eigen vector to the std vector
    std::vector<double> v3(&v2[0], v2.data()+v2.cols()*v2.rows());
    return v3;

}

size_t i = 0;

array<vector<double>, 2> World2Car(vector<double> vect_world_x, vector<double> vect_world_y , double gpx_x, double gps_y, double psi)
{
          cout << "Map\tX\t" ; pvector(vect_world_x);
          cout << "\tY\t" ; pvector(vect_world_y);
          for (unsigned int i = 0 ; i < vect_world_x.size() ; i++)
          {
            double x, y;
            x = vect_world_x[i] - gpx_x;
            y = vect_world_y[i] - gps_y;
            vect_world_x[i] =  x * cos(-psi) - y * sin(-psi);
            vect_world_y[i] =  x * sin(-psi) + y * cos(-psi);

          }
            cout << "Car\tX\t" ; pvector(vect_world_x);
            cout << "\tY\t" ; pvector(vect_world_y);
          array<vector<double>, 2> coord_set;
          coord_set[0] = vect_world_x;
          coord_set[1] = vect_world_y;
          return coord_set ;
}

int main() {
  uWS::Hub h;
  //std::cout << setprecision(8);
  // MPC is initialized here!
  MPC mpc;

  std::vector<double> x_vals ;
  std::vector<double> y_vals ;
  std::vector<double> psi_vals;
  std::vector<double> v_vals ;
  std::vector<double> cte_vals ;
  std::vector<double> epsi_vals ;
  std::vector<double> delta_vals ;
  std::vector<double> a_vals ;

  h.onMessage([&mpc, &x_vals, &y_vals, &psi_vals, &v_vals, &cte_vals, &epsi_vals, &delta_vals, &a_vals](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length, uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    string sdata = string(data).substr(0, length);
    //cout << sdata << endl;
    if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2') {
      string s = hasData(sdata);
      if (s != "") {
        auto j = json::parse(s);
        string event = j[0].get<string>();
        if (event == "telemetry") {
          // j[1] is the data JSON object
          vector<double> ptsx = j[1]["ptsx"];
          vector<double> ptsy = j[1]["ptsy"];
          double px = j[1]["x"];
          double py = j[1]["y"];
          double psi = j[1]["psi"];
          //double psi_unity = j[1]["psi_unity"];
          double v = j[1]["speed"];

          double delta = j[1]["steering_angle"];
          double a = j[1]["throttle"];

          /*
          * TODO: Calculate steering angle and throttle using MPC.
          *
          * Both are in between [-1, 1].
          *
          */

          Eigen::VectorXd ptsx_car(ptsx.size());
          Eigen::VectorXd ptsy_car(ptsy.size());

          for (unsigned int i = 0 ; i < ptsx.size() ; i++)
          {

            double x, y;
            x = ptsx[i] - px;
            y = ptsy[i] - py;
            ptsx_car[i] =  x * cos(-psi) - y * sin(-psi);
            ptsy_car[i] =  x * sin(-psi) + y * cos(-psi);

          }

          //////////

          auto coeffs = polyfit(ptsx_car , ptsy_car ,  3);

            // The cross track error is calculated by evaluating at polynomial at x, f(x)
            // and subtracting y. x = 0 , y = 0

            // in the vehicle coords
            double cte = polyeval(coeffs, 0) ;

            // Due to the sign starting at 0, the orientation error is -f'(x).
            // derivative of coeffs[0] + coeffs[1] * x -> coeffs[1]

            double epsi = -atan(coeffs[1]);

            Eigen::VectorXd state(6);

            double dt = 0.075;

            //predicted values
            double x_val=0.0 , y_val=0.0, psi_val=0.0, v_val = v, cte_val = cte, epsi_val = epsi;
            x_val    = v * cos(0) *dt ;
            y_val    = v * sin(0) *dt ;
            psi_val  = v * -delta / Lf *dt;
            v_val    = v + a *dt;
            cte_val  += v * sin(epsi) *dt;
            epsi_val += v * -delta / Lf *dt;

            //cout << x_val<< "\t" << y_val<< "\t" << psi_val<< "\t" << v_val<<"\t" << cte_val<<"\t" << epsi_val << "\t";
            //cout <<  epsi_val << "\t" << cte_val << "\t" << endl ;

            state << x_val, y_val, psi_val, v_val, cte_val, epsi_val;

            vector<double> vars;
            vars = mpc.Solve(state, coeffs);

            delta_vals.push_back(vars[0]);
            a_vals.push_back(vars[1]);
            cte_vals.push_back(vars[2]);

            psi_vals.push_back(vars[3]);
            epsi_vals.push_back(vars[4]);
            v_vals.push_back(vars[5]);

            x_vals.push_back(px);
            y_vals.push_back(py);

          iters++;

          delta = - vars[0] / (deg2rad(25) * Lf);
          a  = vars[1];

          json msgJson;

          // NOTE: Remember to divide by deg2rad(25) before you send the steering value back.
          // Otherwise the values will be in between [-deg2rad(25), deg2rad(25] instead of [-1, 1].
          msgJson["steering_angle"] = delta;
          msgJson["throttle"] = a;

          vector<double> mpc_x ;
          vector<double> mpc_y ;

          for (unsigned int i = 6 ; i < vars.size() ; i+=2 )
          {
            mpc_x.push_back(vars[i]);
            mpc_y.push_back(polyeval(coeffs , vars[i]) - vars[2]);
            //mpc_y.push_back(vars[i+1]) ;

          }

          //pvector(mpc_x);
          //pvector(mpc_y);

          msgJson["mpc_x"] = mpc_x;
          msgJson["mpc_y"] = mpc_y;

          //Display the waypoints/reference line

          vector<double> eig_next_x_vals = eigen2stdvector( ptsx_car );
          vector<double> eig_next_y_vals = eigen2stdvector( ptsy_car );

          vector<double> next_x_vals ;
          vector<double> next_y_vals ;

          //.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system
          // the points in the simulator are connected by a Yellow line

          double poly_incr = Lf;
          int n_pts = 25;

          for (signed int i = 1; i < n_pts ; i++)
          {
            next_x_vals.push_back(poly_incr * i);
            next_y_vals.push_back(polyeval(coeffs , poly_incr * i));
          }

          msgJson["next_x"] = next_x_vals ;
          msgJson["next_y"] = next_y_vals ;

          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          //std::cout << msg << endl << std::endl;

          // Latency
          // The purpose is to mimic real driving conditions where
          // the car does actuate the commands instantly.
          //
          // Feel free to play around with this value but should be to drive
          // around the track with 100ms latency.
          //
          // NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE
          // SUBMITTING.
          this_thread::sleep_for(chrono::milliseconds(100));
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);

          if (show_graph == true)
          {

          if (iters == 100)
            {

/*

            // Plot values
            // NOTE: feel free to play around with this.
            // It's useful for debugging!
            plt::subplot(6, 1, 1);
            plt::title("CTE");
            plt::plot(cte_vals);

            plt::subplot(6, 1, 2);
            plt::title("Delta (Radians)");
            plt::plot(delta_vals);

            plt::subplot(6, 1, 3);
            plt::title("Velocity");
            plt::plot(v_vals);

            plt::subplot(6, 1, 4);
            plt::title("Epsi");
            plt::plot(epsi_vals);

            plt::subplot(6, 1, 5);
            plt::title("x");
            plt::plot(x_vals);

            plt::subplot(6, 1, 6);
            plt::title("y");
            plt::plot(y_vals);

            cout << "game over";

            //plt::ion();
            plt::show();
            //plt::save("./graph.png");
            //exit(1);

            */

            }
          }

        }
      } else {
        // Manual driving
        std::string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }

  }

);

  // We don't need this since we're not using HTTP but if it's removed the
  // program
  // doesn't compile :-(
  h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
                     size_t, size_t) {
    const std::string s = "
Hello world!
";
    if (req.getUrl().valueLength == 1) {
      res->end(s.data(), s.length());
    } else {
      // i guess this should be done more gracefully?
      res->end(nullptr, 0);
    }
  });

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    std::cout << "Connected!!!" << std::endl;
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    std::cout << "Disconnected" << std::endl;
  });

  int port = 4567;
  if (h.listen(port)) {
    std::cout << "Listening to port " << port << std::endl;
  } else {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }
  h.run();
}


#pragma once

#include <Eigen/Core>
#include <optional>

namespace wmtk {

/**
 * Newton method or gradient descent.
 *
 * @param stack with flattened 4x3 vertices positions, with the mover vertex always on the front.
 * This is the same convention with AMIPS_energy
 * @return Descend direction as computed from Newton method, (or gradient descent if Newton fails).
 */
Eigen::Vector3d newton_method_from_stack(
    std::vector<std::array<double, 12>>& stack,
    std::function<double(const std::array<double, 12>&)> energy,
    std::function<void(const std::array<double, 12>&, Eigen::Vector3d&)> jacobian,
    std::function<void(const std::array<double, 12>&, Eigen::Matrix3d&)> hessian);

Eigen::Vector3d gradient_descent_from_stack(
    std::vector<std::array<double, 12>>& stack,
    std::function<double(const std::array<double, 12>&)> energy,
    std::function<void(const std::array<double, 12>&, Eigen::Vector3d&)> jacobian);
/**
 * Reorders indices in a tetrahedron such that v0 is on the front. Using the tetra symmetry to
 * preserve orientation. Assumes v0 in tetra.
 * @param conn
 * @param v0
 * @return std::array<size_t, 4>
 */
std::array<size_t, 4> orient_preserve_tet_reorder(const std::array<size_t, 4>& tetra, size_t v0);

/**
 * @brief Harmonic Triangulation energy: trace of Laplacian operator
 *
 */
double harmonic_energy(const Eigen::MatrixXd& verts);

std::optional<Eigen::Vector3d> try_project(
    const Eigen::Vector3d& point,
    const std::vector<std::array<double, 12>>& assembled_neighbor);
} // namespace wmtk