#include "gtest/gtest.h"

#include <vector>
#include <iostream>
#include <doublefann.h>
#include <fann_train.h>
#include <fann.h>
#include <boost/filesystem.hpp>

#include "MLP.hpp"
#include <bib/Utils.hpp>
#include <bib/MetropolisHasting.hpp>
#include <bib/Combinaison.hpp>

#define NB_SOL_OPTIMIZATION 25

double OR(double x1, double x2) {
  if (x1 == 1.f || x2 == 1.f)
    return 1.f;
  return -1.f;
}

double AND(double x1, double x2) {
  if (x1 == 1.f && x2 == 1.f)
    return 1.f;
  return -1.f;
}

double LECUNIZATION(double x){
  if(x == 1.f)
    return 1.5f;
  else
    return -1.5f;
}

double sech(double x) {
  return 2. / (exp(x) + exp(-x));
}

TEST(MLP, ConsistentActivationFunction) {
  MLP nn(1, {1}, 0, 0.01f);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 1.f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  std::vector<double> sens(0);
  std::vector<double> ac(1);
  ac[0] = 1;

  double lambda = 0.5;
  fann_set_activation_steepness_output(nn.getNeuralNet(), lambda);
  fann_set_activation_steepness(nn.getNeuralNet(), lambda, 1, 0);
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), tanh(lambda * 1.) / 2.);

  lambda = 0.8;
  fann_set_activation_steepness(nn.getNeuralNet(), lambda, 1, 0);
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), tanh(lambda * 1.) / 2.);

  ac[0] = -1;
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), tanh(lambda * -1.) / 2.);

  lambda = 0.5;
  fann_set_activation_steepness(nn.getNeuralNet(), lambda, 1, 0);
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), tanh(lambda * -1.) / 2.);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 0.f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 5.f);

  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), 5.f / 2.f);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 0.2f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.4f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 0.6f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.8f);

  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), (tanh(lambda * (-1. * 0.2f + 0.4f)) * 0.6f + 0.8f) / 2.f);

  fann_set_activation_steepness_output(nn.getNeuralNet(), 1.f);
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac),  tanh(lambda * (-1. * 0.2f + 0.4f)) * 0.6f + 0.8f);
}

TEST(MLP, LearnOpposite) {
  MLP nn(1, {1}, 0, 0.5f);

  fann_set_weight(nn.getNeuralNet(), 0, 2, -1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);

  fann_set_weight(nn.getNeuralNet(), 2, 4, 1. / tanh(0.5 * 1.));
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = x1 == 1.f ? -1.f : 1.f;

    std::vector<double> sens(0);
    std::vector<double> ac(1);
    ac[0] = x1;

    EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), out);
  }

  // Learn
  for (uint n = 0; n < 1000 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = x1 == 1.f ? -1.f : 1.f;

    std::vector<double> sens(0);
    std::vector<double> ac(1);
    ac[0] = x1;

    nn.learn(sens, ac, out);
  }

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = x1 == 1.f ? -1.f : 1.f;

    std::vector<double> sens(0);
    std::vector<double> ac(1);
    ac[0] = x1;

    EXPECT_EQ(nn.computeOutVF(sens, ac), out);
  }
}

TEST(MLP, LearnAndOr) {
  MLP nn(3, {10}, 2, 0.5f);

  // Learn
  for (uint n = 0; n < 10000 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;
    std::vector<double> ac(1);
    ac[0] = x3;

    nn.learn(sens, ac, out);
  }

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;
    std::vector<double> ac(1);
    ac[0] = x3;

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.02);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.02);
  }
}

#ifndef NO_OPTPP
TEST(MLP, MLPCheckOuput) {
  MLP nn(1, {1}, 2, 0.5f);
  std::vector<double> sens(0);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 1.f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  uint ndim = 1;
  passdata d = {nn.getNeuralNet(), sens};
  ColumnVector ac(ndim);
  ac(1) = -1.f;
  double fx = -1;
  int result = 0;
  ColumnVector gx(ndim);
  SymmetricMatrix Hx(ndim);
  hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);

  EXPECT_DOUBLE_EQ(fx, tanh(0.5 * 1.));

  fann_set_weight(nn.getNeuralNet(), 0, 2, 0.2f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.4f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 0.6f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.8f);

  hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);

  EXPECT_DOUBLE_EQ(-fx, tanh(0.5 * (-1. * 0.2f + 0.4f)) * 0.6f + 0.8f);
}

TEST(MLP, MLPCheckDerivative) {
  MLP nn(1, {1}, 2, 0.5f);
  std::vector<double> sens(0);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 1.f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  uint ndim = 1;
  passdata d = {nn.getNeuralNet(), sens};
  ColumnVector ac(ndim);
  double a = -1;
  ac(1) = a;
  double fx = -1;
  int result = 0;

  ColumnVector gx(ndim);

  SymmetricMatrix Hx(ndim);

  double h = 1e-10;
  ac(1) = a - h;
  hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
  double fx_base = fx;
  ac(1) = a + h;
  hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
  double fx_h = fx;
  double derivative1 = (fx_h - fx_base) / (2.0 * h);

  ac(1) = a;
  hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
  double derivative2 = (fx_h - fx) / h;

  hs65(NLPGradient, ndim, ac, fx, gx, Hx, result, &d);

  EXPECT_GT(derivative1, gx(1) - 1e-5);
  EXPECT_LT(derivative1, gx(1) + 1e-5);

  EXPECT_GT(derivative2, gx(1) - 1e-5);
  EXPECT_LT(derivative2, gx(1) + 1e-5);
}

TEST(MLP, MLPCheckAnalyticscFuncDerHess) {
  MLP nn(1, {1}, 0, 0.5f);

  fann_set_weight(nn.getNeuralNet(), 0, 2, -1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);

  fann_set_weight(nn.getNeuralNet(), 2, 4, 1. / tanh(0.5 * 1.));
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  uint ndim = 1;
  std::vector<double> sens(0);
  passdata d = {nn.getNeuralNet(), sens};
  ColumnVector ac(ndim);
  double a = -1;
  ac(1) = a;
  double fx = -1;
  int result = 0;
  ColumnVector gx(ndim);

  SymmetricMatrix Hx(ndim);

  double C = 1. / (2. * tanh(0.5)) ;

  //exact computation
  for (a = -1; a <= 1.0; a += 1e-2) {
    ac(1) = a;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    EXPECT_GT(- fx, tanh(-0.5 * a) / tanh(0.5)  - 1e-7);  //opposite taking to maximaze
    EXPECT_LT(- fx, tanh(-0.5 * a) / tanh(0.5)  + 1e-7);

    hs65(NLPGradient, ndim, ac, fx, gx, Hx, result, &d);
    EXPECT_GT(- gx(1), - C * sech(0.5 * a) * sech(0.5 * a) - 1e-7);
    EXPECT_LT(- gx(1), - C * sech(0.5 * a) * sech(0.5 * a) + 1e-7);

    hs65(NLPHessian, ndim, ac, fx, gx, Hx, result, &d);
    EXPECT_GT(- Hx(1, 1), C * tanh(0.5 * a) * sech(0.5 * a) * sech(0.5 * a) - 1e-7);
    EXPECT_LT(- Hx(1, 1), C * tanh(0.5 * a) * sech(0.5 * a) * sech(0.5 * a) + 1e-7);
  }
}

TEST(MLP, MLPCheckDerivativeHard) {
  MLP nn(4, {10}, 3, 0.5f);
  std::vector<double> sens(3);
  sens[0] = bib::Utils::randin(-1, 1);
  sens[1] = bib::Utils::randin(-1, 1);
  sens[2] = bib::Utils::randin(-1, 1);

  uint ndim = 1;
  passdata d = {nn.getNeuralNet(), sens};
  ColumnVector ac(ndim);
  ColumnVector gx(ndim);
  SymmetricMatrix Hx(ndim);
  int result = 0;
  double fx = -1;

  for (double a = -1; a <= 1.0; a += 1e-2) {
    ac(1) = a;

    double h = 1e-10;
    ac(1) = a - h;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    double fx_base = fx;
    ac(1) = a + h;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    double fx_h = fx;
    double derivative1 = (fx_h - fx_base) / (2.0 * h);

    ac(1) = a;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    double derivative2 = (fx_h - fx) / h;

    hs65(NLPGradient, ndim, ac, fx, gx, Hx, result, &d);

    EXPECT_GT(derivative1, gx(1) - 1e-5);
    EXPECT_LT(derivative1, gx(1) + 1e-5);

    EXPECT_GT(derivative2, gx(1) - 1e-5);
    EXPECT_LT(derivative2, gx(1) + 1e-5);
  }
}

TEST(MLP, MLPCheckHessian) {
  MLP nn(4, {10}, 3, 0.5f);
  std::vector<double> sens(3);
  sens[0] = bib::Utils::randin(-1, 1);
  sens[1] = bib::Utils::randin(-1, 1);
  sens[2] = bib::Utils::randin(-1, 1);

  uint ndim = 1;
  passdata d = {nn.getNeuralNet(), sens};
  ColumnVector ac(ndim);
  ColumnVector gx(ndim);
  SymmetricMatrix Hx(ndim);
  int result = 0;
  double fx = -1;

  for (double a = -1; a <= 1.0; a += 1e-2) {
    ac(1) = a;

    double h = 1e-5;
    ac(1) = a - h;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    double fx_base = fx;
    ac(1) = a + h;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    double fx_h = fx;

    ac(1) = a;
    hs65(NLPFunction, ndim, ac, fx, gx, Hx, result, &d);
    double snd_derivative = (fx_h - 2.0 * fx + fx_base) / (h * h);

    hs65(NLPHessian, ndim, ac, fx, gx, Hx, result, &d);

    EXPECT_GT(snd_derivative, Hx(1, 1) - 1e-5);
    EXPECT_LT(snd_derivative, Hx(1, 1) + 1e-5);
  }
}

TEST(MLP, OptimizeOpposite) {
  MLP nn(1, {1}, 0, 0.5f);

  fann_set_weight(nn.getNeuralNet(), 0, 2, -1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);

  fann_set_weight(nn.getNeuralNet(), 2, 4, 1. / tanh(0.5 * 1.));
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  // Test
  std::vector<double> sens(0);
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = x1 == 1.f ? -1.f : 1.f;

    std::vector<double> ac(1);
    ac[0] = x1;

    EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), out);
  }

  std::vector<double>* ac = nn.optimized(sens);
  EXPECT_GT(ac->at(0), -1. - 0.01);
  EXPECT_LT(ac->at(0), -1. + 0.01);
  delete ac;

  fann_set_weight(nn.getNeuralNet(), 0, 2, 1.f);
  ac = nn.optimized(sens);
  EXPECT_GT(ac->at(0), 1. - 0.01);
  EXPECT_LT(ac->at(0), 1. + 0.01);
  delete ac;

  // Learn
  for (uint n = 0; n < 1000 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = x1 == 1.f ? -1.f : 1.f;

    std::vector<double> sens(0);
    std::vector<double> acc(1);
    acc[0] = x1;

    nn.learn(sens, acc, out);
  }

  ac = nn.optimized(sens);
  EXPECT_GT(ac->at(0), -1. - 0.01);
  EXPECT_LT(ac->at(0), -1. + 0.01);

