#include <boost/iostreams/stream.hpp>

#include "bib/UTest.hpp"
#include "nn/MLP.hpp"
#include "nn/DODevMLP.hpp"
#include "bib/Combinaison.hpp"

#include "boost/property_tree/ini_parser.hpp"
#include "boost/iostreams/stream.hpp"
#include "caffe/layers/developmental_layer.hpp"

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;
}

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

  EXPECT_DOUBLE_EQ(nn.number_of_parameters(), 4);
  double weights[] = {1,0,1,0};
  nn.copyWeightsFrom(weights, false);

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

  EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), tanh(1.));

  MLP nn2(1, 1, {1}, 0.01f, 1, -1, 1, 0);
  nn2.copyWeightsFrom(weights, false);

  EXPECT_DOUBLE_EQ(nn2.computeOutVF(sens, ac), 1);
  sens[0] = -1;
  EXPECT_GE(nn2.computeOutVF(sens, ac), -0.01 - 0.001);//leaky relu
  EXPECT_LE(nn2.computeOutVF(sens, ac), -0.01 + 0.001);//leaky relu

  double weights2[] = {0,0,0,5};
  nn2.copyWeightsFrom(weights2, false);

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

  double weights3[] = {0.2,0.4,0.6, 0.8};
  nn.copyWeightsFrom(weights3, false);

  EXPECT_GE(nn.computeOutVF(sens, ac), (tanh((-1. * 0.2f + 0.4f)) * 0.6f + 0.8f) - 0.0001);
  EXPECT_LE(nn.computeOutVF(sens, ac), (tanh((-1. * 0.2f + 0.4f)) * 0.6f + 0.8f) + 0.0001);
}

TEST(MLP, LearnOpposite) {
  MLP nn(1, 1, {1}, 0.01f, 1, -1, 2, 0);
  double weights[] = {-1,0.,1. / tanh(1.), 0};
  nn.copyWeightsFrom(weights, false);

  // 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(1);
    std::vector<double> ac(0);
    sens[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(1);
    std::vector<double> ac(0);
    sens[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(1);
    std::vector<double> ac(0);
    sens[0] = x1;

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

TEST(MLP, LearnAndOr) {
  MLP nn(3, 2, {10}, 0.01f, 1, -1, 2, 0);

  // 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.16);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.16);
  }
}

// 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) {

TEST(MLP, LearnNonLinearLabelWeight) {
  uint batch_size = 5004;
  MLP nn(1, 1, {4}, 0.005f, batch_size, -1, 2, 0, true);

  std::vector<double> sensors(batch_size);
  std::vector<double> actions(0);
  std::vector<double> qvalues(batch_size);
  std::vector<double> label_weight(batch_size);

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

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

  bib::Combinaison::continuous<>(iter, 1, 0, 1, batch_size/2 );
  bib::Combinaison::continuous<>(iter2, 1, 0, 1, batch_size/2 );
  EXPECT_EQ(n, batch_size);

  nn.learn_batch_lw(sensors, actions, qvalues, label_weight, 300);

  // 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));
    }
  }
}

TEST(MLP, LearnNonLinearLabelWeightWithNullImportance) {
  MLP nn(1, 1, {4}, 0.01f, 11*2, -1, 2, 0, true);

  std::vector<double> sensors(11*2);
  std::vector<double> actions(0);
  std::vector<double> qvalues(11*2);
  std::vector<double> label_weight(11*2);

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

  auto iter2 = [&](const std::vector<double>& x) {
    sensors[n] = x[0];
    qvalues[n] = x[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_batch_lw(sensors, actions, qvalues, label_weight, 500);

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

    double out = (0.0*x1 + 0.8*x1*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.18);
    EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.18);
  }
}

TEST(MLP, CaffeSaveLoad) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
//       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number
      MLP nn(2, 1, {20,10}, 0.005f, batch_size, -1, hidden_layer, batch_norm);

      std::vector<double> sensors(batch_size);
      std::vector<double> actions(batch_size);
      std::vector<double> qvalues(batch_size);

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

        if(bib::Utils::rand01() < 0.4) {
          sensors[n] = bib::Utils::rand01()*2 -1;
          actions[n] = bib::Utils::rand01()*2 -1;
        }
        qvalues[n] = -(actions[n]*actions[n]-(sensors[n]*sensors[n]));

        n++;
      };

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

      EXPECT_EQ(n, batch_size);
      nn.learn_batch(sensors, actions, qvalues, 5);

      nn.save("CaffeSaveLoad.cache");
      MLP nn_test(nn, false, ::caffe::Phase::TEST);

      MLP nn2(2, 1, {20,10}, 0.005f, batch_size, -1, hidden_layer, batch_norm);
      nn2.load("CaffeSaveLoad.cache");

      MLP nn3(nn2, false, ::caffe::Phase::TEST);
      MLP nn4(nn2, true);//learning net

      for (n = 0; n < VALGRIND_REDUCE(100, 3) ; n++) {
        double x0 = bib::Utils::rand01()*2 -1;
        double x1 = bib::Utils::rand01()*2 -1;

//         double out = -(x1*x1-(x0*x0));

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

//    This is not a test of performance
//      Some configuration of batch norm and hidden layer are very bad
//         EXPECT_GT(nn_test.computeOutVF(sens, ac), out - 0.3);
//         EXPECT_LT(nn_test.computeOutVF(sens, ac), out + 0.3);
//
//         if(batch_norm == 0) {
// //        nn and nn2 are in learning phase so it's still learn batch norm
// //        don't test them during learning with batch norm
//           EXPECT_GT(nn.computeOutVF(sens, ac), out - 0.21);
//           EXPECT_LT(nn.computeOutVF(sens, ac), out + 0.21);
//         }

        //check that learning net are the same
        EXPECT_DOUBLE_EQ(nn.computeOutVF(sens, ac), nn2.computeOutVF(sens, ac));
        EXPECT_DOUBLE_EQ(nn2.computeOutVF(sens, ac), nn4.computeOutVF(sens, ac));

        //then check that testing net are the same
        EXPECT_DOUBLE_EQ(nn_test.computeOutVF(sens, ac), nn3.computeOutVF(sens, ac));

      }

    }
  }
}

TEST(MLP, CaffeCopyActor) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number
      std::vector<double> sensors(batch_size);

      MLP actor(1, {8}, 1, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      MLP actor_test(actor, true);//train
      MLP actor_test2(actor, false, ::caffe::Phase::TEST);//test
      MLP actor_test3(actor, true, ::caffe::Phase::TEST);//test
      MLP actor_test4(actor_test2, false, ::caffe::Phase::TEST);//test

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

        if(bib::Utils::rand01() < 0.4) {
          sensors[n] = bib::Utils::rand01()*2 -1;
        }
        n++;
      };

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

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor_test.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs3 = actor_test2.computeOutBatch(sensors);
        auto all_actions_outputs4 = actor_test3.computeOutBatch(sensors);
        auto all_actions_outputs5 = actor_test4.computeOutBatch(sensors);
        for (uint i =0; i < batch_size; i++) {
          EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));
          EXPECT_DOUBLE_EQ(all_actions_outputs4->at(i), all_actions_outputs3->at(i));
          EXPECT_DOUBLE_EQ(all_actions_outputs4->at(i), all_actions_outputs5->at(i));
          if(batch_norm == 0){ //is not the same because batch norm is not learning
            EXPECT_DOUBLE_EQ(all_actions_outputs2->at(i), all_actions_outputs3->at(i));
          }
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
        delete all_actions_outputs3;
        delete all_actions_outputs4;
        delete all_actions_outputs5;
      }
    }
  }
}

TEST(MLP, CaffeCopyWeightsActor) {
  std::vector<uint> batch_norms = {4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number
      std::vector<double> sensors(batch_size);

      MLP actor(1, {8}, 1, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      MLP actor_nobn(1, {8}, 1, 0.01f, batch_size, hidden_layer, 0, 0);
      EXPECT_EQ(actor_nobn.number_of_parameters(false), (uint) 25);
      EXPECT_GT(actor.number_of_parameters(false), (uint) 25);
    }
  }
}

TEST(MLP, BackwardActor) {//valgrind test only
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <= 3 ; hidden_layer++) {
      LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number

      std::vector<double> sensors(batch_size);

      MLP actor(1, {8}, 1, 0.01f, batch_size, hidden_layer, 2, batch_norm);

      for(uint forced = 0; forced < 5 ; forced++) {
        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];

          if(bib::Utils::rand01() < 0.4) {
            sensors[n] = bib::Utils::rand01()*2 -1;
          }
          n++;
        };

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

        auto all_actions_outputs = actor.computeOutBatch(sensors);

        //call to forward needed
        actor.actor_backward();
        delete all_actions_outputs;

        caffe::Blob<double> diff({400,1});
        all_actions_outputs = actor.computeOutBatch(sensors);
        const auto actor_actions_blob = actor.getNN()->blob_by_name(MLP::actions_blob_name);
        actor_actions_blob->ShareDiff(diff);
        actor.actor_backward();
        actor.getSolver()->ApplyUpdate();
        actor.getSolver()->set_iter(actor.getSolver()->iter() + 1);
        delete all_actions_outputs;
      }
    }
  }
}

TEST(MLP, BackwardCritic) {//valgrind test only
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <= 3 ; hidden_layer++) {
      LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number

      std::vector<double> sensors(batch_size);
      std::vector<double> actions(batch_size);

      MLP nn(2, 1, {50,10}, 0.001f, batch_size, -1, hidden_layer, batch_norm);

      for(uint forced = 0; forced < 5 ; forced++) {
        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];
          actions[n] = x[1];

          if(bib::Utils::rand01() < 0.75) {
            sensors[n] = bib::Utils::rand01()*2 -1;
            actions[n] = bib::Utils::rand01()*2 -1;
          }
          n++;
        };

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

        auto all_q = nn.computeOutVFBatch(sensors, actions);

        //call to forward needed
        nn.critic_backward();
        delete all_q;
      }

    }
  }
}