//     LOG_DEBUG(nn.computeOut(sens, *ac));
  ac->at(0) = -1;
//     LOG_DEBUG(nn.computeOut(sens, *ac));
  delete ac;
}

TEST(MLP, OptimizeAndOr) {
  MLP nn(3, {10}, 2, 0.5f);

  // Learn
  for (uint n = 0; n < 10000 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;
    std::vector<double> ac(1);
    ac[0] = x3;

    nn.learn(sens, ac, out);
  }

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;

    std::vector<double>* ac = nn.optimized(sens);
    double his_sol = nn.computeOutVF(sens, *ac);

    ac->at(0) = 1.f;
    double my_sol = nn.computeOutVF(sens, *ac);

    EXPECT_GE(his_sol, my_sol - 0.01);
    delete ac;
  }
}

TEST(MLP, OptimizeMultiDim) {
  MLP nn(3, {20}, 0, 0.5f);

  // Learn
  for (uint n = 0; n < 10000 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(0);
    std::vector<double> ac(3);
    ac[0] = x1;
    ac[1] = x2;
    ac[2] = x3;

    nn.learn(sens, ac, out);
  }

  // Test
  for (uint n = 0; n < 100 ; n++) {
    std::vector<double> sens(0);
    std::vector<double>* ac = nn.optimized(sens);
    double his_sol = nn.computeOutVF(sens, *ac);
    double my_sol = 1.;

    EXPECT_GE(his_sol, my_sol - 0.01);
    delete ac;
  }
}

TEST(MLP, OptimizeNonExtremum) {
  //learn x^2 - x
  //root min ~0.474
  //root max -1

  double sensv = 1. ;
  for (uint n = 0; n < 1 ; n++) {
    MLP nn(1, {3}, 0, 0.2f);
    std::vector<double> sens(0);
    std::vector<double> ac(1);

    // Learn
    for (uint n = 0; n < 100000 ; n++) {
      double x1 = (bib::Utils::rand01() * 2.) - 1;

      double out = x1 * x1 - x1;
      ac[0] = x1;

      nn.learn(sens, ac, sensv * out);
    }

    //Test learned
    for (uint n = 0; n < 1000 ; n++) {
      double x1 = (bib::Utils::rand01() * 2.) - 1;

      double out = x1 * x1 - x1;
      ac[0] = x1;

      double myout = nn.computeOutVF(sens, ac);
      EXPECT_GT(myout, sensv * out - 0.1);
      EXPECT_LT(myout, sensv * out + 0.1);
      LOG_FILE("OptimizeNonExtremum.data", x1 << " " << myout << " " << out);
//     close all; X=load('OptimizeNonExtremum.data'); plot(X(:,1),X(:,2), '.'); hold on; plot(X(:,1),X(:,3), 'r.');
    }

    std::vector<double>* acopt = nn.optimized(sens, {}, NB_SOL_OPTIMIZATION);
    if (sensv == -1.) {
      EXPECT_GT(acopt->at(0), 0.46);
      EXPECT_LT(acopt->at(0), 0.48);
    } else {
      EXPECT_LT(acopt->at(0), -0.99);
      EXPECT_GT(acopt->at(0), -1.01);
    }
    delete acopt;
    sensv *= -1;
  }
}

TEST(MLP, Optimize2LocalMaxima) {
  //learn - cos(5.*x1)/2.

  MLP nn(1, {10}, 0, 0.1f);
  std::vector<double> sens(0);
  std::vector<double> ac(1);

  fann_set_training_algorithm(nn.getNeuralNet(), FANN_TRAIN_RPROP); //adaptive algorithm without learning rate
  fann_set_train_error_function(nn.getNeuralNet(), FANN_ERRORFUNC_TANH);

  struct fann_train_data* data = fann_create_train(2500, 1, 1);

  // Learn
  for (uint n = 0; n < 2500 ; n++) {
    double x1 = (bib::Utils::rand01() * 2.) - 1;

    double out = - cos(5.*x1) / 2.;
    ac[0] = x1;

    data->input[n][0] = x1;
    data->output[n][0] = out;
  }

  fann_train_on_data(nn.getNeuralNet(), data, 3000, 0, 0.0005);
  fann_destroy_train(data);

  //Test learned
  for (uint n = 0; n < 1000 ; n++) {
    double x1 = (bib::Utils::rand01() * 2.) - 1;

    double out = - cos(5.*x1) / 2.;
    ac[0] = x1;

    double myout = nn.computeOutVF(sens, ac);
    LOG_FILE("Optimize2LocalMaxima.data", x1 << " " << myout << " " << out);
//     close all; X=load('Optimize2LocalMaxima.data'); plot(X(:,1),X(:,2), '.'); hold on; plot(X(:,1),X(:,3), 'r.');
  }

  std::vector<double>* acopt = nn.optimized(sens, {}, NB_SOL_OPTIMIZATION);
//     ac[0] = acopt->at(0);
//     double my_sol = nn.computeOut(sens, ac);
//     ac[0] = M_PI/5;
//     double best_sol = nn.computeOut(sens, ac);
//     LOG_DEBUG("my sol : " << my_sol << " | best : " << best_sol << " (" << acopt->at(0) << " vs " << M_PI/5 << ") ");

  double precision = 0.1;
  if (acopt->at(0) > 0) {
    EXPECT_GT(acopt->at(0), M_PI / 5. - precision);
    EXPECT_LT(acopt->at(0), M_PI / 5. + precision);
  } else {
    EXPECT_GT(acopt->at(0), -M_PI / 5. - precision);
    EXPECT_LT(acopt->at(0), -M_PI / 5 + precision);
  }

  delete acopt;
}

TEST(MLP, OptimizeTrapInEvilLocalOptimal) {
  //learn sin(-3*x*x*x+2*x*x+x)
  //local max ~0.6
  //global max -0.7

  MLP nn(1, {12}, 0, 0.1f);
  std::vector<double> sens(0);
  std::vector<double> ac(1);

  fann_set_training_algorithm(nn.getNeuralNet(), FANN_TRAIN_RPROP); //adaptive algorithm without learning rate
  fann_set_train_error_function(nn.getNeuralNet(), FANN_ERRORFUNC_TANH);

  struct fann_train_data* data = fann_create_train(2500, 1, 1);

  // Learn
  for (uint n = 0; n < 2500 ; n++) {
    double x1 = (bib::Utils::rand01() * 2.) - 1;

    double out = sin(-3 * x1 * x1 * x1 + 2 * x1 * x1 + x1);
    ac[0] = x1;

    data->input[n][0] = x1;
    data->output[n][0] = out;
  }

  fann_train_on_data(nn.getNeuralNet(), data, 10000, 0, 0.0005);
  fann_destroy_train(data);

  //Test learned
  for (uint n = 0; n < 1000 ; n++) {
    double x1 = (bib::Utils::rand01() * 2.) - 1;

    double out = sin(-3 * x1 * x1 * x1 + 2 * x1 * x1 + x1);
    ac[0] = x1;

    double myout = nn.computeOutVF(sens, ac);
    LOG_FILE("OptimizeTrapInEvilLocalOptimal.data", x1 << " " << myout << " " << out);
//     close all; X=load('OptimizeTrapInEvilLocalOptimal.data'); plot(X(:,1),X(:,2), '.'); hold on; plot(X(:,1),X(:,3), 'r.');
  }

  std::vector<double>* acopt = nn.optimized(sens, {}, NB_SOL_OPTIMIZATION+100);
//     LOG_DEBUG(acopt->at(0));

  double precision = 0.1;

  if(acopt->at(0) < -0.7 - precision || acopt->at(0) > -0.7 + precision) {
    LOG_DEBUG(nn.computeOutVF(sens, *acopt));
    ac[0] = -0.7;
    LOG_DEBUG(nn.computeOutVF(sens, ac));
  }

  EXPECT_GT(acopt->at(0), -0.7 - precision);
  EXPECT_LT(acopt->at(0), -0.7 + precision);

  delete acopt;
}

TEST(MLP, OptimizePlateau) {
  //learn sin(-3*x*x*x+2*x*x+x)
  //local max ~0.6
  //global max -0.7

  MLP nn(1, {2}, 0, 0.1f);
  std::vector<double> sens(0);
  std::vector<double> ac(1);

  fann_set_training_algorithm(nn.getNeuralNet(), FANN_TRAIN_RPROP); //adaptive algorithm without learning rate
  fann_set_train_error_function(nn.getNeuralNet(), FANN_ERRORFUNC_TANH);

  struct fann_train_data* data = fann_create_train(5000, 1, 1);

  // Learn
  for (uint n = 0; n < 5000 ; n++) {
    double x1 = n < 2000 ? (bib::Utils::rand01() * 2.) - 1 : bib::Utils::randin(0, 0.5);

    double out = x1 >= 0.2 ? 1. : 0.2;
    ac[0] = x1;

    data->input[n][0] = x1;
    data->output[n][0] = out;
  }

  fann_train_on_data(nn.getNeuralNet(), data, 10000, 0, 0.00001);
  fann_destroy_train(data);

  //Test learned
  for (uint n = 0; n < 1000 ; n++) {
    double x1 = (bib::Utils::rand01() * 2.) - 1;

    double out = x1 >= 0.2 ? 1. : 0.2;
    ac[0] = x1;

    double myout = nn.computeOutVF(sens, ac);
    LOG_FILE("OptimizePlateau.data", x1 << " " << myout << " " << out);
//     close all; X=load('OptimizePlateau.data'); plot(X(:,1),X(:,2), '.'); hold on; plot(X(:,1),X(:,3), 'r.');
  }

  std::vector<double>* acopt = nn.optimized(sens, {}, NB_SOL_OPTIMIZATION);
//     LOG_DEBUG(acopt->at(0));

  double precision = 0.01;

  EXPECT_GT(acopt->at(0), 0.2 - precision);

  delete acopt;
}
#endif //NO_OPTPP

TEST(MLP, ConsistentActivationFunctionLecun) {
  MLP nn(1, {1}, 0, 0.01f, true);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 1.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 1.f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.f);

  std::vector<double> sens(0);
  std::vector<double> ac(1);
  ac[0] = 1.d;

  double lambda = atanh(1.d/sqrt(3.d));
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), sqrt(3.d)*tanh(lambda * 1.d));

  ac[0] = 1.5d;
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), sqrt(3.d)*tanh(lambda * 1.5d));//1.31

  fann_set_weight(nn.getNeuralNet(), 0, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 0.f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 5.f);

  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), 5.f);

  fann_set_weight(nn.getNeuralNet(), 0, 2, 0.2f);
  fann_set_weight(nn.getNeuralNet(), 1, 2, 0.4f);
  fann_set_weight(nn.getNeuralNet(), 2, 4, 0.6f);
  fann_set_weight(nn.getNeuralNet(), 3, 4, 0.8f);

  ac[0] = -1.d;
  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), (sqrt(3.d)*tanh(lambda * (-1. * 0.2f + 0.4f)) * 0.6f + 0.8f));
}

TEST(MLP, LearnAndOrLecun) {
  MLP nn(3, {10}, 2, 0.05f, true);

  // Learn
  for (uint n = 0; n < 10000 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;
    std::vector<double> ac(1);
    ac[0] = x3;

    nn.learn(sens, ac, out);
  }

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;
    std::vector<double> ac(1);
    ac[0] = x3;

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.02);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.02);
  }
}

TEST(MLP, LearnAndOrLecun2) {
  MLP nn(3, {5}, 1, true);

  struct fann_train_data* data = fann_create_train(2*2*2, 3, 1);

  uint n = 0;
  auto iter = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->input[n][1]= x[1];
    data->input[n][2]= x[2];

    double out = AND(OR(x[0], x[1]), x[2]);
    data->output[n][0]= out;
    n++;
  };

  bib::Combinaison::continuous<>(iter, 3, -1, 1, 1);

  nn.learn_stoch(data, 20000, 0, 0.00000001);

  fann_destroy_train(data);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::randBool() ? 1.f : -1.f;
    double x2 = bib::Utils::randBool() ? 1.f : -1.f;
    double x3 = bib::Utils::randBool() ? 1.f : -1.f;

    double out = AND(OR(x1, x2), x3);

    std::vector<double> sens(2);
    sens[0] = x1;
    sens[1] = x2;
    std::vector<double> ac(1);
    ac[0] = x3;

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.02);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.02);

    std::vector<double> in(3);
    in[0] = x1;
    in[1] = x2;
    in[2] = x3;
    std::vector<double>* outnn = nn.computeOut(in);

    EXPECT_GT(outnn->at(0), out - 0.05);
    EXPECT_LT(outnn->at(0), out + 0.05);

    delete outnn;
  }
}

// 1 0.2
// 3 0.3
// 5 0.3
//
// Result:  y = 0.025 x + 1.916666667·10-1
//
// 2.166666667·10-1 		 1.666666667·10-2
// 2.666666667·10-1 		 3.333333333·10-2
// 3.166666667·10-1 		 1.666666667·10-2
TEST(MLP, LearnLabelWeight) {
  fann* lin_nn =fann_create_standard(2, 1, 1);
  fann_set_activation_function_output(lin_nn, FANN_LINEAR);
  fann_set_learning_momentum(lin_nn, 0.);
  fann_set_training_algorithm(lin_nn, FANN_TRAIN_RPROP);

  struct fann_train_data* data = fann_create_train(3, 1, 1);
  fann_type lw [3];

  uint n = 0;
  data->input[n][0]= 1.f;
  data->output[n][0]= 2.f/10.f;
  lw[n] = 1.f;
  n++;
  data->input[n][0]= 3.f;
  data->output[n][0]= 3.f/10.f;
  lw[n] = 0.5f;
  n++;
  data->input[n][0]= 5.f;
  data->output[n][0]= 3.f/10.;
  lw[n] = 1.f;
  n++;

  for(int i=0;i<1000; i++)
    fann_train_epoch(lin_nn, data);

  fann_type out_no_lw[3];
  fann_type * out;
  n=0;
  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.216 - 0.002);
  EXPECT_LT(out[0], 0.216 + 0.002);
  out_no_lw[n]=out[0];
  n++;

  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.266 - 0.002);
  EXPECT_LT(out[0], 0.266 + 0.002);
  out_no_lw[n]=out[0];
  n++;

  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.316 - 0.002);
  EXPECT_LT(out[0], 0.316 + 0.002);
  out_no_lw[n]=out[0];

  for(int i=0;i<1000; i++)
    fann_train_epoch_lw(lin_nn, data, lw);

  n=0;
  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.2);
  EXPECT_LT(out[0], out_no_lw[n]);
  out_no_lw[n]=out[0];
  n++;

  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.25);
  EXPECT_LT(out[0], out_no_lw[n]);
  out_no_lw[n]=out[0];
  n++;

  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.3);
  EXPECT_LT(out[0], out_no_lw[n]);
  out_no_lw[n]=out[0];

  lw[1] = 0.05;
  for(int i=0;i<1000; i++)
    fann_train_epoch_lw(lin_nn, data, lw);

  n=0;
  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.2);
  EXPECT_LT(out[0], out_no_lw[n]);
  n++;

  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.25);
  EXPECT_LT(out[0], out_no_lw[n]);
  n++;

  out = fann_run(lin_nn, data->input[n]);
  EXPECT_GT(out[0], 0.3);
  EXPECT_LT(out[0], out_no_lw[n]);

  fann_destroy_train(data);
}

TEST(MLP, LearnNonLinearLabelWeight) {
  MLP nn(1, {4}, 1, true);

  struct fann_train_data* data = fann_create_train(11*2, 1, 1);
  fann_type label_weight[11*2];

  uint n = 0;
  auto iter = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->output[n][0]= x[0];
    label_weight[n] = 1.f;
    n++;
  };

  auto iter2 = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->output[n][0]= x[0]*x[0];
    label_weight[n] = 0.2f;
    n++;
  };

  bib::Combinaison::continuous<>(iter, 1, 0, 1, 10);
  bib::Combinaison::continuous<>(iter2, 1, 0, 1, 10);

  nn.learn_stoch(data, 20000, 0, 0.00000001);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01();

    double out = (x1 + x1*x1)/2.;

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(0);

    double old_out1 = x1;
    double old_out2 = x1*x1;

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.15);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.15);

    if(x1 > 0.15 && x1 < 0.85){
      EXPECT_GE(fabs(nn.computeOutVF(sens, ac) - old_out1), fabs(nn.computeOutVF(sens, ac) - out));
      EXPECT_GE(fabs(nn.computeOutVF(sens, ac) - old_out2), fabs(nn.computeOutVF(sens, ac) - out));
    }

  }

  nn.learn_stoch_lw(data, label_weight, 20000, 0, 0.00000001);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01();

    double old_out = (x1 + x1*x1)/2.;
    double out = (x1 + 0.2*x1*x1)/1.2;

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(0);

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.1);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.1);

    if(x1 > 0.15 && x1 < 0.85)
      EXPECT_GE(fabs(nn.computeOutVF(sens, ac) - old_out), fabs(nn.computeOutVF(sens, ac) - out));
  }

  fann_destroy_train(data);
}

TEST(MLP, LearnNonLinearLabelWeightWithNullImportance) {
  MLP nn(1, {4}, 1, true);

  struct fann_train_data* data = fann_create_train(11*2, 1, 1);
  fann_type label_weight[11*2];

  uint n = 0;
  auto iter = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->output[n][0]= x[0];
    label_weight[n] = 0.0f;
    n++;
  };

  auto iter2 = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->output[n][0]= -x[0];
    label_weight[n] = 0.8f;
    n++;
  };

  bib::Combinaison::continuous<>(iter, 1, 0, 1, 10);
  bib::Combinaison::continuous<>(iter2, 1, 0, 1, 10);

  nn.learn_stoch_lw(data, label_weight, 20000, 0, 0.00000001);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01();

    double out = (0.0*x1 - 0.8*x1)/(0.0+0.8);

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(0);

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.15);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.15);
  }

  fann_destroy_train(data);
}

double derivative(double* s, double* a, int, void*){
    return -(2*a[0] -s[0]*s[0]);
}

// try to learn a function pi that maximize another one : f(s, pi(a)) = a^2-(s^2)*a
TEST(MLP, OptimizeNNTroughGradient) {
  MLP nn(1, {4}, 1, 0.0, true);

  struct fann_train_data* data = fann_create_train(200, 1, 1);

  uint n = 0;
  auto iter = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
//     data->output[n][0]= -x[0]*x[0];
    data->output[n][0]= x[0];//don't care
    n++;
  };

  bib::Combinaison::continuous<>(iter, 1, -1, 1, 200);

  for(int i=0;i<10000; i++)
    fann_train_epoch_irpropm_gradient(nn.getNeuralNet(), data, derivative, nullptr);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01()*2 -1;

    double out = (x1 * x1)/2;

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(0);

    EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.1);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.1);

    LOG_FILE("OptimizeNNTroughGradient.data", x1 << " " << nn.computeOutVF(sens, ac) << " " << out);
  }

  fann_destroy_train(data);
}

class my_weights {
  typedef struct fann_connection sfn;

 public:
  my_weights(NN neural_net, const double* sensors, const double* x, uint _m, uint _n) : m(_m), n(_n) {
    _lambda = fann_get_activation_steepness(neural_net, 1, 0);

    unsigned int number_connection = fann_get_total_connections(neural_net);
    connections = reinterpret_cast<sfn*>(calloc(number_connection, sizeof(sfn)));

    fann_get_connection_array(neural_net, connections);

    uint number_layer = fann_get_num_layers(neural_net);
    layers = reinterpret_cast<uint*>(calloc(number_layer, sizeof(sfn)));

    fann_get_layer_array(neural_net, layers);
    ASSERT(number_layer == 3, number_layer);

    for (uint i = 0; i < h() * (m + n + 1); i++) {
      _ASSERT_EQ(connections[i].from_neuron, i % (m + n + 1));
      _ASSERT_EQ(connections[i].to_neuron, (m + n + 1) + i / (m + n + 1));
    }

    Ci.clear();
    Ci.resize(h());
    for (uint i = 0; i < h(); i++) {
      Ci[i] = 0;
      for (uint j = 0; j < m; j++)
        Ci[i] += sensors[j] * w(j, i);

      Ci[i] += connections[ i * (m + n + 1) + m + n].weight;
      _ASSERT_EQ(connections[ i * (m + n + 1) + m + n].from_neuron, m + n);
      _ASSERT_EQ(connections[ i * (m + n + 1) + m + n].to_neuron, m + n + 1 + i);
    }

    Di.clear();
    Di.resize(h());
    for (uint i = 0; i < h(); i++) {
      Di[i] = Ci[i];
      for (uint j = 0; j < n; j++)
        Di[i] += x[j] * w(m + j, i);
    }

    ASSERT(number_connection == h() * (m + n + 1) + (h() + 1), "");
  }

  ~my_weights() {
    free(connections);
    free(layers);
  }

  double v(uint i) const {
    sfn conn = connections[ h() * (m + n + 1) + i];

    _ASSERT_EQS(conn.from_neuron, (m + n + 1) + i, " i: " << i  << " m " << m << " n " << n << " h " << h());
    _ASSERT_EQS(conn.to_neuron, (m + n + 1) + (h() + 1), " i: " << i  << " m " << m << " n " << n << " h " << h());
    return conn.weight;
  }

  double w(uint j, uint i) const {
    sfn conn = connections[ i * (m + n + 1) + j];

    _ASSERT_EQS(conn.from_neuron, j, " i: " << i << " j : " << j  << " m " << m << " n " << n << " h " << h());
    _ASSERT_EQS(conn.to_neuron, (m + n + 1) + i, " i: " << i << " j : " << j  << " m " << m << " n " << n << " h " << h());
    return conn.weight;
  }

  uint h() const {
    return layers[1];
  }

  double C(uint i) const {
    return Ci[i];
  }

  double D(uint i) const {
    return Di[i];
  }

  double lambda() const {
    return _lambda;
  }

 private :
  sfn* connections;
  uint* layers;
  std::vector<double> Ci;
  std::vector<double> Di;
  uint m, n;
  double _lambda;
};

#define activation_function(x, lambda) tanh(lambda * x)