TEST(MLP, OptimizeNNTroughGradientOfAnotherNN) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number
      MLP nn(2, 1, {50,10}, 0.001f, batch_size, -1, hidden_layer, batch_norm);//learning

      std::vector<double> sensors(batch_size);
      std::vector<double> actions(batch_size);
      std::vector<double> qvalues(batch_size);

      for(uint forced = 0; forced < VALGRIND_REDUCE(50,2) ; forced++) {
        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];
          actions[n] = x[1];

          if(bib::Utils::rand01() < 0.75) {
            sensors[n] = bib::Utils::rand01()*2 -1;
            actions[n] = bib::Utils::rand01()*2 -1;
          }
          qvalues[n] = -(actions[n]*actions[n]-(sensors[n]*sensors[n])*actions[n]);

          n++;
        };

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

        EXPECT_EQ(n, batch_size);
        nn.learn_batch(sensors, actions, qvalues, VALGRIND_REDUCE(50,2));
      }

      MLP nn_test(nn, false, ::caffe::Phase::TEST);//testing
      MLP actor(1, {8}, 1, 0.01f, batch_size, hidden_layer, 2, batch_norm);
      MLP actor_test(actor, true);

      for(uint forced = 0; forced < VALGRIND_REDUCE(50,2) ; forced++) {
//         LOG_DEBUG("forced  "<< forced);
        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];

          if(bib::Utils::rand01() < 0.4) {
            sensors[n] = bib::Utils::rand01()*2 -1;
          }
          n++;
        };

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

        nn.ZeroGradParameters();
        nn_test.ZeroGradParameters();

        auto all_actions_outputs = actor.computeOutBatch(sensors);
        auto all_actions_outputs2 = actor_test.computeOutBatch(sensors);
        if(forced == 0)
          for (uint i =0; i < batch_size; i++) {
            EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));
          }

        delete nn.computeOutVFBatch(sensors, *all_actions_outputs);
        delete nn_test.computeOutVFBatch(sensors, *all_actions_outputs2);

        const auto q_values_blob = nn.getNN()->blob_by_name(MLP::q_values_blob_name);
        const auto q_values_blob2 = nn_test.getNN()->blob_by_name(MLP::q_values_blob_name);
        double* q_values_diff = q_values_blob->mutable_cpu_diff();
        double* q_values_diff2 = q_values_blob2->mutable_cpu_diff();
        for (uint i =0; i < batch_size; i++) {
          q_values_diff[q_values_blob->offset(i,0,0,0)] = -1.0f;
          q_values_diff2[q_values_blob2->offset(i,0,0,0)] = -1.0f;
        }
        nn.critic_backward();
        nn_test.critic_backward();
        const auto critic_action_blob = nn.getNN()->blob_by_name(MLP::actions_blob_name);
        const auto critic_action_blob2 = nn_test.getNN()->blob_by_name(MLP::actions_blob_name);

        const double* action_diff = critic_action_blob->cpu_diff();
        const double* action_diff2 = critic_action_blob2->cpu_diff();

        if(batch_norm == 0 && forced == 0)
          for (uint n = 0; n < batch_size; ++n) {
            for (uint h = 0; h < 1; ++h) {
              int offset = critic_action_blob->offset(n,h,0,0);
              int offset2 = critic_action_blob2->offset(n,h,0,0);
              EXPECT_DOUBLE_EQ(action_diff[offset], action_diff2[offset2]);
            }
          }

        // Transfer input-level diffs from Critic to Actor
        const auto actor_actions_blob = actor.getNN()->blob_by_name(MLP::actions_blob_name);
        actor_actions_blob->ShareDiff(*critic_action_blob);
        actor.actor_backward();
        actor.getSolver()->ApplyUpdate();
        actor.getSolver()->set_iter(actor.getSolver()->iter() + 1);

        const auto actor_actions_blob2 = actor_test.getNN()->blob_by_name(MLP::actions_blob_name);
        actor_actions_blob2->ShareDiff(*critic_action_blob2);
        actor_test.actor_backward();
        actor_test.getSolver()->ApplyUpdate();
        actor_test.getSolver()->set_iter(actor_test.getSolver()->iter() + 1);

        delete all_actions_outputs;
        delete all_actions_outputs2;
      }

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

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

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

        auto actor1_out = actor.computeOut(sens);
        auto actor2_out = actor_test.computeOut(sens);
        VALGRIND_SKIP(LOG_FILE("OptimizeNNTroughGradientOfAnotherNNFann.data", x1 << " " << actor1_out->at(0) << " " <<
        actor2_out->at(0)<< " " << 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 - actor1_out->at(0));
        ac2_error += fabs(out - actor2_out->at(0));
        q_error += fabs(nn.computeOutVF(sens, ac) - qout);

        delete actor1_out;
        delete actor2_out;
      }

      ac1_error /= 100.f;
      ac2_error /= 100.f;
      q_error /= 100.f;

      //if critic well learned
      if(q_error < 0.1){
        VALGRIND_SKIP(EXPECT_LT(ac1_error, 0.15));
        VALGRIND_SKIP(EXPECT_LT(ac2_error, 0.22));
      }

      LOG_DEBUG("bn " << batch_norm << " hiddenl " << hidden_layer << " " << ac1_error << " " << ac2_error << " " << q_error);
    }
  }
}

TEST(MLP, DevelopmentalLayer) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number
      std::vector<double> sensors(batch_size);

      MLP actor(1, {8}, 1, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      DODevMLP actor2(1, {8}, 1, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      std::string config="[devnn]\nst_scale=false\nst_probabilistic=0\nac_scale=false\nac_probabilistic=0\nst_control=\nac_control=\n";
      boost::property_tree::ptree properties;
      boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
      boost::property_tree::ini_parser::read_ini(stream, properties);
      actor2.exploit(&properties, nullptr);

      EXPECT_GT(actor2.number_of_parameters(), actor.number_of_parameters());
      EXPECT_EQ(actor2.number_of_parameters(true) - 2, actor.number_of_parameters(true));
      uint n = 0;
      auto iter = [&](const std::vector<double>& x) {
        sensors[n] = x[0];

        if(bib::Utils::rand01() < 0.4) {
          sensors[n] = bib::Utils::rand01()*2 -1;
        }
        n++;
      };

      //copy param of actor to actor2
      double* weights = new double[actor.number_of_parameters(true)];
      actor.copyWeightsTo(weights, true);
      double* weights2 = new double[actor2.number_of_parameters(true)];
      actor2.copyWeightsTo(weights2, true);
      for(uint i=1;i< actor2.number_of_parameters(true)-1; i++)
        weights2[i]=weights[i-1];
      weights2[0]=1.f;
      weights2[actor2.number_of_parameters(true)-1]=1.f;
      actor2.copyWeightsFrom(weights2, true);
      bib::Combinaison::continuous<>(iter, 1, -1, 1, batch_size);

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        for (uint i =0; i < batch_size; i++) {
          EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
      }

      weights2[actor2.number_of_parameters(true)-1] = 0.6f;
      actor2.copyWeightsFrom(weights2, true);
      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        uint fit=0;
        for (uint i =0; i < batch_size; i++) {
          if(all_actions_outputs->at(i) == all_actions_outputs2->at(i))
            fit++;
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
        double proba = (((float)fit)/((float)batch_size));
        EXPECT_GE(proba, 0.52);
        EXPECT_LE(proba, 0.68);
      }

      delete[] weights;
      delete[] weights2;
    }
  }
}

TEST(MLP, DevelopmentalLayerMoreDimension) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 343; //must be a power3 number
      std::vector<double> sensors(batch_size*3);

      MLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      DODevMLP actor2(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      std::string config="[devnn]\nst_scale=false\nst_probabilistic=0\nac_scale=false\nac_probabilistic=0\nst_control=\nac_control=None\n";
      boost::property_tree::ptree properties;
      boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
      boost::property_tree::ini_parser::read_ini(stream, properties);
      actor2.exploit(&properties, nullptr);

      EXPECT_EQ(actor2.number_of_parameters(true) - 3 - 4, actor.number_of_parameters(true));
      uint n = 0;
      auto iter = [&](const std::vector<double>& x) {
        sensors[n] = x[0];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[1];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[2];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;
      };

      //copy param of actor to actor2
      double* weights = new double[actor.number_of_parameters(true)];
      actor.copyWeightsTo(weights, true);
      double* weights2 = new double[actor2.number_of_parameters(true)];
      actor2.copyWeightsTo(weights2, true);
      for(uint i=0;i<3;i++)
        weights2[i]=1.f;
      for(uint i=3;i< actor.number_of_parameters(true) + 3; i++)
        weights2[i]=weights[i-3];
      for(uint i=actor.number_of_parameters(true); i < actor2.number_of_parameters(true)-3;i++)
        weights2[i+3]=1.f;
      actor2.copyWeightsFrom(weights2, true);
      bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
      CHECK_EQ(n, batch_size*3);

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        for (uint i =0; i < batch_size; i++) {
          EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
      }

      weights2[actor2.number_of_parameters(true)-4] = 0.1f;
      weights2[actor2.number_of_parameters(true)-3] = 0.8f;
      weights2[actor2.number_of_parameters(true)-2] = 0.4f;
      weights2[actor2.number_of_parameters(true)-1] = 0.6f;
      actor2.copyWeightsFrom(weights2, true);

      delete[] weights;
      delete[] weights2;

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        uint fit0=0;
        uint fit1=0;
        uint fit2=0;
        uint fit3=0;
        uint y=0;
        for (uint i =0; i < batch_size; i++) {
          for(uint j=0;j<4;j++){
            if(all_actions_outputs->at(y) == all_actions_outputs2->at(y)){
              if(j==0)
                fit0++;
              else if(j==1)
                fit1++;
              else if(j==2)
                fit2++;
              else
                fit3++;
            }
            y++;
          }
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
        double proba0 = (((float)fit0)/((float)batch_size));
        double proba1 = (((float)fit1)/((float)batch_size));
        double proba2 = (((float)fit2)/((float)batch_size));
        double proba3 = (((float)fit3)/((float)batch_size));
        double std_=0.1;
        EXPECT_GE(proba0, 0.1-std_);
        EXPECT_LE(proba0, 0.1+std_);
        EXPECT_GE(proba1, 0.8-std_);
        EXPECT_LE(proba1, 0.8+std_);
        EXPECT_GE(proba2, 0.4-std_);
        EXPECT_LE(proba2, 0.4+std_);
        EXPECT_GE(proba3, 0.6-std_);
        EXPECT_LE(proba3, 0.6+std_);
      }
    }
  }
}