double derivative2(double* input, double *neuron_value, int, void* data){
    MLP* nn = (MLP*)data;
    my_weights _w(nn->getNeuralNet(), input, neuron_value, 1, 1);

    std::vector<double> gx(1);
    for (uint j = 0; j < 1; j++) {
      gx[j] = 0;
      for (uint i = 0; i < _w.h() ; i++) {
        double der = activation_function(_w.D(i), _w.lambda());
        gx[j] = gx[j] + _w.v(i) * _w.w(1 + j, i) * _w.lambda() * (1.0 - der * der);
      }
    }

//     LOG_DEBUG((-(2*neuron_value[0] -input[0]*input[0])) << " " << gx[0]);
    return gx[0];
}

TEST(MLP, OptimizeNNTroughGradientOfAnotherNN) {
  MLP nn(2, {50}, 1, 0.0);
  MLP actor(1, {4}, 1);

  struct fann_train_data* data = fann_create_train(200*200, 2, 1);

  uint n = 0;
  auto iter = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->input[n][1]= x[1];

    data->output[n][0]= -(x[1]*x[1]-(x[0]*x[0])*x[1]);
    n++;
  };

  bib::Combinaison::continuous<>(iter, 2, -1, 1, 200);

  nn.learn_stoch(data, 500, 100, 0.0000001,200);
  fann_destroy_train(data);

  data = fann_create_train(2000, 1, 1);
  n = 0;
  auto iter2 = [&](const std::vector<double>& x) {
    data->input[n][0]= x[0];
    data->output[n][0]= x[0];//don't care
    n++;
  };
  bib::Combinaison::continuous<>(iter2, 1, -1, 1, 2000);

  //fann_type *error_begin = nn.getNeuralNet()->train_errors;

  for(int i=0;i<1000; i++)
     fann_train_epoch_irpropm_gradient(actor.getNeuralNet(), data, derivative2, &nn);

  // Test
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01()*2 -1;

    double out = (x1 * x1)/2;
    double qout = -(out*out-(x1*x1)*out);

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(1);
    std::vector<double> ac_empty(0);
    ac[0]= out;

    EXPECT_GT(nn.computeOutVF(sens, ac), qout - 0.15);
    EXPECT_LT(nn.computeOutVF(sens, ac), qout + 0.15);

    EXPECT_GT(actor.computeOutVF(sens, ac_empty), out - 0.2);
    EXPECT_LT(actor.computeOutVF(sens, ac_empty), out + 0.2);

    LOG_FILE("OptimizeNNTroughGradientOfAnotherNN.data", x1 << " " << actor.computeOutVF(sens, ac_empty) << " " << out);
  }

  fann_destroy_train(data);
}

TEST(MLP, OptimizeNNTroughGradientOfAnotherNNFann) {
  MLP nn(2, {50}, 1, 0.0, true);
//   MLP nn(2, 1, 1, 0.0);
  MLP actor(1, {8}, 1);

  MLP actor2(actor);
  fann_set_activation_function_output(actor.getNeuralNet(), FANN_SIGMOID_SYMMETRIC);
  fann_set_activation_function_output(actor2.getNeuralNet(), FANN_LINEAR);

  struct fann_train_data* data;
  if ( !boost::filesystem::exists( "OptimizeNNTroughGradientOfAnotherNNFann.cache.data" ) ){
    data = fann_create_train(200*200, 2, 1);

    uint n = 0;
    auto iter = [&](const std::vector<double>& x) {
      data->input[n][0]= x[0];
      data->input[n][1]= x[1];

      data->output[n][0]= -(x[1]*x[1]-(x[0]*x[0])*x[1]);
      n++;
    };

    bib::Combinaison::continuous<>(iter, 2, -1, 1, 200);

    nn.learn_stoch(data, 5800, 100, 0.0000001,500);
    //nn.learn(data, 300, 0);

    fann_destroy_train(data);
    nn.save("OptimizeNNTroughGradientOfAnotherNNFann.cache.data");
  } else
    nn.load("OptimizeNNTroughGradientOfAnotherNNFann.cache.data");

  datann_derivative d = {&nn, 1, 1};

  for(int i=0;i<5000; i++){
     data = fann_create_train(4000, 1, 1);
     for (uint n = 0; n < 4000 ; n++){
       data->input[n][0]= bib::Utils::rand01()*2-1.;
       data->output[n][0]= 0;
     }

     fann_train_epoch_irpropm_gradient(actor.getNeuralNet(), data, derivative_nn, &d);

     fann_train_epoch_irpropm_gradient(actor2.getNeuralNet(), data, derivative_nn_inverting, &d);
     if(i%200 == 0)
      LOG_DEBUG(actor.weight_l1_norm() << " " << actor2.weight_l1_norm());

     fann_destroy_train(data);
  }

  // Test
  double ac1_error = 0;
  double ac2_error = 0;
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01()*2 -1;

    double out = (x1 * x1)/2;
    //out = x1 > 0 ? 0 : -1.f;
    double qout = out*out-(x1*x1)*out;
    qout = - qout;

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(1);
    std::vector<double> ac_empty(0);
    ac[0]= out;

    EXPECT_GT(nn.computeOutVF(sens, ac), qout - 0.2);
    EXPECT_LT(nn.computeOutVF(sens, ac), qout + 0.2);

    EXPECT_GT(actor.computeOutVF(sens, ac_empty), out - 0.25);
    EXPECT_LT(actor.computeOutVF(sens, ac_empty), out + 0.25);

    EXPECT_GT(actor2.computeOutVF(sens, ac_empty), out - 0.25);
    EXPECT_LT(actor2.computeOutVF(sens, ac_empty), out + 0.25);

    LOG_FILE("OptimizeNNTroughGradientOfAnotherNNFann.data", x1 << " " << actor.computeOutVF(sens, ac_empty) << " " <<
                                                              actor2.computeOutVF(sens, ac_empty)<< " " << out);
    //clear all; close all; X=load('OptimizeNNTroughGradientOfAnotherNNFann.data'); plot(X(:,1),X(:,2), '.'); hold on; plot(X(:,1),X(:,3), 'r.');plot(X(:,1),X(:,4), 'go');
    ac1_error += fabs(out - actor.computeOutVF(sens, ac_empty));
    ac2_error += fabs(out - actor2.computeOutVF(sens, ac_empty));
  }

  ac1_error /= 100;
  ac2_error /= 100;

  LOG_DEBUG(ac1_error << " " << ac2_error);
}

TEST(MLP, OptimizeNNTroughGradientOfAnotherNNFannMinDerivative) {
  MLP nn(2, {50}, 1, 0.0, true);
  MLP actor(1, {8}, 1);

  MLP actor2(actor);
  fann_set_activation_function_output(actor.getNeuralNet(), FANN_SIGMOID_SYMMETRIC);
  fann_set_activation_function_output(actor2.getNeuralNet(), FANN_LINEAR);

  struct fann_train_data* data;
  if ( !boost::filesystem::exists( "OptimizeNNTroughGradientOfAnotherNNFannMinDerivative.cache.data" ) ){
    data = fann_create_train(200*200, 2, 1);

    for(uint n = 0; n < 200*200 ;n++){
      double x0 = bib::Utils::rand01()*2 -1;
      double x1 = bib::Utils::rand01()*2 -1;
      data->input[n][0]= x0;
      data->input[n][1]= x1;

      data->output[n][0]= x1*x1-(x0*x0)*x1;
    }

    nn.learn_stoch(data, 5800, 100, 0.0000001,1000);
    //nn.learn(data, 300, 0);

    fann_destroy_train(data);
    nn.save("OptimizeNNTroughGradientOfAnotherNNFannMinDerivative.cache.data");
  } else
    nn.load("OptimizeNNTroughGradientOfAnotherNNFannMinDerivative.cache.data");

  datann_derivative d = {&nn, 1, 1};

  for(int i=0;i<1000; i++){
     data = fann_create_train(400, 1, 1);
//      uint n = 0;
//      auto iter2 = [&](const std::vector<double>& x) {
//         data->input[n][0]= x[0];
//         data->output[n][0]= x[0];//don't care
//        n++;
//      };
//      bib::Combinaison::continuous<>(iter2, 1, -1, 1, 200);
     for (uint n = 0; n < 400 ; n++){
       data->input[n][0]= bib::Utils::rand01()*2-1.;
       data->output[n][0]= 0;
     }

     fann_train_epoch_irpropm_gradient(actor.getNeuralNet(), data, derivative_nn, &d);

     fann_train_epoch_irpropm_gradient(actor2.getNeuralNet(), data, derivative_nn_inverting, &d);
     if(i%100 == 0)
      LOG_DEBUG(actor.weight_l1_norm() << " " << actor2.weight_l1_norm());

     fann_destroy_train(data);
  }

  // Test
  double ac1_error = 0;
  double ac2_error = 0;
  for (uint n = 0; n < 100 ; n++) {
    double x1 = bib::Utils::rand01()*2 -1;

    double out = -1.f; //not about derivative but constraints in [-1; 1]
    double qout = out*out-(x1*x1)*out;

    std::vector<double> sens(1);
    sens[0] = x1;
    std::vector<double> ac(1);
    std::vector<double> ac_empty(0);
    ac[0]= out;

    EXPECT_GT(nn.computeOutVF(sens, ac), qout - 0.25);
    EXPECT_LT(nn.computeOutVF(sens, ac), qout + 0.25);

    EXPECT_GT(actor.computeOutVF(sens, ac_empty), out - 0.2);
    EXPECT_LT(actor.computeOutVF(sens, ac_empty), out + 0.2);

    EXPECT_GT(actor2.computeOutVF(sens, ac_empty), out - 0.2);
    EXPECT_LT(actor2.computeOutVF(sens, ac_empty), out + 0.2);

    LOG_FILE("OptimizeNNTroughGradientOfAnotherNNFannMinDerivative.data", x1 << " " << actor.computeOutVF(sens, ac_empty) << " " <<
                                                              actor2.computeOutVF(sens, ac_empty)<< " " << out);
    //clear all; close all; X=load('OptimizeNNTroughGradientOfAnotherNNFannMinDerivative.data'); plot(X(:,1),X(:,2), '.'); hold on; plot(X(:,1),X(:,3), 'r.');plot(X(:,1),X(:,4), 'go');
    ac1_error += fabs(out - actor.computeOutVF(sens, ac_empty));
    ac2_error += fabs(out - actor2.computeOutVF(sens, ac_empty));
  }

  ac1_error /= 100;
  ac2_error /= 100;

  LOG_DEBUG(ac1_error << " " << ac2_error);
}


/*
 * InnerMapTask.cpp
 *
 *  Created on: 25 Jul 2018
 *      Author: scsjd
 */

#include <fstream>
#include <sstream>
#include <iterator>
#include <chrono>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/vec_ZZ.h>
#include <jsoncpp/json/json.h>
#include "InnerMapTask.h"
#include "HE1Array.h"

InnerMapTask::InnerMapTask(int numPerLine, const char* parametersPath, const char* inputPath){
	this->inputPath = inputPath;
	this->numPerLine = numPerLine;
	parseParameters(parametersPath);
	totalSumTime=0;
	numberAdditions=0;
	totalProductTime=0;
	numberMultiplications=0;
	HE1Array::create_device_handle();
	HE1Array::create_device_modulus(modulus);
}