TEST(MLP, DevelopmentalLayerControlRestriction) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 343; //must be a power3 number
      std::vector<double> sensors(batch_size*3);

      MLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      DODevMLP actor2(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      std::string config="[devnn]\nst_scale=false\nst_probabilistic=0\nac_scale=false\nac_probabilistic=0\nst_control=1:2\nac_control=1:2\n";
      boost::property_tree::ptree properties;
      boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
      boost::property_tree::ini_parser::read_ini(stream, properties);
      actor2.exploit(&properties, nullptr);

      EXPECT_EQ(actor2.number_of_parameters(true) - (3-1) - (4-2), actor.number_of_parameters(true));
      uint n = 0;
      auto iter = [&](const std::vector<double>& x) {
        sensors[n] = x[0];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[1];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[2];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;
      };

      //copy param of actor to actor2
      double* weights = new double[actor.number_of_parameters(true)];
      actor.copyWeightsTo(weights, true);
      double* weights2 = new double[actor2.number_of_parameters(true)];
      actor2.copyWeightsTo(weights2, true);
      for(uint i=0;i<3-1;i++)
        weights2[i]=1.f;
      for(uint i=3-1;i< actor.number_of_parameters(true) + 3-1; i++)
        weights2[i]=weights[i-(3-1)];
      for(uint i=actor.number_of_parameters(true); i < actor2.number_of_parameters(true)-(3-1);i++)
        weights2[i+(3-1)]=1.f;
      EXPECT_EQ(actor2.number_of_parameters(true)-(3-1) - actor.number_of_parameters(true), 2);
      actor2.copyWeightsFrom(weights2, true);
      bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
      CHECK_EQ(n, batch_size*3);

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        for (uint i =0; i < batch_size; i++) {
          EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
      }

      weights2[actor2.number_of_parameters(true)-2] = 0.4f;
      weights2[actor2.number_of_parameters(true)-1] = 0.6f;
      actor2.copyWeightsFrom(weights2, true);

      delete[] weights;
      delete[] weights2;

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        uint fit0=0;
        uint fit1=0;
        uint fit2=0;
        uint fit3=0;
        uint y=0;
        for (uint i =0; i < batch_size; i++) {
          for(uint j=0;j<4;j++){
            if(all_actions_outputs->at(y) == all_actions_outputs2->at(y)){
              if(j==0)
                fit0++;
              else if(j==1)
                fit1++;
              else if(j==2)
                fit2++;
              else
                fit3++;
            }
            y++;
          }
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
        double proba0 = (((float)fit0)/((float)batch_size));
        double proba1 = (((float)fit1)/((float)batch_size));
        double proba2 = (((float)fit2)/((float)batch_size));
        double proba3 = (((float)fit3)/((float)batch_size));
        double std_=0.11;
        EXPECT_GE(proba0, 1.-std_);
        EXPECT_LE(proba0, 1.+std_);
        EXPECT_GE(proba1, 0.4-std_);
        EXPECT_LE(proba1, 0.4+std_);
        EXPECT_GE(proba2, 0.6-std_);
        EXPECT_LE(proba2, 0.6+std_);
        EXPECT_GE(proba3, 1.-std_);
        EXPECT_LE(proba3, 1.+std_);
      }
    }
  }
}

TEST(MLP, DevelopmentalLayerSharedParameters) {
  std::vector<uint> batch_norms = {0,4,6,7,8,10,11,12,14,15};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 343; //must be a power3 number
      std::vector<double> sensors(batch_size*3);

      MLP actor2(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      DODevMLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      std::string config="[devnn]\nst_scale=false\nst_probabilistic=0\nac_scale=false\nac_probabilistic=0\nst_control=1:2\nac_control=1:2\n";
      boost::property_tree::ptree properties;
      boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
      boost::property_tree::ini_parser::read_ini(stream, properties);
      actor.exploit(&properties, nullptr);

      MLP critic(4+3, 3, {10,5}, 0.005f, batch_size, -1, hidden_layer, batch_norm);
      DODevMLP critic2(4+3, 3, {10,5}, 0.005f, batch_size, -1, hidden_layer, batch_norm);
      critic2.exploit(&properties, (MLP*) &actor);
      DODevMLP critic3(critic2, true);

      EXPECT_EQ(critic.number_of_parameters(true), critic2.number_of_parameters(true)-4);
      EXPECT_EQ(critic2.number_of_parameters(true), critic3.number_of_parameters(true));

      uint n = 0;
      auto iter = [&](const std::vector<double>& x) {
        sensors[n] = x[0];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[1];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[2];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;
      };

      if(batch_norm == 0){
        auto blob2 = actor.getNN()->layer_by_name("devnn_states")->blobs()[0];
        auto blob3 = critic2.getNN()->layer_by_name("devnn_states")->blobs()[0];

        EXPECT_EQ(blob3->count(), blob2->count());
        //careful to learnable_params order
        EXPECT_EQ(blob3->count(), critic2.getNN()->learnable_params()[1]->count());
        EXPECT_EQ(blob3->count(), actor.getNN()->learnable_params()[0]->count());
        for (int i =0;i<blob3->count();i++){
          EXPECT_EQ(blob3->cpu_data()[i], blob2->cpu_data()[i]);
          EXPECT_EQ(blob3->cpu_data()[i], critic2.getNN()->learnable_params()[1]->cpu_data()[i]);
          EXPECT_EQ(blob2->cpu_data()[i], actor.getNN()->learnable_params()[0]->cpu_data()[i]);
        }

        //copy param of actor to actor2
        double* weights = new double[actor.number_of_parameters(true)];
        actor.copyWeightsTo(weights, true);

        double* weights2 = new double[critic2.number_of_parameters(true)];
        critic2.copyWeightsTo(weights2, true);

        double* weights3 = new double[critic3.number_of_parameters(true)];
        critic3.copyWeightsTo(weights3, true);

        const double* weights4 = critic2.getNN()->learnable_params()[1]->cpu_data();
        const double* weights5 = actor.getNN()->learnable_params()[0]->cpu_data();

        ASSERT(critic2.getNN()->learnable_params()[1]->count(), 2);
        ASSERT(actor.getNN()->learnable_params()[0]->count(), 2);

        for(uint i=0;i<3-1;i++){
          EXPECT_DOUBLE_EQ(weights[i], weights5[i]);
          EXPECT_DOUBLE_EQ(weights4[i], weights2[i+2]);
          EXPECT_DOUBLE_EQ(weights[i], weights2[i+2]);
          EXPECT_DOUBLE_EQ(weights2[i], weights3[i]);//copy constructor for DODEVMLP
        }
//      change parameters of actor
        for(uint i=actor.number_of_parameters(true); i < actor2.number_of_parameters(true)-(3-1);i++)
          weights2[i+(3-1)]=1.f;
        actor.copyWeightsFrom(weights, true);

        for(uint i=0;i<3-1;i++){
          EXPECT_DOUBLE_EQ(weights[i], weights5[i]);
          EXPECT_DOUBLE_EQ(weights4[i], weights2[i+2]);
          EXPECT_DOUBLE_EQ(weights[i], weights2[i+2]);
          EXPECT_DOUBLE_EQ(weights2[i], weights3[i]);//copy constructor for DODEVMLP
        }

        delete[] weights;
        delete[] weights2;
        delete[] weights3;
      }
      bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
      CHECK_EQ(n, batch_size*3);

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//no dev layer
        auto q_v = critic2.computeOutVFBatch(sensors, *all_actions_outputs2);

        auto blob2 = actor.getNN()->layer_by_name("devnn_actions")->blobs()[0];
        auto blob3 = critic2.getNN()->layer_by_name("devnn_actions")->blobs()[0];
        auto blob33 = critic3.getNN()->layer_by_name("devnn_actions")->blobs()[0];

        ASSERT(blob3->count(), blob2->count());
        ASSERT(blob33->count(), blob2->count());
        for (int i =0;i<blob3->count();i++){
          EXPECT_EQ(blob3->cpu_data()[i], blob2->cpu_data()[i]);
          EXPECT_EQ(blob33->cpu_data()[i], blob2->cpu_data()[i]);
        }

        auto blob4 = actor.getNN()->layer_by_name("devnn_states")->blobs()[0];
        auto blob5 = critic2.getNN()->layer_by_name("devnn_states")->blobs()[0];
        auto blob55 = critic3.getNN()->layer_by_name("devnn_states")->blobs()[0];

        ASSERT(blob4->count(), blob5->count());
        ASSERT(blob55->count(), blob5->count());
        for (int i =0;i<blob4->count();i++){
          EXPECT_EQ(blob4->cpu_data()[i], blob5->cpu_data()[i]);
          EXPECT_EQ(blob55->cpu_data()[i], blob5->cpu_data()[i]);
        }

        delete q_v;
        delete all_actions_outputs;
        delete all_actions_outputs2;
      }
    }
  }
}

TEST(MLP, DevelopmentalLayerScalingControl) {
  std::vector<uint> batch_norms = {0};
  for(uint batch_norm : batch_norms) {
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 343; //must be a power3 number
      std::vector<double> sensors(batch_size*3);

      MLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      DODevMLP actor2(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
      std::string config="[devnn]\nst_scale=true\nst_probabilistic=0\nac_scale=true\nac_probabilistic=0\nst_control=1:2\nac_control=1:2\n";
      boost::property_tree::ptree properties;
      boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
      boost::property_tree::ini_parser::read_ini(stream, properties);
      actor2.exploit(&properties, nullptr);

      EXPECT_EQ(actor2.number_of_parameters(true) - (3-1) - (4-2), actor.number_of_parameters(true));
      uint n = 0;
      auto iter = [&](const std::vector<double>& x) {
        sensors[n] = x[0];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[1];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;

        sensors[n] = x[2];
        if(bib::Utils::rand01() < 0.4)
          sensors[n] = bib::Utils::rand01()*2 -1;
        n++;
      };

      //copy param of actor to actor2
      double* weights = new double[actor.number_of_parameters(true)];
      actor.copyWeightsTo(weights, true);
      double* weights2 = new double[actor2.number_of_parameters(true)];
      actor2.copyWeightsTo(weights2, true);
      for(uint i=0;i<3-1;i++)
        weights2[i]=1.f;
      for(uint i=3-1;i< actor.number_of_parameters(true)+3-1; i++)
        weights2[i]=weights[i-(3-1)];
      for(uint i=actor.number_of_parameters(true); i < actor2.number_of_parameters(true)-(3-1);i++)
        weights2[i+(3-1)]=1.f;
      actor2.copyWeightsFrom(weights2, true);
      bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
      CHECK_EQ(n, batch_size*3);

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns
        for (uint i =0; i < batch_size; i++) {
          EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
      }

      weights2[actor2.number_of_parameters(true)-2] = 0.4f;
      weights2[actor2.number_of_parameters(true)-1] = 0.6f;
      actor2.copyWeightsFrom(weights2, true);

      delete[] weights;
      delete[] weights2;

      for(uint forced=0;forced<5;forced++){
        auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
        auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns

        uint y=0;
        uint fit0=0;
        uint fit1=0;
        uint fit2=0;
        uint fit3=0;
        for (uint i =0; i < batch_size; i++) {
          for(uint j=0;j<4;j++){
            if(j==0 && all_actions_outputs->at(y) == all_actions_outputs2->at(y))
              fit0++;
            else if(j==1 && std::fabs(all_actions_outputs->at(y)*(1./0.4) - all_actions_outputs2->at(y)) <= 1e-8 )
              fit1++;
            else if(j==2 && std::fabs(all_actions_outputs->at(y)*(1./0.6) - all_actions_outputs2->at(y)) <= 1e-8 )
              fit2++;
            else if(j==3 && all_actions_outputs->at(y) == all_actions_outputs2->at(y))
              fit3++;
            else {
//               LOG_DEBUG(j << " " << all_actions_outputs->at(y) <<" " << all_actions_outputs->at(y)*(1./0.4) << " " << all_actions_outputs2->at(y));
            }
            y++;
          }
        }
        delete all_actions_outputs;
        delete all_actions_outputs2;
        double proba0 = (((float)fit0)/((float)batch_size));
        double proba1 = (((float)fit1)/((float)batch_size));
        double proba2 = (((float)fit2)/((float)batch_size));
        double proba3 = (((float)fit3)/((float)batch_size));
        double std_=0.09;
        EXPECT_GE(proba0, 1.-std_);
        EXPECT_LE(proba0, 1.+std_);
        EXPECT_GE(proba1, 0.4-std_);
        EXPECT_LE(proba1, 0.4+std_);
        EXPECT_GE(proba2, 0.6-std_);
        EXPECT_LE(proba2, 0.6+std_);
        EXPECT_GE(proba3, 1.-std_);
        EXPECT_LE(proba3, 1.+std_);
      }
    }
  }
}

TEST(MLP, DevelopmentalLayerBackward) {
  std::vector<uint> batch_norms = {0};
  for(uint batch_norm : batch_norms)
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++)
      for(uint probabilistic = 0; probabilistic <=3 ; probabilistic++){
        uint batch_size = 343; //must be a power3 number
        std::vector<double> sensors(batch_size*3);

        DODevMLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
        std::string config="[devnn]\nst_scale=false\nst_probabilistic=";
        config += std::to_string(probabilistic);
        config += "\nac_scale=false\nac_probabilistic=";
        config += std::to_string(probabilistic);
        config += "\nst_control=1:2\nac_control=1:2\n";
        boost::property_tree::ptree properties;
        boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
        boost::property_tree::ini_parser::read_ini(stream, properties);
        actor.exploit(&properties, nullptr);

        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;

          sensors[n] = x[1];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;

          sensors[n] = x[2];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;
        };

        bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
        CHECK_EQ(n, batch_size*3);

        for(uint forced=0;forced<5;forced++){
          auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
          actor.actor_backward();
          delete all_actions_outputs;
        }
      }
}

TEST(MLP, DevelopmentalLayerBackwardDiffCompute) {
  std::vector<uint> batch_norms = {0};
  for(uint batch_norm : batch_norms)
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++)
      for(uint probabilistic = 0; probabilistic <=3 ; probabilistic++){
        uint batch_size = 343; //must be a power3 number
        std::vector<double> sensors(batch_size*3);

        DODevMLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
        std::string config="[devnn]\ncompute_diff_backward=true\nst_scale=false\nst_probabilistic=";
        config += std::to_string(probabilistic);
        config += "\nac_scale=false\nac_probabilistic=";
        config += std::to_string(probabilistic);
        config += "\nst_control=1:2\nac_control=1:2\n";
        boost::property_tree::ptree properties;
        boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
        boost::property_tree::ini_parser::read_ini(stream, properties);
        actor.exploit(&properties, nullptr);

        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;

          sensors[n] = x[1];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;

          sensors[n] = x[2];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;
        };

        bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
        CHECK_EQ(n, batch_size*3);

        for(uint forced=0;forced<5;forced++){
          auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
          actor.actor_backward();
          delete all_actions_outputs;
        }
      }
}