InnerMapTask::~InnerMapTask(){
	HE1Array::delete_device_modulus();
	HE1Array::delete_device_handle();
}

void InnerMapTask::parseParameters(const char* parametersPath){
	NTL::ZZ mod;
	std::ifstream ifs(parametersPath);
	if (ifs.is_open()){
		std::string json;
		getline(ifs,json);
		Json::Value root;   // will contains the root value after parsing.
		Json::Reader reader;
		bool parsingSuccessful = reader.parse(json,root);
		if (parsingSuccessful){
			modulus = NTL::conv<NTL::ZZ>(root["modulus"].asCString());
		}
	}
}

NTL::ZZ InnerMapTask::run(){
	std::ifstream ifs(inputPath);
	if (!ifs.is_open()){
		throw std::ios_base::failure("Could not open input file.");
	}
	std::string line;

	NTL::vec_ZZ v[numPerLine];

	while(getline(ifs,line)){
		std::istringstream iss(line);
		std::vector<std::string> words((std::istream_iterator<std::string>(iss)),std::istream_iterator<std::string>());
		for(int i = 0; i < words.size(); i++){
			NTL::ZZ z = NTL::conv<NTL::ZZ>(words[i].c_str());
			v[i].append(z);
		}
	}
	ifs.close();

	//Do multiplication on device
	HE1Array prod(v[0]);
	numberMultiplications = v[0].length();
	for (int i=1; i < numPerLine; i++){
		HE1Array bn(v[i]);
		auto start = std::chrono::high_resolution_clock::now();
		prod*=bn;
		auto finish = std::chrono::high_resolution_clock::now();
		totalProductTime += std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
	}

	//Do prefix sum reduce on CPU because array reduction not supported by xmp
	NTL::vec_ZZ products = prod.to_ZZ_vector();
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p result;
	for (int i = 0; i < products.length(); i++){
		auto start = std::chrono::high_resolution_clock::now();
		result += NTL::conv<NTL::ZZ_p>(products.at(i));
		auto finish = std::chrono::high_resolution_clock::now();
		totalSumTime += std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
		numberAdditions++;
	}

	return NTL::rep(result);
};


#include "SparseSolverEigenLLT.h"
#include <Eigen/SparseCholesky>

std::vector<double> SparseSolverEigenLLT::solve_Ax_b(SparseMatrix<double> A, SparseVector<double> b, int numel, std::vector<int> & uidx, const std::vector<int> * labels, const std::vector<int> * seeds, int active_label)
{
	SimplicialLLT<SparseMatrix<double> > solver;

	// Convert to dense format for solver compatibility - this might become a memory issue
	VectorXd b_dense = b;

	// LLT Decomposition
	solver.compute(A);

	/*if(solver.info()!=true) {
	  std::cout << "Decomposition failed!\n";
	}*/

	// Solve system with decomposition
	VectorXd x_dense = solver.solve(b_dense);

	/*if(solver.info()!=true) {
		std::cout << "Solver failed!\n";
	}*/

	// solve for another right hand side - good feature for multi-label setup
	//x1 = solver.solve(b1);

	// Holder for saving to external format
	std::vector<double> xmat(numel);

	for (int i=0; i<x_dense.rows(); i++)
	{
		double val = x_dense(i);
		xmat[uidx[i]] = val;
	}

	for (int i=0; i<seeds->size(); i++)
	{
		if((*labels)[i] == active_label)
			xmat[(*seeds)[i]] = 1.0;
		else
			xmat[(*seeds)[i]] = 0.0;
	}

	return xmat;
}


/* aHAFF.cpp
   g++ -I/usr/local/include/eigen3 -o aHAF aHAF.cpp -O3
*/

#include <iostream>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;

int *pl = NULL;
int *rl = NULL;
unsigned long int rsn;
double *phi = NULL;
double *phiOld = NULL;
double *phiOlder = NULL;
double *Hphi = NULL;
double *A;
double *B;
MatrixXd Lanczos;
SelfAdjointEigenSolver<MatrixXd> es;
int L,Sztot;
int maxlen;
double g, oldnorm, norm;
ofstream ofs,ofs2;

void initialize(int);
void exit();
int Sz(unsigned long int);
int spin(unsigned long int, int);
int INV(unsigned long int);
unsigned long int flip(unsigned long int,int);
bool flippable(unsigned long int,int);