TEST(MLP, DevelopmentalLayerHeuristic1) {
  std::vector<uint> batch_norms = {0};
  for(uint batch_norm : batch_norms)
    for(uint hidden_layer = 1; hidden_layer <=3 ; hidden_layer++)
      for(uint probabilistic = 0; probabilistic <=3 ; probabilistic++){
        uint batch_size = 343; //must be a power3 number
        std::vector<double> sensors(batch_size*3);

        DODevMLP actor(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
        {
          std::string config="[devnn]\nheuristic=1\nst_scale=false\nst_probabilistic=1\n";
          config += "ac_scale=false\nac_probabilistic=1\nst_control=1:2\nac_control=1:2\n";
          config += "heuristic_devpoints=0,0,30,30\n";
          boost::property_tree::ptree properties;
          boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
          boost::property_tree::ini_parser::read_ini(stream, properties);
          actor.exploit(&properties, nullptr);
        }

        DODevMLP actor2(3, {8}, 4, 0.01f, batch_size, hidden_layer, 0, batch_norm);//train
        {
          std::string config="[devnn]\nheuristic=1\nst_scale=false\nst_probabilistic=1\n";
          config += "ac_scale=false\nac_probabilistic=1\nst_control=None\nac_control=1:2\n";
          config += "heuristic_devpoints=30,30\n";
          boost::property_tree::ptree properties;
          boost::iostreams::stream<boost::iostreams::array_source> stream(config.c_str(), config.size());
          boost::property_tree::ini_parser::read_ini(stream, properties);
          actor2.exploit(&properties, nullptr);
        }

        uint n = 0;
        auto iter = [&](const std::vector<double>& x) {
          sensors[n] = x[0];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;

          sensors[n] = x[1];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;

          sensors[n] = x[2];
          if(bib::Utils::rand01() < 0.4)
            sensors[n] = bib::Utils::rand01()*2 -1;
          n++;
        };

        bib::Combinaison::continuous<>(iter, 3, -1, 1, 6);
        CHECK_EQ(n, batch_size*3);

        double* weights = new double[actor.number_of_parameters(true)];
        actor.copyWeightsTo(weights, true);
        actor2.copyWeightsFrom(weights, true);
        delete[] weights;

        EXPECT_EQ(actor.number_of_parameters(true), actor2.number_of_parameters(true));
        EXPECT_GT(actor.number_of_parameters(false), actor2.number_of_parameters(false));

        {
          auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
          auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns

          for (uint i =0; i < batch_size; i++)
            for (uint j =0; j < 4; j++)
              if(all_actions_outputs->at(i*4+j) != 0 && sensors[i*3+1] > 0.0001 && sensors[i*3+2] > 0.0001){
                EXPECT_NE(all_actions_outputs->at(i*4+j), all_actions_outputs2->at(i*4+j));
              }

          delete all_actions_outputs;
          delete all_actions_outputs2;
        }
        actor.inform(0, 0.f);
        {
          auto all_actions_outputs = actor.computeOutBatch(sensors);//batch norm learns
          auto all_actions_outputs2 = actor2.computeOutBatch(sensors);//batch norm learns

          for (uint i =0; i < batch_size; i++)
            EXPECT_DOUBLE_EQ(all_actions_outputs->at(i), all_actions_outputs2->at(i));

          delete all_actions_outputs;
          delete all_actions_outputs2;
        }
      }
}

TEST(MLP, CaffeBNTesting) {
  std::vector<uint> batch_norms = {4,6,7,8,10,11,12,14,15};
  std::vector<bool> alola = {true,false};
  uint hidden_layer = 1;
  for(uint batch_norm : batch_norms) {
    for(bool add_loss_layer : alola) {
      //       LOG_DEBUG("bn " << batch_norm << " " << hidden_layer);
      uint batch_size = 400; //must be a squared number
      std::vector<double> sensors(batch_size);
      std::vector<double> sensors2(batch_size);

      MLP actor(1, {50,25}, 1, 0.01f, batch_size, hidden_layer, 0, batch_norm, add_loss_layer);//train
      MLP actor_bn_dontlearn(actor, false, ::caffe::Phase::TEST);
      EXPECT_EQ(actor_bn_dontlearn.number_of_parameters(false), actor.number_of_parameters(false));

      uint n = 0;
      auto iter = [&](const std::vector<double>& x) {
        sensors[n] = x[0];
        sensors2[n] = 5.f + x[0];

        if(bib::Utils::rand01() < 0.4) {
          sensors[n] = bib::Utils::rand01()*2 -1;
          sensors2[n] = bib::Utils::rand01()*2 -1;
        }
        n++;
      };

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

      if(add_loss_layer)
          actor.learn_batch(sensors2, {}, sensors2, 5);
      auto all_actions_outputs1 = actor.computeOutBatch(sensors);
      delete actor.computeOutBatch(sensors2);
      delete actor.computeOutBatch(sensors);
      delete actor.computeOutBatch(sensors2);
      delete actor.computeOutBatch(sensors);
      delete actor.computeOutBatch(sensors2);
      auto all_actions_outputs2 = actor.computeOutBatch(sensors);
      //because BN is learning all_actions_outputs1 != all_actions_outputs2
//       apparently it is not that sure...
      double diff = 0;
      for(uint i=0; i<batch_size; i++)
        diff += fabs(all_actions_outputs1->at(i) - all_actions_outputs2->at(i));
//       LOG_DEBUG(diff);
//       conclusion BN output is the same over severy same batch

      delete all_actions_outputs1;
      delete all_actions_outputs2;
//       but at least the weight of BN changed
      double* weights = new double[actor.number_of_parameters(false)];
      double* weights2 = new double[actor_bn_dontlearn.number_of_parameters(false)];
      actor.copyWeightsTo(weights, false);
      actor_bn_dontlearn.copyWeightsTo(weights2, false);
      diff = 0;
      for(uint i=0; i<actor.number_of_parameters(false); i++)
        diff += fabs(weights[i] - weights2[i]);
      EXPECT_GT(diff, 2.f);

      delete[] weights;
      delete[] weights2;

//       now check that classic copy is enough to take advantage of bn
      auto all_actions_outputs0 = actor.computeOutBatch(sensors);
      MLP actor_bn_dontlearn_after(actor, false, ::caffe::Phase::TEST);
      auto all_actions_outputs4 = actor_bn_dontlearn.computeOutBatch(sensors);
      {
        double* weights = new double[actor.number_of_parameters(false)];
        actor.copyWeightsTo(weights, false);
        actor_bn_dontlearn.copyWeightsFrom(weights, false);
        delete[] weights;
      }

      all_actions_outputs1 = actor.computeOutBatch(sensors);
      all_actions_outputs2 = actor_bn_dontlearn.computeOutBatch(sensors);
      auto all_actions_outputs5 = actor_bn_dontlearn_after.computeOutBatch(sensors);

      diff = 0;
      double diff2 = 0;
      double diff3 = 0;
      double diff4 = 0;
      for(uint i=0; i<batch_size; i++) {
        diff += fabs(all_actions_outputs1->at(i) - all_actions_outputs2->at(i));
        diff2 += fabs(all_actions_outputs0->at(i) - all_actions_outputs2->at(i));
        diff3 += fabs(all_actions_outputs4->at(i) - all_actions_outputs2->at(i));
        diff4 += fabs(all_actions_outputs5->at(i) - all_actions_outputs2->at(i));
      }
//       LOG_DEBUG(diff << " " << diff2 << " " << diff3 << " " << diff4 << " " <<
//                 fabs(actor.weight_l1_norm() - actor_bn_dontlearn.weight_l1_norm()));

// conclusion : BN is not the same in learning and testing
//       EXPECT_DOUBLE_EQ(diff, 0.000f);
//       EXPECT_DOUBLE_EQ(diff2, 0.000f);
//       EXPECT_DOUBLE_EQ(diff3, 0.000f);
      EXPECT_DOUBLE_EQ(diff4, 0.000f);

      delete all_actions_outputs1;
      delete all_actions_outputs2;
      delete all_actions_outputs0;
      delete all_actions_outputs4;

//       if it's in testing then is output different than training
      EXPECT_GT(fabs(actor.weight_l1_norm() - actor_bn_dontlearn.weight_l1_norm()), 0.01);
    }
  }
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2010 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "sparse.h"
#include <Eigen/SparseExtra>

#ifdef EIGEN_CHOLMOD_SUPPORT
#include <Eigen/CholmodSupport>
#endif

template<typename Scalar> void sparse_ldlt(int rows, int cols)
{
  static bool odd = true;
  odd = !odd;
  double density = (std::max)(8./(rows*cols), 0.01);
  typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
  typedef Matrix<Scalar,Dynamic,1> DenseVector;

  SparseMatrix<Scalar> m2(rows, cols);
  DenseMatrix refMat2(rows, cols);

  DenseVector b = DenseVector::Random(cols);
  DenseVector refX(cols), x(cols);

  initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, 0, 0);

  SparseMatrix<Scalar> m3 = m2 * m2.adjoint(), m3_lo(rows,rows), m3_up(rows,rows);
  DenseMatrix refMat3 = refMat2 * refMat2.adjoint();

  refX = refMat3.template selfadjointView<Upper>().ldlt().solve(b);
  typedef SparseMatrix<Scalar,Upper|SelfAdjoint> SparseSelfAdjointMatrix;
  x = b;
  SparseLDLT<SparseSelfAdjointMatrix> ldlt(m3);
  if (ldlt.succeeded())
    ldlt.solveInPlace(x);
  else
    std::cerr << "warning LDLT failed\n";

  VERIFY_IS_APPROX(refMat3.template selfadjointView<Upper>() * x, b);
  VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: default");

#ifdef EIGEN_CHOLMOD_SUPPORT
  {
    x = b;
    SparseLDLT<SparseSelfAdjointMatrix, Cholmod> ldlt2(m3);
    if (ldlt2.succeeded())
    {
      ldlt2.solveInPlace(x);
      VERIFY_IS_APPROX(refMat3.template selfadjointView<Upper>() * x, b);
      VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: cholmod solveInPlace");

      x = ldlt2.solve(b);
      VERIFY_IS_APPROX(refMat3.template selfadjointView<Upper>() * x, b);
      VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: cholmod solve");
    }
    else
      std::cerr << "warning LDLT failed\n";
  }
#endif

  // new Simplicial LLT

  // new API
  {
    SparseMatrix<Scalar> m2(rows, cols);
    DenseMatrix refMat2(rows, cols);

    DenseVector b = DenseVector::Random(cols);
    DenseVector ref_x(cols), x(cols);
    DenseMatrix B = DenseMatrix::Random(rows,cols);
    DenseMatrix ref_X(rows,cols), X(rows,cols);

    initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, 0, 0);

    for(int i=0; i<rows; ++i)
      m2.coeffRef(i,i) = refMat2(i,i) = internal::abs(internal::real(refMat2(i,i)));

    SparseMatrix<Scalar> m3 = m2 * m2.adjoint(), m3_lo(rows,rows), m3_up(rows,rows);
    DenseMatrix refMat3 = refMat2 * refMat2.adjoint();

    m3_lo.template selfadjointView<Lower>().rankUpdate(m2,0);
    m3_up.template selfadjointView<Upper>().rankUpdate(m2,0);

    // with a single vector as the rhs
    ref_x = refMat3.template selfadjointView<Lower>().llt().solve(b);

    x = SimplicialCholesky<SparseMatrix<Scalar>, Lower>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(b);
    VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, lower, single dense rhs");

    x = SimplicialCholesky<SparseMatrix<Scalar>, Upper>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(b);
    VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, upper, single dense rhs");

    x = SimplicialCholesky<SparseMatrix<Scalar>, Lower>(m3_lo).solve(b);
    VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, lower only, single dense rhs");

    x = SimplicialCholesky<SparseMatrix<Scalar>, Upper>(m3_up).solve(b);
    VERIFY(ref_x.isApprox(x,test_precision<Scalar>()) && "SimplicialCholesky: solve, upper only, single dense rhs");

    // with multiple rhs
    ref_X = refMat3.template selfadjointView<Lower>().llt().solve(B);

    X = SimplicialCholesky<SparseMatrix<Scalar>, Lower>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(B);
    VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, lower, multiple dense rhs");

    X = SimplicialCholesky<SparseMatrix<Scalar>, Upper>().setMode(odd ? SimplicialCholeskyLLt : SimplicialCholeskyLDLt).compute(m3).solve(B);
    VERIFY(ref_X.isApprox(X,test_precision<Scalar>()) && "SimplicialCholesky: solve, full storage, upper, multiple dense rhs");

    // with a sparse rhs
//     SparseMatrix<Scalar> spB(rows,cols), spX(rows,cols);
//     B.diagonal().array() += 1;
//     spB = B.sparseView(0.5,1);
//
//     ref_X = refMat3.template selfadjointView<Lower>().llt().solve(DenseMatrix(spB));
//
//     spX = SimplicialCholesky<SparseMatrix<Scalar>, Lower>(m3).solve(spB);
//     VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
//
//     spX = SimplicialCholesky<SparseMatrix<Scalar>, Upper>(m3).solve(spB);
//     VERIFY(ref_X.isApprox(spX.toDense(),test_precision<Scalar>()) && "LLT: cholmod solve, multiple sparse rhs");
  }

//   for(int i=0; i<rows; ++i)
//     m2.coeffRef(i,i) = refMat2(i,i) = internal::abs(internal::real(refMat2(i,i)));
//
//   refX = refMat2.template selfadjointView<Upper>().ldlt().solve(b);
//   typedef SparseMatrix<Scalar,Upper|SelfAdjoint> SparseSelfAdjointMatrix;
//   x = b;
//   SparseLDLT<SparseSelfAdjointMatrix> ldlt(m2);
//   if (ldlt.succeeded())
//     ldlt.solveInPlace(x);
//   else
//     std::cerr << "warning LDLT failed\n";
//
//   VERIFY_IS_APPROX(refMat2.template selfadjointView<Upper>() * x, b);
//   VERIFY(refX.isApprox(x,test_precision<Scalar>()) && "LDLT: default");

}

void test_sparse_ldlt()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1(sparse_ldlt<double>(8, 8) );
    int s = internal::random<int>(1,300);
    CALL_SUBTEST_2(sparse_ldlt<std::complex<double> >(s,s) );
    CALL_SUBTEST_1(sparse_ldlt<double>(s,s) );
  }
}


#ifndef PA_STAGES_PARTITIONER_HPP
#define PA_STAGES_PARTITIONER_HPP

#include <array>
#include <optional>

#include <boost/mpi.hpp>

#include <pa/math/types.hpp>
#include <pa/export.hpp>

namespace pa
{
class PA_EXPORT partitioner
{
public:
  struct PA_EXPORT rank_info
  {
    rank_info(integer rank, const ivector3& grid_size, const ivector3& block_size);

    integer  rank               = 0 ;
    ivector3 multi_rank         = {};
    ivector3 offset             = {};
    ivector3 ghosted_block_size = {};
  };

  explicit partitioner  (boost::mpi::communicator* communicator);
  partitioner           (const partitioner&  that) = default;
  partitioner           (      partitioner&& temp) = default;
 ~partitioner           ()                         = default;
  partitioner& operator=(const partitioner&  that) = default;
  partitioner& operator=(      partitioner&& temp) = default;

  void                                           set_domain_size   (const ivector3& domain_size);

  boost::mpi::communicator*                      communicator      ();
  const ivector3&                                domain_size       () const;
  const ivector3&                                grid_size         () const;
  const ivector3&                                block_size        () const;

  const std::optional<rank_info>&                local_rank_info   () const;
  const std::array<std::optional<rank_info>, 6>& neighbor_rank_info() const;

protected:
  boost::mpi::communicator*               communicator_       = nullptr;

  ivector3                                domain_size_        = {};
  ivector3                                grid_size_          = {};
  ivector3                                block_size_         = {};

  std::optional<rank_info>                local_rank_info_    = {};
  std::array<std::optional<rank_info>, 6> neighbor_rank_info_ = {};
};
}

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file NonlinearISAM-inl.h
 * @date Jan 19, 2010
 * @author Viorela Ila and Richard Roberts
 */

#include <gtsam/nonlinear/NonlinearISAM.h>

#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Ordering.h>

#include <boost/foreach.hpp>

#include <iostream>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const {
  isam_.saveGraph(s, keyFormatter);
}

/* ************************************************************************* */
void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& initialValues) {

  if(newFactors.size() > 0) {

    // Reorder and relinearize every reorderInterval updates
    if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
      reorder_relinearize();
      reorderCounter_ = 0;
    }

    factors_.push_back(newFactors);

    // Linearize new factors and insert them
    // TODO: optimize for whole config?
    linPoint_.insert(initialValues);

    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_);

    // Update ISAM
    isam_.update(*linearizedNewFactors, eliminationFunction_);
  }
}

/* ************************************************************************* */
void NonlinearISAM::reorder_relinearize() {

//  cout << "Reordering, relinearizing..." << endl;

  if(factors_.size() > 0) {
    // Obtain the new linearization point
    const Values newLinPoint = estimate();

    isam_.clear();

    // Just recreate the whole BayesTree
    // TODO: allow for constrained ordering here
    // TODO: decouple relinearization and reordering to avoid
    isam_.update(*factors_.linearize(newLinPoint), eliminationFunction_);

    // Update linearization point
    linPoint_ = newLinPoint;
  }
}

/* ************************************************************************* */
Values NonlinearISAM::estimate() const {
  if(isam_.size() > 0)
    return linPoint_.retract(isam_.optimize());
  else
    return linPoint_;
}

/* ************************************************************************* */
Matrix NonlinearISAM::marginalCovariance(Key key) const {
  return isam_.marginalCovariance(key);
}

/* ************************************************************************* */
void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) const {
  cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
  isam_.print("GaussianISAM:\n");
  linPoint_.print("Linearization Point:\n", keyFormatter);
  factors_.print("Nonlinear Graph:\n", keyFormatter);
}

/* ************************************************************************* */
void NonlinearISAM::printStats() const {
  isam_.getCliqueData().getStats().print();
}

/* ************************************************************************* */

}///\ namespace gtsam


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/log.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( log_real,  NT2_REAL_TYPES)
{
  using nt2::log;
  using nt2::tag::log_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<log_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), nt2::Mone<T>())), cT(nt2::Inf<T>(), -nt2::Pi<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(nt2::Inf<cT>()), cT(nt2::Inf<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(nt2::Minf<cT>()), cT(nt2::Inf<T>(), nt2::Pi<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan<T>(), nt2::Zero<T>())),  cT(nt2::Nan<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan<T>(), nt2::Nan<T>())),  cT(nt2::Nan<T>(), nt2::Nan<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(nt2::Zero<cT>()), cT(nt2::Minf<r_t>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(nt2::Mzero<cT>()), cT(nt2::Minf<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Zero<T>(),  nt2::Zero<T>())),  cT(nt2::Minf<T>(), nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), nt2::Zero<T>())),  cT(nt2::Inf<T>(), nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), nt2::Zero<T>())),  cT(nt2::Inf<T>(), nt2::Pi<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), nt2::Zero<T>())),  cT(nt2::Inf <T>(), nt2::Zero <T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::One  <T>(), nt2::Inf <T>())), cT(nt2::Inf<T>(), nt2::Pio_2<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::One  <T>(), nt2::Nan <T>())), cT(nt2::Nan<T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Inf<T>(), nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), nt2::One<T>())),  cT(nt2::Inf<T>(), nt2::Pi<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), nt2::One <T>())), cT(nt2::Inf <T>(), nt2::Zero <T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), nt2::Inf<T>())),  cT(nt2::Inf <T>(), 3*nt2::Pi<T>()/4), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), nt2::Inf<T>())),  cT(nt2::Inf <T>(), nt2::Pi<T>()/4), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), nt2::Nan<T>())),  cT(nt2::Inf <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), nt2::Nan<T>())),  cT(nt2::Inf <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), nt2::One<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), nt2::Mone<T>())), cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), nt2::Inf<T>())),  cT(nt2::Inf <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), nt2::Nan<T>())),  cT(nt2::Nan <T>(), nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::One  <T>(), -nt2::Inf <T>())), cT(nt2::Inf<T>(), -nt2::Pio_2<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::One  <T>(), -nt2::Nan <T>())), cT(nt2::Nan<T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), -nt2::One <T>())), cT(nt2::Inf<T>(), -nt2::Zero<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), -nt2::One<T>())),  cT(nt2::Inf<T>(), -nt2::Pi<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), -nt2::One <T>())), cT(nt2::Inf <T>(), -nt2::Zero <T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), -nt2::Inf<T>())),  cT(nt2::Inf <T>(), -3*nt2::Pi<T>()/4), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), -nt2::Inf<T>())),  cT(nt2::Inf <T>(), -nt2::Pi<T>()/4), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Minf <T>(), -nt2::Nan<T>())),  cT(nt2::Inf <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Inf  <T>(), -nt2::Nan<T>())),  cT(nt2::Inf <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), -nt2::One<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), -nt2::Mone<T>())), cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), -nt2::Inf<T>())),  cT(nt2::Inf <T>(), -nt2::Nan<T>()), 0.75);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Nan  <T>(), -nt2::Nan<T>())),  cT(nt2::Nan <T>(), -nt2::Nan<T>()), 0.75);