void initialize(int opt) {
  A = new double[200];
  B = new double[200];
  pl = new int [L];           // watch zero start !
  for (int i=0;i<L;i++) {
    pl[i] = i+1;
  }
  pl[L-1] = 0;
  // allocate memory
  unsigned long int size=1;   // L!/(L/2)!(L/2)!
  int L2 = L/2;               // L must be even
  int j=1;
  for (int i=L2+1;i<=L;i++) {
    size = size*i/j;
    j += 1;
  }
  cout << "     " <<  endl;
  double d;
  cout << "double: " << sizeof(d) << endl;
  cout << "int: " << sizeof(j) << endl;
  cout << "ulint: " << sizeof(size) << endl;
  cout << "basis size for Sztot=0: " << size << endl;
  if (!(phi  = new double[size+1] )) {   // rsn starts from 1 !!
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(phiOld  = new double[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(phiOlder  = new double[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(Hphi  = new double[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  if (!(rl = new int[size+1] )) {
   cout << "out of memory" << endl;
   exit(1);
  }
  // set up relabeled state codes
  unsigned long int maxnum = 1 << L;   // a dirty trick to get 2^L (cpp does not support pow(int,int)!!
  cout << " 2^L = " << maxnum << endl;
  rsn = 1;
  for (unsigned long int sn=0;sn<maxnum;sn++) {
    if (Sz(sn) == Sztot) {
      rl[rsn] = sn;
      rsn++;
    }
  }
  maxlen = rsn - 1;
  cout << " basis size for Sztot= " << Sztot << " : " << maxlen << endl;
  // initial state
  if (opt == 1){                      // initial state = neel1 + neel2
    int neel1 = maxnum/3;
    int neel2 = 2*neel1;
    for (rsn=1;rsn<=maxlen;rsn++) {
       phi[rsn] = 0.0;
    }
    phi[INV(neel1)] = 1.0/sqrt(2.0);
    phi[INV(neel2)] = 1.0/sqrt(2.0);
    oldnorm = 1.0;
    if (L == 4) {
      cout << "neel1 " << neel1 << " inv " << INV(neel1) << endl;
      cout << "neel2 " << neel2 << " inv " << INV(neel2) << endl;
    }
  } else {                          // random start
    int seed;
    cout << " enter the random seed " << endl;
    cin >> seed;
    srand(seed);
    oldnorm = 0.0;
    for (rsn=1;rsn<=maxlen;rsn++) {
     phi[rsn] = 2.0*( (double) rand()/RAND_MAX - 0.5);
     oldnorm = oldnorm + phi[rsn]*phi[rsn];
    }
  }
  // file miscellany
  ofs2.open("aHAFab.dat");
  ofs.open("aHAFout.dat");
  ofs << "# aHAF.cpp" << endl;
  ofs << "# L = " << L <<  endl;
  ofs << "# g = " << g << endl;
  ofs << "# Sztot = " << Sztot << endl;
  if (opt == 1) {
   ofs << "# Neel start " << endl;
  } else {
   ofs << "# random start " << endl;
  }
}

void exit() {
  delete [] phi;
  delete [] phiOld;
  delete [] phiOlder;
  delete [] Hphi;
  delete [] rl;
  delete [] pl;
  delete [] A;
  delete [] B;
  ofs.close();
  ofs2.close();
}

int INV(unsigned long int sn) {
  // find the state index, rsn, for a state, sn=rl(rsn)
  unsigned long int mid;
  unsigned long int low = 1;
  unsigned long int high = maxlen;
  st:  mid = (low + high)/2;
  if (sn ==  rl[mid]) return mid;
  if (sn <  rl[mid]) {
    high = mid - 1;
  } else {
    low = mid + 1;
  }
  goto st;
}

int spin(unsigned long int sn, int bt) {
  // return the bt'th bit in sn converted to +-
  int S = -1;
  int k = sn & 1 << bt;
  if (k != 0) S=1;
  return S;
}

int Sz(unsigned long int sn) {
 // compute Sztot for a state sn
 int S=0;
 for (int b=0;b<L;b++) {
  S += spin(sn,b);
 }
 return S;
}

int T(unsigned long int rsn) {
  // compute H0 for a state rsn  (need to divide by 4)
  int S=0;
  unsigned long int sn = rl[rsn];
  for (int bt=0;bt<L;bt++) {
    S += spin(sn,bt)*spin(sn,pl[bt]);
  }
  return S;
}

unsigned long int flip(unsigned long int rsn, int i) {
 // flip bits i and i+1  using a nasty trick; return a relabeled statecode
 // will fail drastically if b(i) = b(i+1)
 unsigned long int sn = rl[rsn];
 sn ^= 1 << i;
 sn ^= 1 << pl[i];
 return INV(sn);
}

bool flippable(unsigned long int rsn, int bt) {
 // check if bit i != bit i+1
 bool f = false;
 int b0 = (rl[rsn] >> bt) & 1;
 int b1 = (rl[rsn] >> pl[bt]) & 1;
 if (b0 != b1) f = true;
 return f;
}

int main() {
  int opt;
  cout << " enter L, Sztot, g " << endl;
  cin >> L >> Sztot >> g;
  cout << " enter 1 for Neel start, 2 for random " << endl;
  cin >> opt;

  initialize(opt);

  // H{phi}
  for (rsn=1;rsn<=maxlen;rsn++) {
   Hphi[rsn] = T(rsn)*phi[rsn]/4.0;
  }
  for (rsn=1;rsn<=maxlen;rsn++) {
   for (int bt=0;bt<L;bt++) {
    if (flippable(rsn,bt)) {
     Hphi[flip(rsn,bt)] += g/2.0*phi[rsn];
    }
   }
  }

  double sum = 0.0;
  for (rsn=1;rsn<=maxlen;rsn++) {
    sum += phi[rsn]*Hphi[rsn];
    phiOld[rsn] = phi[rsn];
  }
  A[0] = sum/oldnorm;
  B[0] = 0.0;
  cout << " variational  e0 = " << A[0]/L << endl;
  ofs << "0 A: " << A[0] << endl;

  for (int loop=1;loop<=4*L;loop++) {    // Lanzcos loop  :::::::::::::::::::::::::::
   for (rsn=1;rsn<=maxlen;rsn++) {
     phi[rsn] = Hphi[rsn] - A[loop-1]*phiOld[rsn] - B[loop-1]*phiOlder[rsn];
   }
   // H{phi}
   for (rsn=1;rsn<=maxlen;rsn++) {
    Hphi[rsn] = T(rsn)*phi[rsn]/4.0;
   }
   for (rsn=1;rsn<=maxlen;rsn++) {
    for (int bt=0;bt<L;bt++) {
     if (flippable(rsn,bt)) {
      Hphi[flip(rsn,bt)] += g/2.0*phi[rsn];
     }
    }
   }
   double norm = 0.0;
   for (rsn=1;rsn<=maxlen;rsn++) {
     norm += phi[rsn]*phi[rsn];
   }
   double sum = 0.0;
   for (rsn=1;rsn<=maxlen;rsn++) {
     sum += phi[rsn]*Hphi[rsn];
   }
   A[loop] = sum/norm;
   B[loop] = norm/oldnorm;
   oldnorm = norm;
   for (rsn=1;rsn<=maxlen;rsn++) {
     phiOlder[rsn] = phiOld[rsn];
     phiOld[rsn] = phi[rsn] ;
   }

   ofs2 << loop << setprecision(16) << std::fixed << " A: " << A[loop] << " B: " << B[loop] << endl;
   // diagonalise
   Lanczos.resize(loop+1,loop+1);
   Lanczos.setZero();
   for (int i=0;i<=loop;i++) {
     Lanczos(i,i) = A[i];
   }
   for (int i=0;i<loop;i++) {
     Lanczos(i,i+1) = sqrt(B[i+1]);
     Lanczos(i+1,i) = Lanczos(i,i+1);
   }
   es.compute(Lanczos);
   double lam0 = es.eigenvalues()[0];
   double lam1 = es.eigenvalues()[1];
   ofs << loop << setprecision(12) << std::fixed << " " << lam0 << " " << lam1;
   if (loop > 3 ) {
    double lam2 = es.eigenvalues()[2];
    double lam3 = es.eigenvalues()[3];
    ofs << " " << lam2 << " " << lam3;
   }
   ofs << endl;
   cout << loop << setprecision(12) << std::fixed <<  "\t e0: " << lam0/L << "\t e1: " << lam1/L << "\t gap: " << lam1-lam0 << endl;
  }  // end of Lanczos iteration loop           ::::::::::::::::::::::::::::::

  exit();
  cout << endl << " data in aHAFout.dat" << endl;
  cout << " A,B in aHAFab.dat" << endl << endl;
  return 0;
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(plane)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;
  typedef MT::real_type          T;

  {
    V const p0 = V::make(1,0,2);
    V const p1 = V::make(1,1,2);
    V const p2 = V::make(0,1,2);

    geometry::Plane<V> P = geometry::make_plane( p0, p1, p2);

    BOOST_CHECK_CLOSE( P.n()(0), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.n()(1), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.n()(2), 1.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.w(),    2.0f, 0.01f );

    V const q0  = V::make(0,0,4);
    V const q1  = V::make(0,0,0);

    T const d0 = geometry::get_signed_distance(q0, P);
    BOOST_CHECK_CLOSE( d0,  2.0f, 0.01f );

    T const d1 = geometry::get_signed_distance(q1, P);
    BOOST_CHECK_CLOSE( d1,  -2.0f, 0.01f );

    T const a0 = geometry::get_distance(q0, P);
    BOOST_CHECK_CLOSE( a0,  2.0f, 0.01f );

    T const a1 = geometry::get_distance(q1, P);
    BOOST_CHECK_CLOSE( a1,  2.0f, 0.01f );
  }
  {
    V const normal = V::make(0,0,1);
    T const offset = 2.0;

    geometry::Plane<V> P = geometry::make_plane( normal, offset);

    BOOST_CHECK_CLOSE( P.n()(0), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.n()(1), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.n()(2), 1.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.w(),    2.0f, 0.01f );
  }
  {
    V const p0 = V::make(1,0,2);
    V const p1 = V::make(1,1,2);
    V const p2 = V::make(0,1,2);

    geometry::Triangle<V> triangle = geometry::make_triangle(p0, p1, p2);

    geometry::Plane<V> P = geometry::make_plane( triangle );

    BOOST_CHECK_CLOSE( P.n()(0), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.n()(1), 0.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.n()(2), 1.0f, 0.01f );
    BOOST_CHECK_CLOSE( P.w(),    2.0f, 0.01f );
  }

}

BOOST_AUTO_TEST_SUITE_END();


//
// Created by egrzrbr on 2019-04-26.
//

#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

#include <boost/numeric/ublas/matrix.hpp>
using namespace boost::numeric::ublas;

/**********************************
 * Pseudo-random number generator *
 **********************************/

static uint64_t mat_rng[2] = { 11ULL, 1181783497276652981ULL };

static inline uint64_t xorshift128plus(uint64_t s[2])
{
	uint64_t x, y;
	x = s[0], y = s[1];
	s[0] = y;
	x ^= x << 23;
	s[1] = x ^ y ^ (x >> 17) ^ (y >> 26);
	y += s[1];
	return y;
}

double mat_drand(void)
{
	return (xorshift128plus(mat_rng)>>11) * (1.0/9007199254740992.0);
}

void mat_gen_random_ublas(matrix<float> &m)
{
	size_t i, j;
	for (i = 0; i < m.size1(); ++i)
		for (j = 0; j < m.size2(); ++j)
			m(i, j) = mat_drand();
}

/*****************
 * Main function *
 *****************/

#include <unistd.h>
#include <time.h>

int main(int argc, char *argv[])
{
	int c, n = 1000;
	clock_t t;

	while ((c = getopt(argc, argv, "n:h")) >= 0) {
		if (c == 'n') n = atoi(optarg);
		else if (c == 'h') {
			fprintf(stderr, "Usage: mat-eval [options]\n");
			fprintf(stderr, "Options:\n");
			fprintf(stderr, "  -n INT    size of the square matrix [%d]\n", n);
			fprintf(stderr, "  -h        this help message\n");
			return 1;
		}
	}

	matrix<float> a(n, n), b(n, n), m(n, n);
	mat_gen_random_ublas(a);
	mat_gen_random_ublas(b);

	t = clock();
	m = prod(a, b);
	fprintf(stderr, "CPU time: %g\n", (double)(clock() - t) / CLOCKS_PER_SEC);
	fprintf(stderr, "Central cell: %g\n", m(n/2, n/2));

	return 0;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <cstdint>
#include <vector>
using gint = std::int64_t;
using Coordinate = std::complex<gint>;
enum Command {
  TurnRight,
  TurnLeft
};
class Car
{
  std::vector<std::pair<std::complex<gint>, std::complex<gint>>> v_;
  std::complex<gint> pos_{ 0, 0 };
  std::complex<gint> direction_{ 1, 0 };
  std::complex<gint> turn_[2]{
    { 0, -1 },
    { 0, 1 }
  };
  Coordinate interPoint_{ 0, 0 };

public:
  constexpr gint distance() const noexcept
  {
    return std::abs(interPoint_.real()) + std::abs(interPoint_.imag());
  }
  bool inter(std::pair<Coordinate, Coordinate> l, std::pair<Coordinate, Coordinate> r)
  {
    auto linestart = l.first;
    if (l.first.real() > l.second.real()) {
      std::swap(l.first, l.second);
    } else if (l.first.imag() > l.second.imag()) {
      std::swap(l.first, l.second);
    }
    if (r.first.real() > r.second.real()) {
      std::swap(r.first, r.second);
    } else if (r.first.imag() > r.second.imag()) {
      std::swap(r.first, r.second);
    }
    auto between = [](auto f, auto l, auto m) {
      return (f <= m) && (m <= l);
    };
    if (l.first.imag() == l.second.imag() && r.first.real() == r.second.real() && between(l.first.real(), l.second.real(), r.first.real()) && between(r.first.imag(), r.second.imag(), l.first.imag())) {
      interPoint_ = Coordinate(r.first.real(), l.first.imag());
      if (interPoint_ != linestart) {
        return true;
      }
    } else {
      swap(l, r);
      if (l.first.imag() == l.second.imag() && r.first.real() == r.second.real() && between(l.first.real(), l.second.real(), r.first.real()) && between(r.first.imag(), r.second.imag(), l.first.imag())) {
        interPoint_ = Coordinate(r.first.real(), l.first.imag());
        if (interPoint_ != linestart) {
          return true;
        }
      }
    }
    return false;
  }
  bool findinter(std::pair<Coordinate, Coordinate> l)
  {
    for (auto r : v_) {
      if (inter(l, r)) {

        return true;
      }
    }
    return false;
  }

  bool process(Command command, gint value) noexcept
  {
    direction_ *= turn_[command];

    auto newpos = pos_ + direction_ * value;

    auto road = std::make_pair(pos_, newpos);
    if (findinter(road)) {
      return true;
    } else {
      v_.push_back(std::make_pair(pos_, newpos));
      pos_ = newpos;
      return false;
    }
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    char c;
    int forward;
    Car car;
    Command com[256];
    com['L'] = Command::TurnLeft;
    com['R'] = Command::TurnRight;
    while (ifs >> c >> forward) {
      auto r = car.process(com[c], forward);
      if (r) {
        fmt::print("d:{}\n", car.distance());
        break;
      }
    }
  }
}


/*  calculate CMB related observables, depends on Cosmology calculator(s).

    Shift parameter: R = (1+z*) * DA(z�6�5) * sqrt(Omegam) * H0 / c
    acoustic scale: lA = (1+z*) * Pi * DA(z*) / rs(z*)

where z* is the decoupling redshift
*/

#ifndef __CMB_DISTANCE_PRIOR__
#define __CMB_DISTANCE_PRIOR__

#include <vector>
#include <string>
#include <armadillo>
#include <imcmc/imcmc.hpp>
#include "ParamList.hpp"

using namespace imcmc;

struct Data_CMB_Dist {

    DataInfo data_info;
    bool use_Hu_fitting;
    int format;                     // 0--WMAP; 1--Planck
    arma::rowvec distance_prior;    // {lA,R,zdec}
    arma::mat covmat_inv;           // inverse of the covariance matrix.
    void Init( std::string& CMB_dist_prior_dataset );

    Data_CMB_Dist();
    ~Data_CMB_Dist();

//  add Planck CMB distance prior
    arma::rowvec distance_prior_plk;    // {R,lA,Omegabh2}
    arma::mat covmat_inv_plk;           // this (inv-)covariance matrix is different from those from WMAP
    double std_R, std_lA, std_Obh2;     // 1-sigma errors for {R,lA,Omegabh2}
};

typedef Data_CMB_Dist CMB_Dist;

//  likelihood function prototype:
double Prior_CMB_Dist(  imcmc_double&   param,
                        double&         lndet,
                        double&         chisq,
                        void*           model,
                        void*           data,
                        istate&         state );

#endif  //  __CMB_DISTANCE_PRIOR__


/*
 * Metric.hpp
 *
 *  Created on: May 18, 2012
 *      Author: david
 */

#ifndef DASP_METRIC_HPP_
#define DASP_METRIC_HPP_

#include "Point.hpp"
#include <Danvil/Tools/MoreMath.h>
#include <Danvil/Tools/FunctionCache.h>
#include <Eigen/Dense>

namespace dasp
{

	namespace metric
	{
		inline int PixelDistanceSquared(const Point& a, const Point& b) {
			const int dx = a.px - b.px;
			const int dy = a.py - b.py;
			return dx*dx + dy*dy;
		}

		inline float ImageDistanceRaw(const Point& x, const Point& y) {
			return static_cast<float>(PixelDistanceSquared(x,y)) / (y.cluster_radius_px * y.cluster_radius_px);
		}

		inline float SpatialDistanceRaw(const Eigen::Vector3f& x, const Eigen::Vector3f& y) {
			return (x - y).squaredNorm();
		}

		inline float SpatialDistanceRaw(const Point& x, const Point& y) {
			return SpatialDistanceRaw(x.position, y.position);
		}

		inline float ColorDistanceRaw(const Eigen::Vector3f& u, const Eigen::Vector3f& v) {
			return (u - v).squaredNorm();
		}

		inline float ColorDistanceRaw(const Point& u, const Point& v) {
			return ColorDistanceRaw(u.color, v.color);
		}

		inline float NormalDistanceRaw(const Eigen::Vector3f& u, const Eigen::Vector3f& v) {
			// this is an approximation to the angle between the normals
			return 1.0f - u.dot(v);
		}

		inline float NormalDistanceRaw(const Point& u, const Point& v) {
			return NormalDistanceRaw(u.normal, v.normal);
		}

		inline float NormalDistanceWithDepth(const Point& u, const Point& v) {
			const float q = NormalDistanceRaw(u.normal, v.normal);
			return 2.0f * q / (u.position.z() + v.position.z());
		}

	}

	/** Computes the density-adaptive distance from a point to a center point
	 * - uses pixel distance and color metric
	 * - Takes the density at the center point
	 */
	struct DensityAdaptiveMetric_UxRGB
	{
		DensityAdaptiveMetric_UxRGB(float w_u, float w_c) {
			weights_ = {
				w_u,
				w_c };
		}

		float operator()(const Point& p, const Point& q) const {
			return weights_.dot(
					Eigen::Vector2f(
							metric::ImageDistanceRaw(p, q),
							metric::ColorDistanceRaw(p, q)));
		}

	private:
		Eigen::Vector2f weights_;
	};

	/** Computes the density-adaptive distance from a point to a center point
	 * - uses pixel distance, color metric and depth difference
	 * - Takes the density at the center point
	 */
	struct DensityAdaptiveMetric_UxRGBxD
	{
		DensityAdaptiveMetric_UxRGBxD(float w_u, float w_c, float w_d) {
			weights_ = {
				w_u,
				w_c,
				w_d };
		}

		float operator()(const Point& p, const Point& q) const {
			return weights_.dot(
					Eigen::Vector3f(
							metric::ImageDistanceRaw(p, q),
							metric::ColorDistanceRaw(p, q),
							std::abs(p.depth() - q.depth())));
		}

	private:
		Eigen::Vector3f weights_;
	};

	/** Computes the depth-adaptive distance from a point to a center point
	 * - Takes the density at the center point
	 * - Uses 3D position instead of pixel position
	 * - Additionally uses normals.
	 */
	struct DepthAdaptiveMetric
	{
		DepthAdaptiveMetric(float w_r, float w_c, float w_n, float R) {
			weights_ = {
				w_r / (R * R),
				w_c,
				w_n };
		}

		float operator()(const Point& p, const Point& q) const {
			return weights_.dot(
					Eigen::Vector3f(
							metric::SpatialDistanceRaw(p, q),
							metric::ColorDistanceRaw(p, q),
							metric::NormalDistanceWithDepth(p, q)));
		}

	private:
		Eigen::Vector3f weights_;
	};

	template<bool SupressConvexEdges=true>
	struct ClassicSpectralAffinity
	{
		ClassicSpectralAffinity(unsigned int num_superpixels, float superpixel_radius, float w_spatial=1.0f, float w_color=1.0f, float w_normal=1.0f)
		: num_superpixels_(num_superpixels),
		  superpixel_radius_(superpixel_radius)
		{
			scl_spatial_ = w_spatial / (4.0f * superpixel_radius_ * superpixel_radius_);
			scl_color_ = w_color / (std::sqrt(static_cast<float>(num_superpixels_)) * cWeightRho);
			scl_normal_ = w_normal;
		}

		float operator()(const Point& x, const Point& y) const {
			const Eigen::Vector3f& x_pos = x.position;
			const Eigen::Vector3f& y_pos = y.position;
			const Eigen::Vector3f& x_col = x.color;
			const Eigen::Vector3f& y_col = y.color;
			const Eigen::Vector3f& x_norm = x.normal;
			const Eigen::Vector3f& y_norm = y.normal;
			// spatial distance
			float scl_d_spatial = metric::SpatialDistanceRaw(x_pos, y_pos) * scl_spatial_;
			scl_d_spatial = std::max(0.0f, scl_d_spatial - 1.2f); // distance of 1 indicates estimated distance
			// color distance
			float d_color = metric::ColorDistanceRaw(x_col, y_col);
			// normal distance
			float d_normal;
			if(SupressConvexEdges) {
				// only use concave edges
				Eigen::Vector3f d = y_pos - x_pos;
				d_normal = (x_norm - y_norm).dot(d) * Danvil::MoreMath::FastInverseSqrt(d.squaredNorm());
				d_normal = std::max(0.0f, d_normal);
			}
			else {
				d_normal = metric::NormalDistanceRaw(x_norm, y_norm);
			}
			// compute total edge connectivity
			float d_combined = scl_d_spatial + scl_color_*d_color + scl_normal_*d_normal;
			return exp_cache_(d_combined);
		}

	private:
		static constexpr float cWeightRho = 0.01f; // 640x480 clusters would yield 0.1 which is used in gPb
		unsigned int num_superpixels_;
		float superpixel_radius_;
		float scl_spatial_;
		float scl_color_;
		float scl_normal_;
		Danvil::ExpNegFunctionCache<float> exp_cache_; // used for std::exp(-x)
	};

	struct ImprovedSpectralAffinity
	{
		ImprovedSpectralAffinity(float superpixel_radius,
			float w_spatial=1.0f, float w_color=1.0f, float w_normal=1.0f)
		: superpixel_radius_(superpixel_radius),
		  ww_(w_spatial), wc_(w_color), wn_(w_normal)
		{
			float w_total = ww_ + wc_ + wn_;
			ww_ /= w_total;
			wc_ /= w_total;
			wn_ /= w_total;
		}

		float operator()(const Cluster& x, const Cluster& y) const {
			const Eigen::Vector3f& x_pos = x.center.position;
			const Eigen::Vector3f& y_pos = y.center.position;
			const Eigen::Vector3f& x_col = x.center.color;
			const Eigen::Vector3f& y_col = y.center.color;
			const Eigen::Vector3f& x_norm = x.center.normal;
			const Eigen::Vector3f& y_norm = y.center.normal;
			// spatial distance
			float dw = (x_pos - y_pos).squaredNorm() / (4.0f * superpixel_radius_ * superpixel_radius_);
			dw = std::max(0.0f, dw - 1.2f); // distance of 1 indicates estimated distance
			// color distance
			float dc = 3.2f * (x_col - y_col).squaredNorm();
			// normal distance (only use concave edges)
			Eigen::Vector3f u = y_pos - x_pos;
			float dn = (x_norm - y_norm).dot(u) / u.norm();
			dn = std::max(0.0f, dn);
			// compute total edge connectivity
			float d = ww_*dw + wc_*dc + wn_*dn;
			return exp_cache_(d);
		}

	private:
		float superpixel_radius_;
		float ww_, wc_, wn_;
		Danvil::ExpNegFunctionCache<float> exp_cache_; // used for std::exp(-x)
	};

	struct ClassicSpectralAffinitySLIC
	{
		ClassicSpectralAffinitySLIC(unsigned int num_superpixels, float w_color=1.0f)
		: num_superpixels_(num_superpixels),
		  w_color(w_color)
		{}

		float operator()(const Point& x, const Point& y) const {
			float w_maha_color = 4.0f * metric::ColorDistanceRaw(x, y) / (std::sqrt(static_cast<float>(num_superpixels_)) * cWeightRho);
			return std::exp(-w_color*w_maha_color);
		}

	private:
		static constexpr float cWeightRho = 0.01f; // 640x480 clusters would yield 0.1 which is used in gPb
		unsigned int num_superpixels_;
		float w_color;
	};

}

#endif


//==================================================================================================
/*!
  @file
  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_GAMMA_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_GAMMA_HPP_INCLUDED

#include <boost/simd/function/std.hpp>
#include <boost/config.hpp>
#include <boost/simd/arch/common/detail/generic/gamma_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/copysign.hpp>
#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/is_even.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/simd/function/scalar/sinpi.hpp>
#include <boost/simd/function/scalar/stirling.hpp>

#include <boost/dispatch/function/overload.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  using bs::std_tag;
  BOOST_DISPATCH_OVERLOAD ( gamma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      if (is_eqz(a0)) return copysign(Inf<A0>(), a0);
      #ifndef BOOST_SIMD_NO_INVALIDS
      if( is_nan(a0) || (a0 == Minf<A0>()) ) return Nan<A0>();
      if (a0 == Inf<A0>()) return a0;
      #endif

      A0 x = a0;
      A0 q = bs::abs(x);
      if(x < A0(-33.0))
      {
        return std::tgamma(a0);
        A0 st = stirling(q);
        A0 p =  floor(q);
        auto iseven =  is_even((int32_t)p);
        if (p == q) return Nan<A0>();
        A0 z = q - p;
        if( z > Half<A0>() )
        {
          p += One<A0>();
          z = q - p;
        }
        z = q*sinpi(z);
        if( is_eqz(z) ) return Nan<A0>();
        st = Pi<A0>()/(bs::abs(z)*st);
        return iseven  ? -st : st;
      }
      A0 z = One<A0>();
      while( x >= Three<A0>() )
      {
        x -= One<A0>();
        z *= x;
      }
      while( is_ltz(x) )
      {
        z /= x;
        x += One<A0>();
      }
      while( x < Two<A0>() )
      {
        if( is_eqz(x)) return Nan<A0>();
        z /= x;
        x +=  One<A0>();
      }
      if( x == Two<A0>() ) return(z);
      x -= Two<A0>();
      return z*detail::gamma_kernel<A0>::gamma1(x);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( gamma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bs::std_tag
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::tgamma(a0);
    }
  };
} } }

#endif


//
//  GridRaycast.hpp
//  PinkTopaz
//
//  Created by Andrew Fox on 11/18/17.
//
//

#ifndef GridRaycast_hpp
#define GridRaycast_hpp

#include "Ray.hpp"
#include <vector>
#include <glm/vec3.hpp>
#include <boost/coroutine2/all.hpp>

namespace GridRaycastRange {

// Iterate over cells which fall within the specified frustum.
inline void
raycast(boost::coroutines2::coroutine<glm::vec3>::push_type &sink,
        const Ray &ray,
        size_t maxDepth)
{
    /* Implementation is based on:
     * "A Fast Voxel Traversal Algorithm for Ray Tracing"
     * John Amanatides, Andrew Woo
     * http://www.cse.yorku.ca/~amana/research/grid.pdf
     *
     * See also: http://www.xnawiki.com/index.php?title=Voxel_traversal
     */

    // NOTES:
    // * This code assumes that the ray's position and direction are in 'cell coordinates', which means
    //   that one unit equals one cell in all directions.
    // * When the ray doesn't start within the voxel grid, calculate the first position at which the
    //   ray could enter the grid. If it never enters the grid, there is nothing more to do here.
    // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
    // * The Point3D structure is a simple structure having three integer fields (X, Y and Z).

    // The cell in which the ray starts.
    int x = (int)ray.origin.x;
    int y = (int)ray.origin.y;
    int z = (int)ray.origin.z;

    // Determine which way we go.
    int stepX = (ray.direction.x<0) ? -1 : (ray.direction.x==0) ? 0 : +1;
    int stepY = (ray.direction.y<0) ? -1 : (ray.direction.y==0) ? 0 : +1;
    int stepZ = (ray.direction.z<0) ? -1 : (ray.direction.z==0) ? 0 : +1;

    // Calculate cell boundaries. When the step (i.e. direction sign) is positive,
    // the next boundary is AFTER our current position, meaning that we have to add 1.
    // Otherwise, it is BEFORE our current position, in which case we add nothing.
    glm::ivec3 cellBoundary(x + (stepX > 0 ? 1 : 0),
                            y + (stepY > 0 ? 1 : 0),
                            z + (stepZ > 0 ? 1 : 0));

    // NOTE: For the following calculations, the result will be Single.PositiveInfinity
    // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
    // value of the division also equals zero, the result is Single.NaN, which is not OK.

    // Determine how far we can travel along the ray before we hit a voxel boundary.
    glm::vec3 tMax((cellBoundary.x - ray.origin.x) / ray.direction.x,    // Boundary is a plane on the YZ axis.
                   (cellBoundary.y - ray.origin.y) / ray.direction.y,    // Boundary is a plane on the XZ axis.
                   (cellBoundary.z - ray.origin.z) / ray.direction.z);   // Boundary is a plane on the XY axis.
    if (isnan(tMax.x)) { tMax.x = +INFINITY; }
    if (isnan(tMax.y)) { tMax.y = +INFINITY; }
    if (isnan(tMax.z)) { tMax.z = +INFINITY; }

    // Determine how far we must travel along the ray before we have crossed a gridcell.
    glm::vec3 tDelta(stepX / ray.direction.x,                    // Crossing the width of a cell.
                     stepY / ray.direction.y,                    // Crossing the height of a cell.
                     stepZ / ray.direction.z);                   // Crossing the depth of a cell.
    if (isnan(tDelta.x)) { tDelta.x = +INFINITY; }
    if (isnan(tDelta.y)) { tDelta.y = +INFINITY; }
    if (isnan(tDelta.z)) { tDelta.z = +INFINITY; }

    // For each step, determine which distance to the next voxel boundary is lowest (i.e.
    // which voxel boundary is nearest) and walk that way.
    for (size_t i = 0; i < maxDepth; ++i) {
        sink(glm::vec3(x, y, z));

        // Do the next step.
        if (tMax.x < tMax.y && tMax.x < tMax.z) {
            // tMax.X is the lowest, an YZ cell boundary plane is nearest.
            x += stepX;
            tMax.x += tDelta.x;
        } else if (tMax.y < tMax.z) {
            // tMax.Y is the lowest, an XZ cell boundary plane is nearest.
            y += stepY;
            tMax.y += tDelta.y;
        } else {
            // tMax.Z is the lowest, an XY cell boundary plane is nearest.
            z += stepZ;
            tMax.z += tDelta.z;
        }
    }
}

} // namespace GridRaycastRange

// Return a range to iterate over grid cells which fall on the specified ray.
// The ray is specified in world space coordinates.
inline boost::coroutines2::coroutine<glm::vec3>::pull_type
slice(const GridIndexer &grid,
      const Ray &ray,
      size_t maxDepth)
{
    if constexpr (EnableVerboseBoundsChecking) {
        if (!grid.inbounds(ray.origin)) {
            throw OutOfBoundsException(fmt::format("OutOfBoundsException -- grid.boundingBox={} ; ray.origin={}",
                                                   grid.boundingBox(),
                                                   glm::to_string(ray.origin)));
        }
    }

#if 0
    // TODO: Fix the conversion from world-space ray to cell-space direction.
    // Convert the world-space ray direction to a cell-space direction.
    const AABB box = grid.boundingBox();
    const glm::vec3 mins = box.mins();
    const glm::vec3 p = (ray.direction - mins) / (box.extent*2.0f);
    const glm::ivec3 res = grid.gridResolution();
    const glm::vec3 ccDir(glm::normalize(glm::vec3(p.x * res.x, p.y * res.y, p.z * res.z)));

    // Convert the world-space ray origin to cell-space.
    const glm::ivec3 iccOrigin = grid.cellCoordsAtPoint(ray.origin);
    const glm::vec3 ccOrigin(iccOrigin.x, iccOrigin.y, iccOrigin.z);

    return boost::coroutines2::coroutine<glm::vec3>::pull_type([&](boost::coroutines2::coroutine<glm::vec3>::push_type &sink){
        GridRaycastRange::raycast(sink, Ray(ccOrigin, ccDir), maxDepth);
    });
#else
    return boost::coroutines2::coroutine<glm::vec3>::pull_type([&](boost::coroutines2::coroutine<glm::vec3>::push_type &sink){
        GridRaycastRange::raycast(sink, ray, maxDepth);
    });
#endif
}

#endif /* GridRaycast_hpp */


/*
 * File: pybind.cc
 * Created Date: 2019-09-11
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Wednesday September 25th 2019 11:37:53 am
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#include "fast_hankel_transform.hpp"

#include <Eigen/Dense>
#include <pybind11/complex.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;
using namespace Eigen;
using CRefCMat = const Ref<const MatrixXd>;

PYBIND11_MODULE(fhtcxx, m) {
  py::class_<FastHankelTransform>(m, "FastHankelTransform")
      .def(py::init<int, double, double>())
      .def("sampling", &FastHankelTransform::sampling)
      .def("set_feval", &FastHankelTransform::set_feval)
      .def("calculate", &FastHankelTransform::calculate);
}


// BLAS level 1 (matrix rows and columns)

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "utils.h"

namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;
namespace ublas = boost::numeric::ublas;

using std::cout;
using std::endl;
using std::size_t;

typedef ublas::vector<double> vct_t;
typedef ublas::matrix<double> matr_t;
typedef ublas::matrix_row<matr_t> mr_t;
typedef ublas::matrix_column<matr_t> mc_t;
typedef ublas::matrix_range<matr_t> mrng_t;
typedef ublas::matrix_slice<matr_t> msl_t;
typedef ublas::matrix_slice<matr_t const> cmsl_t;

int main() {

  cout << endl;

  int r = 7;
  int c = 8;
  matr_t m (r, c);
  init_m (m, times_plus<double> (10, 1, 1));
  print_m (m, "m");
  cout << endl;

  vct_t v (10);
  blas::set (0., v);
  print_v (v, "v");
  cout << endl;

  // m[2,.] <- 0.1 m[2,.]
  mr_t mr2 (m, 2);
  blas::scal (0.1, mr2);
  print_m (m, "0.1 m[2,.]");
  cout << endl;

  // m[2,.] <-> m[4,.]
  mr_t mr4 (m, 4);
  blas::swap (mr2, mr4);
  print_m (m, "m[2,.] <-> m[4,.]");
  cout << endl;

  // m[4,.] <- m[2,.]
  blas::copy (mr2, mr4);
  print_m (m, "m[4,.] <- m[2,.]");
  cout << endl;

  // v[2..6] <- 10 m[5,.][1..5]
  mr_t mr5 (m, 5);
  ublas::vector_range<vct_t> vr (v, ublas::range (2, 6));
  ublas::vector_range<mr_t> mr5r (mr5, ublas::range (1, 5));
  blas::axpy (10.0, mr5r, vr);
  print_v (v, "v[2..6] <- 10 m[5,.][1..5]");
  cout << endl;

  // ||m[.,3]||_1, ||m[.,3]||_2
  mc_t mc3 (m, 3);
  cout << "||m[.,3]||_1 = " << blas::asum (mc3) << endl;
  cout << "||m[.,3]||_2 = " << blas::nrm2 (mc3) << endl;
  cout << endl;

  // m[.,5] <- 0.01 m[.,3] + m[.,5]
  mc_t mc5 (m, 5);
  blas::axpy (0.01, mc3, mc5);
  print_m (m, "m[.,5] <- 0.01 m[.,3] + m[.,5]");
  cout << endl;

  // 0.1 m[.,5][1:2:3]
  ublas::vector_slice<mc_t> mc5s (mc5, ublas::slice (1, 2, 3));
  blas::scal (0.1, mc5s);
  print_m (m, "0.1 m[.,5][1:2:3]");
  cout << endl;

  // 0.1 m[4,.][1:2:4][1..3]
  ublas::vector_slice<mr_t> mr4s (mr4, ublas::slice (1, 2, 4));
  ublas::vector_range<ublas::vector_slice<mr_t> >
    mr4sr (mr4s, ublas::range (1, 3));
  blas::scal (0.1, mr4sr);
  print_m (m, "0.1 m[4,.][1:2:4][1..3]");
  cout << endl;

  // new initialization
  init_m (m, times_plus<double> (10, 1, 1));
#ifndef F_USE_DETAIL
  for (int i = 0; i < m.size1(); ++i) {
    mr_t mri (m, i);
    blas::scal (0.1, mri);
  }
#else
  // cblas level 1 function applied to matrix
  blas::detail::scal (traits::matrix_storage_size (m),
                       0.1, traits::matrix_storage (m), 1);
#endif
  matr_t const cm (m);
  print_m (cm, "new m, cm == const m");
  cout << endl;

  // m[2..6][1..8]
  mrng_t mrng (m, ublas::range (2, 6), ublas::range (1, 8));
  print_m (mrng, "mrng = m[2..6][1..8]");
  cout << endl;

  // mrng[1,.] <-> mrng[2,.]
  ublas::matrix_row<mrng_t> mrngr1 (mrng, 1);
  ublas::matrix_row<mrng_t> mrngr2 (mrng, 2);
  blas::swap (mrngr1, mrngr2);
  print_m (m, "mrng[1,.] <-> mrng[2,.]");
  cout << endl;

  // mrng[2,.] <-> mrng[1,.]
  blas::swap (mrngr2, mrngr1);
  print_m (m, "mrng[2,.] <-> mrng[1,.]");
  cout << endl;

  // mrng[.,3] <- 0.01 mrng[.,5] + mrng[.,3]
  ublas::matrix_column<mrng_t> mrngc3 (mrng, 3);
  ublas::matrix_column<mrng_t> const mrngc5 (mrng, 5);
  blas::axpy (0.01, mrngc5, mrngc3);
  print_m (m, "mrng[.,3] <- 0.01 mrng[.,5] + mrng[.,3]");
  cout << endl;

  // cm[1:2:3][2:3:2]
  cmsl_t msl (cm, ublas::slice (1, 2, 3), ublas::slice (2, 3, 2));
  print_m (msl, "cmsl = cm[1:2:3][2:3:2]");
  cout << endl;

  // ||cmsl[.,0]||_1
  ublas::matrix_column<cmsl_t> mslc0 (msl, 0);
  cout << "||cmsl[.,0]||_1 = " << blas::asum (mslc0) << endl;
  cout << endl;

  // mrng[.,3][1..4] <= 0.0001 cmsl[.,0] + mrng[.,3][1..4]
  ublas::vector_range<ublas::matrix_column<mrng_t> >
    vrmrngc3 (mrngc3, ublas::range (1, 4));
  blas::axpy (0.0001, mslc0, vrmrngc3);
  print_m (m, "mrng[.,3][1..4] <= 0.0001 cmsl[.,0] + mrng[.,3][1..4]");
  cout << endl;

}