#endif
  NT2_TEST_ULP_EQUAL(nt2::log(nt2::Mone<cT>()), cT(nt2::Zero<T>(), nt2::Pi<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(nt2::One<cT>()), cT(nt2::Zero<r_t>()), 0.5);
  NT2_TEST_ULP_EQUAL(nt2::log(cT(nt2::Zero<T>(), nt2::One<T>())),cT(nt2::Zero<T>(), nt2::Pio_2<T>()), 0.5);
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2016.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg$
// $Authors: Erhan Kenar, Holger Franken $
// --------------------------------------------------------------------------

#include <OpenMS/CONCEPT/ClassTest.h>
#include <OpenMS/test_config.h>

///////////////////////////
#include <OpenMS/FILTERING/SMOOTHING/LowessSmoothing.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
///////////////////////////

using namespace OpenMS;
using namespace std;

double targetFunction(double x)
{
  return 10 + 20*x + 40*x*x;
}

START_TEST(LowessSmoothing, "$Id$")

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

LowessSmoothing* ptr = 0;
LowessSmoothing* null_ptr = 0;
START_SECTION(LowessSmoothing())
{
	ptr = new LowessSmoothing();
	TEST_NOT_EQUAL(ptr, null_ptr)
}
END_SECTION

START_SECTION(~LowessSmoothing())
{
	delete ptr;
}
END_SECTION

/////
std::vector<double> x, y, y_noisy, out;

//exact data
for (double i = 1.0; i <= 20.0; i += 1.0)
{
    x.push_back(i);
    y.push_back(targetFunction(i));
}

//noisy data
// make some noise

boost::random::mt19937 rnd_gen_;
for (Size i = 0; i < y.size(); ++i)
{
  boost::normal_distribution<float> udist (y.at(i), 0.05);
  y_noisy.push_back( udist(rnd_gen_) );
}

LowessSmoothing lowsmooth;
Param lowpar;
lowpar.setValue("window_size", 15);

TOLERANCE_RELATIVE(1.0004);
TOLERANCE_ABSOLUTE(0.07);
START_SECTION(void smoothData(const DoubleVector&, const DoubleVector&, DoubleVector&))
{
    y.push_back(-1.0);
    TEST_EXCEPTION(Exception::InvalidValue, lowsmooth.smoothData(x, y, out));
    y.pop_back();
    out.clear();

    lowsmooth.smoothData(x, y, out);
    Size idx = 1;
    for (Size i = 0; i < out.size(); ++i, ++idx)
    {
        TEST_REAL_SIMILAR(out[i], targetFunction(idx));
    }

    out.clear();
    lowsmooth.setParameters(lowpar);
    lowsmooth.smoothData(x, y, out);
    idx = 1;
    for (Size i = 0; i < out.size(); ++i, ++idx)
    {
        TEST_REAL_SIMILAR(out[i], targetFunction(idx));
    }

    out.clear();
    lowsmooth.smoothData(x, y_noisy, out);
    idx = 1;
    for (Size i = 0; i < out.size(); ++i, ++idx)
    {
        TEST_REAL_SIMILAR(out[i], targetFunction(idx));
    }

}
END_SECTION

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
END_TEST


// MIT License

// Copyright (c) 2021 Daniel Kowalczyk

// 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.

#ifndef ZEROHALFSYSTEM_H
#define ZEROHALFSYSTEM_H

#include <boost/dynamic_bitset/dynamic_bitset.hpp>
#include <cstddef>
#include <vector>
#include "CoinPackedVector.hpp"
#include "ModernDD/NodeBddStructure.hpp"
#include "NodeBdd.hpp"

class ZeroHalfSystem {
    using MatrixDouble = std::vector<std::vector<double>>;
    using VectorDouble = std::vector<double>;
    struct ConstraintData {
        CoinPackedVector        constraint{};
        double                  slack{};
        boost::dynamic_bitset<> row_indices{};
        int                     rhs{};
    };

   public:
    ZeroHalfSystem() = default;
    ZeroHalfSystem(const MatrixDouble& A,
                   const VectorDouble& b,
                   const VectorDouble& _x);
    ZeroHalfSystem(const DdStructure<NodeBdd>& bdd);
    ZeroHalfSystem(ZeroHalfSystem&&) = default;
    ZeroHalfSystem(const ZeroHalfSystem&) = default;
    ~ZeroHalfSystem() = default;

    auto operator=(ZeroHalfSystem&&) -> ZeroHalfSystem& = default;
    auto operator=(const ZeroHalfSystem&) -> ZeroHalfSystem& = default;

   private:
    std::vector<CoinPackedVector>        assignment_constraints;
    std::vector<double>                  assignment_slack;
    std::vector<boost::dynamic_bitset<>> assignment_index;
    std::vector<int>                     assignment_rhs;

    std::vector<CoinPackedVector>        flow_constraints;
    std::vector<double>                  flow_slack;
    std::vector<boost::dynamic_bitset<>> flow_index;
    std::vector<int>                     flow_rhs;

    int nb_assignments{};
    int nb_nz_vertices{};
    int nb_nz_edges{};

    std::vector<std::vector<int>> A_bar;
    std::vector<int>              b_bar;
    std::vector<double>           x_star;
    std::vector<double>           slack;

    std::vector<boost::dynamic_bitset<>> row_index;

    size_t nb_rows{};
    size_t nb_columns{};

    static constexpr double HALF = 2.0;
    static constexpr double EPS = 1e-6;

    void remove_row(size_t _row);
    void remove_col(size_t _col);

    void reduce_system();
    void reduce_gauss();
    void evaluate_rows(const std::vector<int>& _rows);

    void add_to_row(size_t i, size_t j);

   public:
    void generate_cuts();
};

#endif  // ZEROHALFSYSTEM_H


// Read Abaqus input file
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <Eigen/Eigen>
#include "Abaqus_read.hpp"
#include "mesh_Abaqus_multi_faults.hpp"
#include "share_header.hpp"
using namespace Eigen;
//void Abaqus_read(std::string & fname);

int main (int argc, char *argv[])
{
    std::string filename = "abaqus_cpp_5degree_100m_10km_4km.inp";
    std::cout<<filename<<std::endl;
    double x_min = -30e3;
    double x_max = 30e3;
    double y_min = -0.5e3;
    double y_max = 0.5e3;
    int dim = 2.0;
    double dx = 3.125;
    double dy = 3.125;
    int nx_el = (x_max-x_min)/dx;
    int ny = (y_max-y_min)/dy;
    //ArrayXi BIE_top_surf_nodes = ArrayXi::Zero((nx_el+1),1);
    //ArrayXi BIE_bot_surf_nodes = ArrayXi::Zero((nx_el+1),1);
    std::vector<int> BIE_top, BIE_bot;
    MatrixXd Nodes;
    MatrixXi_rm Element;
    std::vector<std::vector<int>> fault_nodes(2);
    mesh_Abaqus_multi_faults(filename, Nodes, Element, BIE_top, BIE_bot,fault_nodes);
    std::cout<<Nodes<<std::endl;
    std::cout<<"Node size=" << Nodes.rows()<<std::endl;
    std::cout<<Element<<std::endl;
    std::cout<<"Element size="<<Element.rows()<<std::endl;
    return 1;
}


#define BOOST_TEST_MODULE como_Model
#include <boost/test/unit_test.hpp>

#include "sycomore/como/Model.h"
#include "sycomore/GridScanner.h"
#include "sycomore/Species.h"
#include "sycomore/TimeInterval.h"

void test_magnetization(
    sycomore::ComplexMagnetization const & m1,
    sycomore::ComplexMagnetization const & m2, sycomore::Real tolerance=1e-9)
{
    BOOST_TEST(m1.p.real() == m2.p.real(), boost::test_tools::tolerance(tolerance));
    BOOST_TEST(m1.p.imag() == m2.p.imag(), boost::test_tools::tolerance(tolerance));

    BOOST_TEST(m1.z == m2.z, boost::test_tools::tolerance(tolerance));

    BOOST_TEST(m1.m.real() == m2.m.real(), boost::test_tools::tolerance(tolerance));
    BOOST_TEST(m1.m.imag() == m2.m.imag(), boost::test_tools::tolerance(tolerance));
}

BOOST_AUTO_TEST_CASE(Constructor)
{
    using namespace sycomore::units;

    sycomore::Species const species(1_s, 0.1_s);
    sycomore::TimeInterval const echo(10_ms);

    sycomore::como::Model const model(species, {0,0,1}, {{"echo", echo}});

    BOOST_TEST((
        model.dimensions() == std::map<std::string, size_t>{{"echo", 0}}));
    BOOST_TEST((
        model.time_intervals() == std::vector<sycomore::TimeInterval>{{echo}}));

    BOOST_TEST((model.magnetization().origin() <= sycomore::Index{0}));
    BOOST_TEST((model.magnetization().shape() >= sycomore::Shape{1}));
}

BOOST_AUTO_TEST_CASE(Pulse)
{
    using namespace sycomore::units;

    sycomore::Species const species(1_s, 0.1_s);
    sycomore::como::Model model(species, {0,0,1}, {{"dummy", {0*s}}});

    sycomore::Pulse const pulse{41_deg, 27_deg};
    model.apply_pulse(pulse);

    auto && grid = model.magnetization();

    BOOST_TEST((grid.origin() <= sycomore::Index{0}));
    BOOST_TEST((grid.shape() >= sycomore::Shape{1}));

    for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
    {
        auto & m = grid[index.first];
        if(index.first == sycomore::Index{0})
        {
            // Values from CoMoTk@2b0ef02
            test_magnetization(
                m,
                {
                    {0.210607912662250, -0.413341301933443},
                    0.754709580222772,
                    {0.210607912662250, +0.413341301933443}
                });
        }
        else
        {
            test_magnetization(m, sycomore::ComplexMagnetization::zero);
        }
    }
}

BOOST_AUTO_TEST_CASE(TimeInterval, *boost::unit_test::tolerance(1e-9))
{
    using namespace sycomore::units;

    sycomore::Species const species{std::log(2)*Hz, std::log(2)*Hz};
    // Dummy time intervals to build a 2D model
    sycomore::como::Model model(species, {0, 0, 1}, {{"foo", {1*s}}, {"bar", {1*s}}});

    // Resulting complex magnetization: 1/2, sqrt(2)/2, 1/2
    model.apply_pulse({45_deg, 90_deg});

    // Transverse magnetization is divided by 2, and becomes 1/4; longitudinal
    // magnetization becomes 1/2*sqrt(2)/2 + (1-1/2) == 1/2*(1+sqrt(2)/2)
    // This value includes relaxation and repolarization effects with
    // m_eq == (0,0,1)
    model.apply_time_interval("foo");

    {
        auto && grid = model.magnetization();

        BOOST_TEST((model.magnetization().origin() <= sycomore::Index{-1, 0}));
        BOOST_TEST((model.magnetization().shape() >= sycomore::Shape{3, 0}));

        for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
        {
            auto && m = grid[index.first];
            if(index.first == sycomore::Index{-1,0})
            {
                test_magnetization(m, {0., 0., 0.25});
            }
            else if(index.first == sycomore::Index{0,0})
            {
                test_magnetization(m, {0., 1./2.*(1+std::sqrt(2.)/2.), 0.});
            }
            else if(index.first == sycomore::Index{1,0})
            {
                test_magnetization(m, {0.25, 0., 0.});
            }
            else
            {
                test_magnetization(m, sycomore::ComplexMagnetization::zero);
            }
        }
    }

    // Same modifications as before: transverse magnetization becomes 1/8,
    // longitudinal magnetization becomes 1/2 + 1/4*(1+sqrt(2)/2)
    // Configurations (0, -1), (1, -1), (-1, 0), (1, 0), (-1, 1), and (0, 1) are
    // unoccupied
    model.apply_time_interval("bar");

    {
        auto && grid = model.magnetization();

        BOOST_TEST((model.magnetization().origin() <= sycomore::Index{-1, -1}));
        BOOST_TEST((model.magnetization().shape() >= sycomore::Shape{3, 3}));

        for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
        {
            auto && m = grid[index.first];
            if(index.first == sycomore::Index{-1,-1})
            {
                test_magnetization(m, {0., 0., 0.125});
            }
            else if(index.first == sycomore::Index{0,0})
            {
                test_magnetization(m, {0., 0.5 + 0.25*(1+std::sqrt(2.)/2.), 0.});
            }
            else if(index.first == sycomore::Index{1,1})
            {
                test_magnetization(m, {0.125, 0., 0.});
            }
            else
            {
                test_magnetization(m, sycomore::ComplexMagnetization::zero);
            }
        }
    }

    auto isochromat = model.isochromat();
    BOOST_TEST(isochromat[0] == 0.125*std::sqrt(2.));
    BOOST_TEST(isochromat[1] == 0.);
    BOOST_TEST(isochromat[2] == 0.5 + 0.25*(1+std::sqrt(2.)/2.));

    isochromat = model.isochromat({{0,0}, {-1,-1}});
    BOOST_TEST(isochromat[0] == 0.125*std::sqrt(2.)/2.);
    BOOST_TEST(isochromat[1] == 0.);
    BOOST_TEST(isochromat[2] == 0.5 + 0.25*(1+std::sqrt(2.)/2.));
}

BOOST_AUTO_TEST_CASE(Diffusion)
{
    using namespace sycomore::units;

    sycomore::Species const species{0_Hz, 0_Hz, 1_um*um/ms};
    sycomore::TimeInterval const echo{500_ms, 0.1*rad/um};

    sycomore::como::Model model(species, {0,0,1}, {{"echo", echo}});

    model.apply_pulse({40_deg, 0_deg});
    model.apply_time_interval("echo");

    // model = CoMoTk;
    // model.R1 = 0;
    // model.R2 = 0;
    // model.D = 1;
    // model.init_configuration([0;0;1]);
    // model.RF(deg2rad(40), 0);
    // model.time(1, 'tau', 500, 'p', 0.1);
    {
        auto && grid = model.magnetization();

        BOOST_TEST((model.magnetization().origin() <= sycomore::Index{-1}));
        BOOST_TEST((model.magnetization().shape() >= sycomore::Shape{3}));

        for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
        {
            auto && m = grid[index.first];
            if(index.first == sycomore::Index{-1})
            {
                test_magnetization(m, {0., 0., {0, 0.003062528150606}});
            }
            else if(index.first == sycomore::Index{0})
            {
                test_magnetization(m, {0., 0.766044443118978, 0.});
            }
            else if(index.first == sycomore::Index{1})
            {
                test_magnetization(m, {{0., -0.003062528150606}, 0., 0.});
            }
            else
            {
                test_magnetization(m, sycomore::ComplexMagnetization::zero);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(CleanUp)
{
    using namespace sycomore::units;

    sycomore::Species const species{std::log(2)*Hz, std::log(2)*Hz};
    // Dummy time intervals to build a 2D model
    sycomore::como::Model model(
        species, {0, 0, 1}, {{"short", {1*s}}, {"long", {2*s}}});
    model.set_epsilon(1.5*1./(1<<4));

    // Resulting complex magnetization: 1/2, sqrt(2)/2, 1/2
    model.apply_pulse({45_deg, 90_deg});
    model.apply_time_interval("long");

    // All occupied configurations are above the threshold
    {
        auto && grid = model.magnetization();

        BOOST_TEST((model.magnetization().origin() <= sycomore::Index{0, -1}));
        BOOST_TEST((model.magnetization().shape() >= sycomore::Shape{0, 1}));

        for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
        {
            auto && m = grid[index.first];
            if(index.first == sycomore::Index{0,-1})
            {
                test_magnetization(m, {0., 0., 0.125});
            }
            else if(index.first == sycomore::Index{0,0})
            {
                test_magnetization(m, {0., 0.25*std::sqrt(2.)/2. + 1-0.25, 0.});
            }
            else if(index.first == sycomore::Index{0,1})
            {
                test_magnetization(m, {0.125, 0., 0.});
            }
            else
            {
                test_magnetization(m, sycomore::ComplexMagnetization::zero);
            }
        }
    }

    model.apply_time_interval("short");

    // Magnetizations on non-zero configurations fall below the threshold
    {
        auto && grid = model.magnetization();

        BOOST_TEST((model.magnetization().origin() <= sycomore::Index{0, -3}));
        BOOST_TEST((model.magnetization().shape() >= sycomore::Shape{0, 3}));

        for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
        {
            auto && m = grid[index.first];
            if(index.first == sycomore::Index{0,0})
            {
                test_magnetization(
                    m, {0., 0.5*(0.25*std::sqrt(2.)/2. + 1-0.25)+(1-0.5), 0.});
            }
            else
            {
                test_magnetization(m, sycomore::ComplexMagnetization::zero);
            }
        }
    }
}


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_linear_dae.hpp>
#include <test/unit/math/rev/functor/dae_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using linear_dae_test_types
    = boost::mp11::mp_product<ode_test_tuple, ::testing::Types<dae_functor>,
                              ::testing::Types<double>,  // t
                              ::testing::Types<double>,  // yy0
                              ::testing::Types<double>,  // yp0
                              ::testing::Types<double>   // theta
                              >;

TYPED_TEST_SUITE_P(linear_dae_test);
TYPED_TEST_P(linear_dae_test, analytical_value) {
  double theta = 3.0;
  this->theta = theta;
  Eigen::VectorXd x(1);
  x << theta;
  const double pi = 3.14159265358979323846;
  const double t = 2.3 * pi;
  this->dae_sol.ts = std::vector<double>{t};
  this->test_analytical(this->dae_sol, this->analy_sol_functor(), 1e-6, x, t);
}

TYPED_TEST_P(linear_dae_test, finite_diff) { this->test_fd_dv(1.e-3, 5e-6); }

REGISTER_TYPED_TEST_SUITE_P(linear_dae_test, analytical_value, finite_diff);
INSTANTIATE_TYPED_TEST_SUITE_P(StanDAE, linear_dae_test, linear_dae_test_types);


#include "builder.h"
#include "include/utils/matrix_utils.h"

#include <Eigen/Core>
#include <opencv2/opencv.hpp>

#include <memory>   // for make_shared

#include <iostream>

// //PCL
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>

using namespace Eigen;
using namespace std;

void Builder::processFrame(cv::Mat &rgb, cv::Mat &depth, Eigen::VectorXd &pose, double depth_thresh){
    if(!mbInitialized) {
        std::cerr << "Please initialize the builder first." << std::endl;
        return;
    }
    PointCloudPCL::Ptr pLocalPointCloud = image2PointCloud(rgb, depth, depth_thresh);

    PointCloudPCL::Ptr pGlobalPointCloud = transformToWolrd(pLocalPointCloud, pose);   // pose: Twc
    mpCurrentMap = pGlobalPointCloud;

    // add to map
    addPointCloudToMap(pGlobalPointCloud);

    return;
}

void Builder::setCameraIntrinsic(Matrix3d &calibMat, double scale){
    mmCalib = calibMat;
    mdScale = scale;

    mbInitialized = true;
}

Builder::Builder():mbInitialized(false), mpMap(new PointCloudPCL){

}

PointCloudPCL::Ptr Builder::image2PointCloud( cv::Mat& rgb, cv::Mat& depth, double depth_thresh)
{
    PointCloudPCL::Ptr temp_cloud ( new PointCloudPCL );

    Camera camera;
    getCameraParam(mmCalib, camera);
    for (int m = 0; m < depth.rows; m++)
        for (int n=0; n < depth.cols; n++)
        {
            ushort * ptd = depth.ptr<ushort>(m);
            ushort d = ptd[n];
            if (d == 0)
                continue;

            PointT p;
            p.z = double(d) / mdScale;

            if( p.z < 0.5 || p.z > depth_thresh )
                continue;

            p.x = (n - camera.cx) * p.z / camera.fx;
            p.y = (m - camera.cy) * p.z / camera.fy;

            //  B G R
            p.b = rgb.ptr<uchar>(m)[n*3];
            p.g = rgb.ptr<uchar>(m)[n*3+1];
            p.r = rgb.ptr<uchar>(m)[n*3+2];

            temp_cloud->points.push_back( p );
        }

    temp_cloud->height = 1;
    temp_cloud->width = temp_cloud->points.size();
    temp_cloud->is_dense = true;
    return temp_cloud;
}

void Builder::getCameraParam(Matrix3d &calib, Camera &cam){
    // kalib matrix:
    // fx s cx
    // 0 fy cy
    // 0 0 1
    cam.fx = calib(0,0);
    cam.fy = calib(1,1);
    cam.cx = calib(0,2);
    cam.cy = calib(1,2);
}

PointCloudPCL::Ptr Builder::getMap()
{
    return mpMap;
}

PointCloudPCL::Ptr Builder::getCurrentMap()
{
    return mpCurrentMap;
}

PointCloudPCL::Ptr Builder::transformToWolrd( PointCloudPCL::Ptr& pPointCloud, VectorXd &pose){
    // pose: Tcw
    // tx ty tz (3 floats) give the position of
    // the optical center of the color camera
    // with respect to the world origin as defined by the motion capture system.
    PointCloudPCL::Ptr transformed_cloud(new PointCloudPCL);
    Matrix4d transform = getTransformFromVector(pose);
    std::cout << "transform " << transform << std::endl;
    pcl::transformPointCloud (*pPointCloud, *transformed_cloud, transform);

    return transformed_cloud;
}

void Builder::addPointCloudToMap(PointCloudPCL::Ptr pPointCloud){
    *mpMap = *mpMap + *pPointCloud;
}

void Builder::saveMap(const string& path){
  if(mpMap->points.size() < 1) return;
  pcl::io::savePCDFileASCII(path, *mpMap);
  std::cerr << "Saved " << mpMap->points.size () << " data points to " << path << std::endl;
}

void Builder::voxelFilter(double grid_size){
    // Voxel grid downsample
    static pcl::VoxelGrid<PointT> voxel;

    double gridsize = grid_size;
    voxel.setLeafSize( gridsize, gridsize, gridsize );
    voxel.setInputCloud( mpMap );
    PointCloudPCL::Ptr tmp( new PointCloudPCL() );
    voxel.filter( *tmp );
    pcl::copyPointCloud(*tmp, *mpMap);
}


// #pragma once

// #include <boost/multiprecision/cpp_int.hpp>
// #include <boost/random.hpp>

// #include <string>

// using namespace boost::multiprecision;
// using namespace boost::random;

// typedef independent_bits_engine<mt19937, 512, uint512_t> generator512_type;

// class Random
// {
// public:
//     Random()
//     {
//         generator512_type gen512;
//         number_ = gen512();
//     }
//     std::string get_random_number()
//     {
//         std::string str;
//         std::stringstream stream;
//         stream << std::hex << number_;
//         str = stream.str();
//         return str;
//     }
// private:
//     uint512_t number_;
// };