#include "imu.h"
#include "global_config.h"
#include "SPI.h"
#include "ricardo_pins.h"
#include "Storage/systemstatus.h"
#include "Storage/logController.h"
#include "flags.h"
#include "SparkFunLSM9DS1.h"

#include "sensorStructs.h"

#include "Preferences.h"

#include <Eigen/Core>

Imu::Imu(SPIClass* spi, SystemStatus* systemstatus,LogController* logcontroller,raw_measurements_t* raw_data):
    _spi(spi),
    _systemstatus(systemstatus),
    _logcontroller(logcontroller),
    imu(spi),
    _raw_data(raw_data),
    _magCal{1,
           0,
           0,
           Eigen::Matrix3f{{1,0,0},{0,1,0},{0,0,1}},
           Eigen::Vector3f{{0,0,0}}} // default for mag biases
{};

void Imu::setup(){

    imu.setAccelScale(ACCEL_SCALE);
    //set samplerate of accel to 476Hz
    imu.settings.accel.sampleRate = 5;
    imu.settings.accel.enabled = true; // Enable accelerometer

    imu.setGyroScale(GYRO_SCALE);
    //set samplerate of gyro to 476Hz
    imu.settings.gyro.sampleRate = 5;
    imu.settings.gyro.lowPowerEnable = false;
    //imu.settings.accel.enabled = false; // Enable accelerometer
    // [HPFEnable] enables or disables the high-pass filter
    //imu.settings.gyro.HPFEnable = true; // HPF disabled
    // [HPFCutoff] sets the HPF cutoff frequency (if enabled)
    // Allowable values are 0-9. Value depends on ODR.
    // (Datasheet section 7.14)
    //imu.settings.gyro.HPFCutoff = 1; // HPF cutoff = 4Hz

    imu.setMagScale(MAG_SCALE);
    //imu.setMagScale(12);
    imu.settings.mag.XYPerformance = 3; // Ultra-high perform.
    imu.settings.mag.ZPerformance = 3; // Ultra-high perform.
    imu.settings.mag.sampleRate = 7;
    imu.settings.mag.lowPowerEnable = false;
    imu.settings.mag.operatingMode = 0; // Continuous mode
    //mag temp compensation -> this is a good thing right?
    imu.settings.mag.tempCompensationEnable = true;

    loadAccelGyroBias(); //load previously callibrated bias values from nvs
    loadMagCal(); //load mag calibration coefficents from nvs

    if (!imu.beginSPI(_SCLK,_MISO,_MOSI,ImuCs, MagCs)){
        _systemstatus->new_message(SYSTEM_FLAG::ERROR_IMU, "Unable to initialize the imu");
        return;
    };
    _logcontroller->log("IMU Initialized");

};

void Imu::update(){

    read_gyro();
    read_accel();
    read_mag();
    read_temp();

};

void Imu::read_gyro(){
    imu.readGyro(); //degrees per second
    _raw_data->gx = imu.calcGyro(imu.gx);
    _raw_data->gy = -imu.calcGyro(imu.gy);
    _raw_data->gz = imu.calcGyro(imu.gz);

}
void Imu::read_accel(){

    imu.readAccel();//g's
    _raw_data->ax = imu.calcAccel(imu.ax);
    _raw_data->ay = -imu.calcAccel(imu.ay);
    _raw_data->az = imu.calcAccel(imu.az);
 }
void Imu::read_mag(){
    imu.readMag();
    //Gauss
    float mx = -imu.calcMag(imu.mx);
    float my = -imu.calcMag(imu.my);
    float mz = imu.calcMag(imu.mz);
    Eigen::Vector3f corrected_mag = _magCal.A_1*(Eigen::Vector3f{{mx,my,mz}} - _magCal.b);
    _raw_data->mx = corrected_mag[0];
    _raw_data->my = corrected_mag[1];
    _raw_data->mz = corrected_mag[2];

}
void Imu::read_temp(){
    imu.readTemp();
    _raw_data->imu_temp = imu.temperature;
}

void Imu::calibrateAccelGyroBias(bool autocalc){
    //4.56 0.76 0.45 521 87 51
    imu.calibrate(autocalc);
    writeAccelGyroBias(); // write bias offsets to nvs
    _logcontroller->log("IMU accel gyro bias callibration complete");
}

void Imu::calibrateMagBias(bool loadIn){ // simple bias correction.
    imu.calibrateMag(loadIn);
    _logcontroller->log("IMU simple mag bias callibration complete");
}

void Imu::calibrateMagFull(MagCalibrationParameters magCal)
{
    _magCal = magCal;
    writeMagCal();
}

void Imu::writeAccelGyroBias(){
    Preferences pref;

    if (!pref.begin("IMU")){
        _logcontroller->log("nvs failed to start. Can't write calbration offsets");
        return;
    }
    //dont have time to write a new library for this
    //i dont like the way we cant log the errors to our log file as the error handling
    //is wrapped up in the preferences class
    // it shouldnt be too hard to rewrite at a later date
    if (!pref.putShort("gxBias",imu.gBiasRaw[0])){_logcontroller->log("nvs error while writing");};
    if (!pref.putShort("gyBias",imu.gBiasRaw[1])){_logcontroller->log("nvs error while writing");};
    if (!pref.putShort("gzBias",imu.gBiasRaw[2])){_logcontroller->log("nvs error while writing");};
    if (!pref.putShort("axBias",imu.aBiasRaw[0])){_logcontroller->log("nvs error while writing");};
    if (!pref.putShort("ayBias",imu.aBiasRaw[1])){_logcontroller->log("nvs error while writing");};
    if (!pref.putShort("azBias",imu.aBiasRaw[2])){_logcontroller->log("nvs error while writing");};
    //preferences end is called in destructor of preference class  as it goes out of scope

}

void Imu::loadAccelGyroBias(){
    Preferences pref;

    if (!pref.begin("IMU",true)){
        _logcontroller->log("nvs failed to start");
        return;
    }

    imu.gBiasRaw[0] = pref.getShort("gxBias");
    imu.gBiasRaw[1] = pref.getShort("gyBias");
    imu.gBiasRaw[2] = pref.getShort("gzBias");
    imu.aBiasRaw[0] = pref.getShort("axBias");
    imu.aBiasRaw[1] = pref.getShort("ayBias");
    imu.aBiasRaw[2] = pref.getShort("azBias");

}

void Imu::writeMagCal()
{
    Preferences pref;

    if (!pref.begin("IMU")){
        _logcontroller->log("nvs failed to start. Can't write calbration offsets");
        return;
    }

    if (!pref.putFloat("F",_magCal.fieldMagnitude)){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("I",_magCal.inclination)){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("D",_magCal.declination)){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A11",_magCal.A_1(0,0))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A12",_magCal.A_1(0,1))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A12",_magCal.A_1(0,2))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A21",_magCal.A_1(1,0))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A22",_magCal.A_1(1,1))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A22",_magCal.A_1(1,2))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A31",_magCal.A_1(2,0))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A32",_magCal.A_1(2,1))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("A32",_magCal.A_1(2,2))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("b1",_magCal.b(0))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("b2",_magCal.b(1))){_logcontroller->log("nvs error while writing");};
    if (!pref.putFloat("b3",_magCal.b(2))){_logcontroller->log("nvs error while writing");};

}

void Imu::loadMagCal()
{
    Preferences pref;

    if (!pref.begin("IMU",true)){
        _logcontroller->log("nvs failed to start");
        return;
    }

    _magCal.fieldMagnitude = pref.getFloat("F",1);
    _magCal.inclination = pref.getFloat("I",0);
    _magCal.declination = pref.getFloat("D",0);

    _magCal.A_1 << pref.getFloat("A11",1),pref.getFloat("A12",0),pref.getFloat("A13",0),
                   pref.getFloat("A21",0),pref.getFloat("A22",1),pref.getFloat("A23",0),
                   pref.getFloat("A31",0),pref.getFloat("A32",0),pref.getFloat("A33",1);

    _magCal.b << pref.getFloat("b1",0),pref.getFloat("b2",0),pref.getFloat("b3",0);
}


#include <random>
#include <boost/math/constants/constants.hpp>

#include <a4wd2/toolkit/waypoint_generators.h>

using mrpt::nav::TWaypoint;
using mrpt::nav::TWaypointSequence;

static constexpr double PI = boost::math::constants::pi<double>();

namespace a4wd2::toolkit
{

TWaypointSequence uniform_random_waypoint_generator::operator()(
        const odometry_provider& odometry_provider) const
{
    m_logger->debug("Request for new waypoint");

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> distance_sample(1., 3.0);
    std::uniform_real_distribution<> theta_sample(-PI, PI);

    mrpt::math::TPose2D pose;
    mrpt::math::TTwist2D twist;
    bool have_odometry = odometry_provider.get_odometry(pose, twist);

    if (have_odometry)
    {
        double theta = theta_sample(gen);
        double distance = distance_sample(gen);

        double x = pose.x + distance * std::cos(theta);
        double y = pose.y + distance * std::sin(theta);

        TWaypointSequence waypoints;
        waypoints.waypoints = {TWaypoint(x, y, 0.1, false, theta)};

        m_logger->info("New waypoint distance: {}, theta: {}", distance, theta);
        m_logger->info("Current pose: x: {}, y: {}", pose.x, pose.y);
        m_logger->info("New waypoint list: x: {}, y: {}, theta: {}", x, y, theta);
        return waypoints;
    }
    else
    {
        m_logger->warn("Don't have odometry, returning empty waypoint list");
        return TWaypointSequence{};
    }
}

}  // namespace a4wd2::toolkit


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* blockMatmul1D.cpp - Data-movement operations on arrays of slots
 */
#include <algorithm>
#include <NTL/BasicThreadPool.h>
#include "matmul.h"

// A class that implements the basic (sparse) 1D matrix-vector functions
template<class type> class blockMatmul1D_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  BlockMatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  blockMatmul1D_impl(MatMulBase& _mat, MatrixCacheType tag, long dim)
    : buildCache(tag), mat(dynamic_cast< BlockMatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    long n = ea.sizeOfDimension(dim) * ea.getDegree();
    if (buildCache==cachezzX)
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE, n));
    else if (buildCache==cacheDCRT)
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE, n));
  }

  // Extract one "column" from a matrix that was built with buildLinPolyCoeffs
  bool shiftedColumnInDiag(zzX& zpoly, long f,
                           const std::vector< std::vector<RX> >& diag)
  {
    // extract "column" with index f and store it in cvev
    bool zero = true;
    vector<RX> cvec(ea.size());
    for (long j = 0; j < ea.size(); j++) {
      cvec[j] = diag[j][f];
      if (!NTL::IsZero(cvec[j])) zero = false;
    }
    if (zero) { // all are zeros
      zpoly.kill();
      return true;
    }
    ea.encode(zpoly, cvec);

    if (f>0) {
      long p = ea.getContext().zMStar.getP();
      long m = ea.getContext().zMStar.getM();
      long d = ea.getDegree();
      long exp = PowerMod(mcMod(p, m), d-f, m); // apply inverse automorphism
      const auto& F = ea.getTab().getPhimXMod();
      RX rpoly1, rpoly2;

      convert(rpoly1, zpoly);
      plaintextAutomorph(rpoly2,rpoly1, exp, m, F);
      convert(zpoly, rpoly2);
    }
    return false;
  }

  // Process a single block diagonal with index idx along dimenssion dim,
  // making calls to the get(i,j) method and storing the result in
  // the diag matrix (# of vectors = extenssion-degree).
  bool processDiagonal1(std::vector< std::vector<RX> >& diag,
			long dim, long i, long D, long d)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry

    mat_R entry(INIT_SIZE, d, d);
    std::vector<RX> entry1(d);
    std::vector< std::vector<RX> > tmpDiag(D);

    // Process the entries in this diagonal one at a time
    for (long j = 0; j < D; j++) { // process entry j
      bool zEntry = mat.get(entry, mcMod(j-i, D), j); // entry [j-i mod D, j]
      // get(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry = true;// zero is an empty entry too
      assert(zEntry || (entry.NumRows() == d && entry.NumCols() == d));

      if (!zEntry) {   // not a zero entry
        zDiag = false; // mark diagonal as non-empty

	for (long jj = nzLast+1; jj < j; jj++) {// clear from last nonzero entry
          tmpDiag[jj].assign(d, RX());
        }
        nzLast = j; // current entry is the last nonzero one

        // recode entry as a vector of polynomials
        for (long k = 0; k < d; k++) conv(entry1[k], entry[k]);

        // compute the linearlized polynomial coefficients
	ea.buildLinPolyCoeffs(tmpDiag[j], entry1);
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < D; jj++) {
      tmpDiag[jj].assign(d, RX());
    }

    if (D==1) diag.assign(ea.size(), tmpDiag[0]); // dimension of size one
    else for (long j = 0; j < ea.size(); j++)
           diag[j] = tmpDiag[ ea.coordinate(dim,j) ];
           // rearrange the indexes based on the current dimension

    return false; // a nonzero diagonal
  }

  // Process a single block diagonal with index idx along dimenssion dim,
  // making calls to the multiGet(i,j,k) method and storing the result in
  // the diag matrix (# of vectors = extenssion-degree).
  bool processDiagonal2(std::vector< std::vector<RX> >& diag,
			long dim, long idx, long D, long d)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry

    mat_R entry(INIT_SIZE, d, d);
    std::vector<RX> entry1(d);

    // Get the slots in this diagonal one at a time
    long blockIdx, rowIdx, colIdx;
    for (long j = 0; j < ea.size(); j++) { // process entry j
      if (dim == ea.dimension()) { // "special" last dimenssion of size 1
	rowIdx = colIdx = 0; blockIdx=j;
      } else {
        std::tie(blockIdx, colIdx)
	  = ea.getContext().zMStar.breakIndexByDim(j, dim);
	rowIdx = mcMod(colIdx-idx,D);
      }
      bool zEntry = mat.multiGet(entry,rowIdx,colIdx,blockIdx);
      // entry [i,j-i mod D] in the block corresponding to blockIdx
      // multiGet(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry=true; // zero is an empty entry too
      assert(zEntry ||
             (entry.NumRows() == d && entry.NumCols() == d));

      if (!zEntry) {    // non-empty entry
	zDiag = false;  // mark diagonal as non-empty

	for (long jj = nzLast+1; jj < j; jj++) {// clear from last nonzero entry
	  for (long k = 0; k < d; k++)
	    clear(diag[jj][k]);
        }
	nzLast = j; // current entry is the last nonzero one

	// recode entry as a vector of polynomials
	for (long k = 0; k < d; k++) conv(entry1[k], entry[k]);

        // compute the linearlized polynomial coefficients
	ea.buildLinPolyCoeffs(diag[j], entry1);
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < ea.size(); jj++)
      for (long k = 0; k < d; k++)
	clear(diag[jj][k]);

    return false; // a nonzero diagonal
  }

  void multiply(Ctxt* ctxt, long dim, bool oneTransform)
  {
    assert(dim >= 0 && dim <= ea.dimension());
    RBak bak; bak.save(); ea.getTab().restoreContext(); // backup NTL modulus

    long nslots = ea.size();
    long d = ea.getDegree();
    long D = (dim == ea.dimension()) ? 1 : ea.sizeOfDimension(dim);

    const PAlgebra& zMStar = ea.getContext().zMStar;
    long p = zMStar.getP();
    long m = zMStar.getM();

    std::vector< std::vector<RX> > diag(nslots); // scratch space
    for (long j = 0; j < nslots; j++) diag[j].resize(d);

    std::vector<Ctxt> acc;
    std::unique_ptr<Ctxt> shCtxt;
    if (ctxt!=nullptr) { // we need to do an actual multiplication
      ctxt->cleanUp(); // not sure, but this may be a good idea
      acc.assign(d, Ctxt(ZeroCtxtLike, *ctxt));
      shCtxt.reset(new Ctxt(*ctxt));
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    // Process the diagonals one at a time
    for (long e = 0; e < D; e++) { // process diagonal e
      bool zeroDiag = true;
      // For each diagonal e, we update the d accumulators y_0,..,y_{d-1}
      // with y_f += \sigma^{-f}(\lambda_{e,f}) * \rho^e(x)

      std::vector<zzX> zpoly(d, zzX());
      std::vector<zzX*> zzxPtr(d,nullptr);
      std::vector<DoubleCRT*> dcrtPtr(d,nullptr);

      if (dcp!=nullptr) for (long f=0; f<d; f++) { // DoubleCRT cache exists
          dcrtPtr[f] = (*dcp)[d*e +f].get();
          if (dcrtPtr[f]!=nullptr) zeroDiag = false;
      }
      else if (zcp!=nullptr) for (long f=0; f<d; f++) {// zzX but no DoubleCRT
          zzxPtr[f] = (*zcp)[d*e +f].get();
          if (zzxPtr[f]!=nullptr) zeroDiag = false;
      } else {
       zeroDiag = oneTransform? this->processDiagonal1(diag, dim, e, D, d)
                               : this->processDiagonal2(diag, dim, e, D, d);

        // extract the "columns" from diag and encode them in zpoly
        if (!zeroDiag) for (long f=0; f<d; f++) {
          if (!shiftedColumnInDiag(zpoly[f], f, diag))// returns true on zero
            zzxPtr[f] = &(zpoly[f]);
        }
      }
      if (zeroDiag) continue; // nothing to do for this diagonal
      // done preparing all the zzxPtr, dcrtPtr variables

      // Rotate the ciphertext to position corresponding to diagonal e.
      // The code below uses only rotate-by-one operations if this is
      // a good dimenssion and the matrix is dense, hence it may require
      // fewer key-switching matrices

      if (ctxt!=nullptr) {
	if (e > 0) { // rotate the ciphertext
          *shCtxt = *ctxt;
          ea.rotate1D(*shCtxt, dim, e);
	} // if (e>0)
	// The implementation above incurs an extra mult-by-constant due
	// to the masks in rotate1D when applied in a "bad dimension".
	// These masks can be folded into the constants here, but then
	// we would need two constants for each (e,f) rather than one,
	// namely const*mask and const*(1-mask).
	// We should implement that optimization at some point.

	// Depending on zzxPtr, dcrtPtr, update the accumulated sums
	for (long f=0; f<d; f++) if (dcrtPtr[f]!=nullptr || zzxPtr[f]!=nullptr) {
            Ctxt tmp1(*shCtxt);
            if (dcrtPtr[f] != nullptr) tmp1.multByConstant(*(dcrtPtr[f]));
            else                    tmp1.multByConstant(*(zzxPtr[f]));
            acc[f] += tmp1;
          }
      } // if (ctxt!=nullptr)

      // allocate constants and store in the cache, if needed
     if (buildCache==cachezzX) for (long f=0; f<d; f++) {
          (*zCache)[d*e +f].reset( new zzX(*(zzxPtr[f])) );
      }
      else if (buildCache==cacheDCRT) for (long f=0; f<d; f++) {
          (*dCache)[d*e +f].reset(new DoubleCRT(*(zzxPtr[f]),ea.getContext()));
      }
    } // end of e'th diagonal

    // Finally, compute the result as \sum_{f=0}^{d-1} \sigma_f(y_f)
    if (ctxt!=nullptr) {
      *ctxt = acc[0];
      for (long f = 1; f < d; f++) {
	acc[f].frobeniusAutomorph(f);
	*ctxt += acc[f];
      }
    }
    // "install" the cache (if needed)
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void blockMatmul1d(Ctxt* ctxt, MatMulBase& mat, long dim,
		          MatrixCacheType buildCache, bool oneTransform)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      blockMatmul1D_impl<PA_GF2> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    case PA_zz_p_tag: {
      blockMatmul1D_impl<PA_zz_p> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    default:
      throw std::logic_error("matmul1d: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4BlockMatMul1D(MatMulBase& mat,
			      long dim, MatrixCacheType buildCache)
{ blockMatmul1d(nullptr, mat, dim, buildCache, true); }

void blockMatMul1D(Ctxt& ctxt, MatMulBase& mat, long dim,
               MatrixCacheType buildCache)
{ blockMatmul1d(&ctxt, mat, dim, buildCache, true); }

void buildCache4BlockMatMulti1D(MatMulBase& mat,
				long dim, MatrixCacheType buildCache)
{ blockMatmul1d(nullptr, mat, dim, buildCache, false); }

void blockMatMulti1D(Ctxt& ctxt, MatMulBase& mat, long dim,
		     MatrixCacheType buildCache)
{ blockMatmul1d(&ctxt, mat, dim, buildCache, false); }

// Versions for plaintext rather than ciphertext, useful for debugging
template<class type> class blockMatmul1D_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, BlockMatMul<type>& mat,
		       long dim, bool oneTrans=false)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    const PAlgebra& zMStar = ea.getContext().zMStar;
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long D = ea.sizeOfDimension(dim);
    long d = ea.getDegree();

    vector< vector<RX> > data1(n/D);
    for (long k = 0; k < n/D; k++)
      data1[k].resize(D);

    // copy the data into a vector of 1D vectors
    vector<RX>& data = pa.getData<type>();
    for (long i = 0; i < n; i++) {
      long k,j;
      std::tie(k,j) = zMStar.breakIndexByDim(i, dim);
      data1[k][j] = data[i];       // k= along dim, j = the rest of i
    }
    for (long k = 0; k < n/D; k++) { // multiply each vector by a matrix
      for (long j = 0; j < D; j++) { // matrix-vector multiplication
	vec_R acc, tmp, tmp1;
	mat_R val;
	acc.SetLength(d);
	for (long i = 0; i < D; i++) {
          bool zero = oneTrans? mat.get(val, i, j)
                              : mat.multiGet(val, i, j, k);
	  if (!zero) { // if non-zero, multiply and add
            VectorCopy(tmp1, data1[k][i], d);
            mul(tmp, tmp1, val);
            add(acc, acc, tmp);
	  }
	}
	long idx = zMStar.assembleIndexByDim(make_pair(k,j), dim);
        conv(data[idx], acc);
      }
    }
  }
};
void blockMatMul1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      BlockMatMul<PA_GF2>& mat1= dynamic_cast< BlockMatMul<PA_GF2>& >(mat);
      blockMatmul1D_pa_impl<PA_GF2>::multiply(pa, mat1, dim, true);
      break;
    }
    case PA_zz_p_tag: {
      BlockMatMul<PA_zz_p>& mat1= dynamic_cast< BlockMatMul<PA_zz_p>& >(mat);
      blockMatmul1D_pa_impl<PA_zz_p>::multiply(pa, mat1, dim, true);
      break;
    }
    default:
      throw std::logic_error("blockMatMul1D: neither PA_GF2 nor PA_zz_p");
  }
}
void blockMatMulti1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      BlockMatMul<PA_GF2>& mat1= dynamic_cast< BlockMatMul<PA_GF2>& >(mat);
      blockMatmul1D_pa_impl<PA_GF2>::multiply(pa, mat1, dim, false);
      break;
    }
    case PA_zz_p_tag: {
      BlockMatMul<PA_zz_p>& mat1= dynamic_cast< BlockMatMul<PA_zz_p>& >(mat);
      blockMatmul1D_pa_impl<PA_zz_p>::multiply(pa, mat1, dim, false);
      break;
    }
    default:
      throw std::logic_error("blockMatMulti1D: neither PA_GF2 nor PA_zz_p");
  }
}


#include <boost/cstdint.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/assert.hpp>
#include <nt2/sdk/aligned/next_power_of_2.hpp>

using boost::mpl::int_;
using boost::mpl::equal_to;
using nt2::meta::next_power_of_2;

int main()
{
  BOOST_MPL_ASSERT(( equal_to<next_power_of_2<int_<0> >::type,int_<0> >::type ));
  BOOST_MPL_ASSERT(( equal_to<next_power_of_2<int_<1> >::type,int_<1> >::type ));
  BOOST_MPL_ASSERT(( equal_to<next_power_of_2<int_<2> >::type,int_<2> >::type ));
  BOOST_MPL_ASSERT(( equal_to<next_power_of_2<int_<2> >::type,int_<2> >::type ));
  BOOST_MPL_ASSERT(( equal_to<next_power_of_2<int_<3> >::type,int_<4> >::type ));
  BOOST_MPL_ASSERT(( equal_to<next_power_of_2<int_<1055> >::type,int_<2048> >::type ));
}


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_se3_tpl_hpp__
#define __pinocchio_se3_tpl_hpp__

#include <Eigen/Geometry>
#include "pinocchio/math/quaternion.hpp"
#include "pinocchio/spatial/cartesian-axis.hpp"

namespace pinocchio
{
  template<typename _Scalar, int _Options>
  struct traits< SE3Tpl<_Scalar,_Options> >
  {
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix3 AngularType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef;
    typedef Vector3 LinearType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
  }; // traits SE3Tpl

  template<typename _Scalar, int _Options>
  struct SE3Tpl : public SE3Base< SE3Tpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl);
    typedef SE3Base< SE3Tpl<_Scalar,_Options> > Base;
    typedef Eigen::Quaternion<Scalar,Options> Quaternion;
    typedef typename traits<SE3Tpl>::Vector3 Vector3;
    typedef typename traits<SE3Tpl>::Matrix3 Matrix3;
    typedef typename traits<SE3Tpl>::Matrix4 Matrix4;
    typedef typename traits<SE3Tpl>::Vector4 Vector4;
    typedef typename traits<SE3Tpl>::Matrix6 Matrix6;

    using Base::rotation;
    using Base::translation;

    SE3Tpl(): rot(), trans() {};

    template<typename QuaternionLike,typename Vector3Like>
    SE3Tpl(const Eigen::QuaternionBase<QuaternionLike> & quat,
           const Eigen::MatrixBase<Vector3Like> & trans)
    : rot(quat.matrix()), trans(trans)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
    }

    template<typename Matrix3Like,typename Vector3Like>
    SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R,
           const Eigen::MatrixBase<Vector3Like> & trans)
    : rot(R), trans(trans)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
    }

    template<typename Matrix4Like>
    explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m)
    : rot(m.template block<3,3>(LINEAR,LINEAR))
    , trans(m.template block<3,1>(LINEAR,ANGULAR))
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
    }

    SE3Tpl(int)
    : rot(AngularType::Identity())
    , trans(LinearType::Zero())
    {}

    template<int O2>
    SE3Tpl(const SE3Tpl<Scalar,O2> & clone)
    : rot(clone.rotation()),trans(clone.translation()) {}

    template<int O2>
    SE3Tpl & operator=(const SE3Tpl<Scalar,O2> & other)
    {
      rot = other.rotation();
      trans = other.translation();
      return *this;
    }

    static SE3Tpl Identity()
    {
      return SE3Tpl(1);
    }

    SE3Tpl & setIdentity()
    { rot.setIdentity (); trans.setZero (); return *this;}

    /// aXb = bXa.inverse()
    SE3Tpl inverse() const
    {
      return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
    }

    static SE3Tpl Random()
    {
      return SE3Tpl().setRandom();
    }

    SE3Tpl & setRandom()
    {
      Quaternion q; quaternion::uniformRandom(q);
      rot = q.matrix();
      trans.setRandom();

      return *this;
    }

    HomogeneousMatrixType toHomogeneousMatrix_impl() const
    {
      HomogeneousMatrixType M;
      M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,1>(LINEAR,ANGULAR) = trans;
      M.template block<1,3>(ANGULAR,LINEAR).setZero();
      M(3,3) = 1;
      return M;
    }

    /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
    ActionMatrixType toActionMatrix_impl() const
    {
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
      ActionMatrixType M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(ANGULAR,LINEAR).setZero();
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);

      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }

    ActionMatrixType toActionMatrixInverse_impl() const
    {
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
      ActionMatrixType M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
      Block3 C = M.template block<3,3>(ANGULAR,LINEAR); // used as temporary
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);

#define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \
  CartesianAxis<axis_id>::cross(v3_in,v3_out); \
  res.col(axis_id).noalias() = R.transpose() * v3_out;

      PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
      PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
      PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);

#undef PINOCCHIO_INTERNAL_COMPUTATION

      C.setZero();
      return M;
    }

    ActionMatrixType toDualActionMatrix_impl() const
    {
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
      ActionMatrixType M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(LINEAR,ANGULAR).setZero();
      Block3 B = M.template block<3,3>(ANGULAR,LINEAR);

      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }

    void disp_impl(std::ostream & os) const
    {
      os
      << "  R =\n" << rot << std::endl
      << "  p = " << trans.transpose() << std::endl;
    }

    /// --- GROUP ACTIONS ON M6, F6 and I6 ---

    /// ay = aXb.act(by)
    template<typename D>
    typename SE3GroupAction<D>::ReturnType
    act_impl(const D & d) const
    {
      return d.se3Action(*this);
    }

    /// by = aXb.actInv(ay)
    template<typename D> typename SE3GroupAction<D>::ReturnType
    actInv_impl(const D & d) const
    {
      return d.se3ActionInverse(*this);
    }

    template<typename EigenDerived>
    typename EigenDerived::PlainObject
    actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
    { return (rotation()*p+translation()).eval(); }

    template<typename MapDerived>
    Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
    { return Vector3(rotation()*p+translation()); }

    template<typename EigenDerived>
    typename EigenDerived::PlainObject
    actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
    { return (rotation().transpose()*(p-translation())).eval(); }

    template<typename MapDerived>
    Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
    { return Vector3(rotation().transpose()*(p-translation())); }

    Vector3 act_impl(const Vector3 & p) const
    { return Vector3(rotation()*p+translation()); }

    Vector3 actInv_impl(const Vector3 & p) const
    { return Vector3(rotation().transpose()*(p-translation())); }

    template<int O2>
    SE3Tpl act_impl(const SE3Tpl<Scalar,O2> & m2) const
    { return SE3Tpl(rot*m2.rotation()
                    ,translation()+rotation()*m2.translation());}

    template<int O2>
    SE3Tpl actInv_impl(const SE3Tpl<Scalar,O2> & m2) const
    { return SE3Tpl(rot.transpose()*m2.rotation(),
                    rot.transpose()*(m2.translation()-translation()));}

    template<int O2>
    SE3Tpl __mult__(const SE3Tpl<Scalar,O2> & m2) const
    { return this->act_impl(m2);}

    template<int O2>
    bool isEqual(const SE3Tpl<Scalar,O2> & m2) const
    {
      return (rotation() == m2.rotation() && translation() == m2.translation());
    }

    template<int O2>
    bool isApprox_impl(const SE3Tpl<Scalar,O2> & m2,
                       const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return rotation().isApprox(m2.rotation(), prec)
      && translation().isApprox(m2.translation(), prec);
    }

    bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return rotation().isIdentity(prec) && translation().isZero(prec);
    }

    ConstAngularRef rotation_impl() const { return rot; }
    AngularRef rotation_impl() { return rot; }
    void rotation_impl(const AngularType & R) { rot = R; }
    ConstLinearRef translation_impl() const { return trans;}
    LinearRef translation_impl() { return trans;}
    void translation_impl(const LinearType & p) { trans = p; }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    SE3Tpl<NewScalar,Options> cast() const
    {
      typedef SE3Tpl<NewScalar,Options> ReturnType;
      ReturnType res(rot.template cast<NewScalar>(),
                     trans.template cast<NewScalar>());
      return res;
    }

    ///
    /// \brief Linear interpolation on the SE3 manifold.
    ///
    /// \param[in] A Initial transformation.
    /// \param[in] B Target transformation.
    /// \param[in] alpha Interpolation factor in [0 ... 1].
    ///
    /// \returns An interpolated transformation between A and B.
    ///
    /// \note This is similar to the SLERP operation which acts initially for rotation but applied here to rigid transformation.
    ///
    template<typename OtherScalar>
    static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha);

  protected:
    AngularType rot;
    LinearType trans;

  }; // class SE3Tpl

} // namespace pinocchio

#endif // ifndef __pinocchio_se3_tpl_hpp__


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::meta::failure_random.hpp                      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_META_FAILURE_RANDOM_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_META_FAILURE_RANDOM_HPP_ER_2009
#include <boost/dist_random/include.hpp>
#include <boost/statistics/model/wrap/aggregate/model_covariate_parameter.hpp>
#include <boost/statistics/survival/data/meta/failure_distribution.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

    // Model + Covariate + Parameter --->  RandomDistribution (of failure time)
    //
    // These are default implementations that can be overloaded with a
    // specialization on M

    template<typename M>
    struct meta_failure_random{
        typedef meta_failure_distribution<M>                        map1_;
        typedef typename map1_::type                                dist_;
        typedef boost::dist_random<dist_>                           map2_;
        typedef typename map2_::type                                type;
    };

    template<typename M,typename X,typename P>
    typename meta_failure_random<M>::type
    make_failure_random(
        boost::statistics::model::model_covariate_parameter_<M,X,P>
    );

    // Implementation //

    template<typename M,typename X,typename P>
    typename meta_failure_random<M>::type
    make_failure_random(
        boost::statistics::model::model_covariate_parameter_<M,X,P> mcp
    ){
        typedef meta_failure_random<M>                              map_;
        typedef typename map_::map2_                                map2_;
        return map2_::make(
            make_failure_distribution(mcp)
        );
    }

}// data
}// survival
}// statistics
}// boost

#endif


//
// Created by rgrandia on 23.09.19.
//

#pragma once

#include <urdf/model.h>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

#include <urdf2robcogen/UrdfStructure.hpp>

Eigen::Matrix3d inertiaMatrixFromLink(const urdf::Link& link);
bool isValidInertiaMatrix(const Eigen::Matrix3d& I);
Eigen::Matrix3d expressInertiaFromFrameAInComFrame(const Eigen::Matrix3d& A_I_A, double m, const PoseInWorld& poseA, const PoseInWorld& poseC);

Eigen::Matrix3d skewSymMatrixFromVector(const Eigen::Vector3d& vec);

Eigen::Vector3d toEigen(const urdf::Vector3& vec);
urdf::Vector3 fromEigen(const Eigen::Vector3d& vec);

Eigen::Quaterniond toEigen(const urdf::Rotation& rotation);
urdf::Rotation fromEigen(const Eigen::Quaterniond& rotation);

std::string printVector(const urdf::Vector3& vector);

urdf::Vector3 rotationXyz(const urdf::Rotation& rot_quaternion);

std::pair<Eigen::Vector3d, Eigen::Quaterniond> getRelativePose(const PoseInWorld& poseChild, const PoseInWorld& poseParent);

std::string print_timeStamp();

template <typename K, typename T>
std::vector<K> getKeys(std::map<K, T> m) {
  std::vector<K> v;
  v.reserve(m.size());
  for (const auto& keyValue : m) {
    v.push_back(keyValue.first);
  }
  return v;
}

template <typename T>
std::vector<std::pair<std::string, T>> toIDSortedVector(std::map<std::string, T> m) {
  std::vector<std::pair<std::string, T>> v;
  v.reserve(m.size());
  for (const auto& keyValue : m) {
    v.push_back(keyValue);
  }

  std::sort(v.begin(), v.end(),
            [](const std::pair<std::string, T>& a, const std::pair<std::string, T>& b) { return a.second.id < b.second.id; });
  return v;
}

template <typename K>
void removeFromContainer(const K& key, std::vector<K>& container) {
  auto keyIterator = std::find(container.begin(), container.end(), key);
  if (keyIterator != container.end()) {
    container.erase(keyIterator);
  } else {
    throw std::runtime_error("Key to be deleted not found");
  }
}

template <typename K, typename T>
void removeFromContainer(const K& key, std::map<K, T>& container) {
  auto keyIterator = container.find(key);
  if (keyIterator != container.end()) {
    container.erase(keyIterator);
  } else {
    throw std::runtime_error("Key to be deleted not found");
  }
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE algebra_curves_test

#include <iostream>
#include <vector>
#include <array>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>

#include <nil/crypto3/algebra/pairing/bls12.hpp>
#include <nil/crypto3/algebra/pairing/mnt4.hpp>
#include <nil/crypto3/algebra/pairing/mnt6.hpp>

#include <nil/crypto3/algebra/algorithms/pair.hpp>

#include <nil/crypto3/algebra/fields/detail/element/fp.hpp>
#include <nil/crypto3/algebra/fields/detail/element/fp4.hpp>
#include <nil/crypto3/algebra/fields/detail/element/fp6_2over3.hpp>
#include <nil/crypto3/algebra/fields/detail/element/fp12_2over3over2.hpp>

using namespace nil::crypto3::algebra::pairing;
using namespace nil::crypto3::algebra;
using namespace nil::crypto3::multiprecision;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp<FieldParams> &e) {
    os << e.data;
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp2<FieldParams> &e) {
    os << "[";
    print_field_element(os, e.data[0]);
    os << ", ";
    print_field_element(os, e.data[1]);
    os << "]";
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp3<FieldParams> &e) {
    os << "[";
    print_field_element(os, e.data[0]);
    os << ", ";
    print_field_element(os, e.data[1]);
    os << ", ";
    print_field_element(os, e.data[2]);
    os << "]";
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp4<FieldParams> &e) {
    os << "[";
    print_field_element(os, e.data[0]);
    os << ", ";
    print_field_element(os, e.data[1]);
    os << "]";
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp6_2over3<FieldParams> &e) {
    os << "[";
    print_field_element(os, e.data[0]);
    os << ", ";
    print_field_element(os, e.data[1]);
    os << "]";
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const fields::detail::element_fp12_2over3over2<FieldParams> &e) {
    os << "[[[" << e.data[0].data[0].data[0].data << "," << e.data[0].data[0].data[1].data << "],["
       << e.data[0].data[1].data[0].data << "," << e.data[0].data[1].data[1].data << "],["
       << e.data[0].data[2].data[0].data << "," << e.data[0].data[2].data[1].data << "]],"
       << "[[" << e.data[1].data[0].data[0].data << "," << e.data[1].data[0].data[1].data << "],["
       << e.data[1].data[1].data[0].data << "," << e.data[1].data[1].data[1].data << "],["
       << e.data[1].data[2].data[0].data << "," << e.data[1].data[2].data[1].data << "]]]";
}

template<typename CurveGroupValue>
void print_curve_group_element(std::ostream &os, const CurveGroupValue &e) {
    os << "(";
    print_field_element(os, e.X);
    os << ",";
    print_field_element(os, e.Y);
    os << ",";
    print_field_element(os, e.Z);
    os << ")" << std::endl;
}

void print_g1_precomp_element(std::ostream &os, const typename pairing::pairing_policy<curves::bls12<381>>::g1_precomputed_type &e) {
    os << "{\"PX\": ";
    print_field_element(os, e.PX);
    os << ", \"PY\": ";
    print_field_element(os, e.PY);
    os << "}" << std::endl;
}

void print_g1_precomp_element(std::ostream &os, const typename pairing::pairing_policy<curves::mnt4<298>>::g1_precomputed_type &e) {
    os << "{\"PX\": ";
    print_field_element(os, e.PX);
    os << ", \"PY\": ";
    print_field_element(os, e.PY);
    os << ", \"PX_twist\": ";
    print_field_element(os, e.PX_twist);
    os << ", \"PY_twist\": ";
    print_field_element(os, e.PY_twist);
    os << "}" << std::endl;
}

void print_g1_precomp_element(std::ostream &os, const typename pairing::pairing_policy<curves::mnt6<298>>::g1_precomputed_type &e) {
    os << "{\"PX\": ";
    print_field_element(os, e.PX);
    os << ", \"PY\": ";
    print_field_element(os, e.PY);
    os << ", \"PX_twist\": ";
    print_field_element(os, e.PX_twist);
    os << ", \"PY_twist\": ";
    print_field_element(os, e.PY_twist);
    os << "}" << std::endl;
}

void print_g2_precomp_element(std::ostream &os, const typename pairing::pairing_policy<curves::bls12<381>>::g2_precomputed_type &e) {
    os << "\"coordinates\": [[" << e.QX.data[0].data << " , " << e.QX.data[1].data << "] , [" << e.QY.data[0].data
       << " , " << e.QY.data[1].data << "]]" << std::endl;
    auto print_coeff = [&os](const auto &c) {
        os << "\"ell_0\": [" << c.ell_0.data[0].data << "," << c.ell_0.data[1].data << "],"
           << "\"ell_VW\": [" << c.ell_VW.data[0].data << "," << c.ell_VW.data[1].data << "],"
           << "\"ell_VV\": [" << c.ell_VV.data[0].data << "," << c.ell_VV.data[1].data << "]";
    };
    os << "coefficients: [";
    for (auto &c : e.coeffs) {
        os << "{";
        print_coeff(c);
        os << "},";
    }
    os << "]" << std::endl;
}

void print_g2_precomp_element(std::ostream &os, const typename pairing::pairing_policy<curves::mnt4<298>>::g2_precomputed_type &e) {
    os << "\"coordinates\": {\"QX\": ";
    print_field_element(os, e.QX);
    os << ", \"QY\": ";
    print_field_element(os, e.QY);
    os << ", \"QY2\": ";
    print_field_element(os, e.QY2);
    os << ", \"QX_over_twist\": ";
    print_field_element(os, e.QX_over_twist);
    os << ", \"QY_over_twist\": ";
    print_field_element(os, e.QY_over_twist);
    os << "}" << std::endl;

    auto print_dbl_coeff = [&os](const auto &c) {
        os << "{\"c_H\": ";
        print_field_element(os, c.c_H);
        os << ", \"c_4C\": ";
        print_field_element(os, c.c_4C);
        os << ", \"c_J\": ";
        print_field_element(os, c.c_J);
        os << ", \"c_L\": ";
        print_field_element(os, c.c_L);
        os << "}" << std::endl;
    };
    auto print_add_coeff = [&os](const auto &c) {
        os << "{\"c_L1\": ";
        print_field_element(os, c.c_L1);
        os << ", \"c_RZ\": ";
        print_field_element(os, c.c_RZ);
        os << "}" << std::endl;
    };

    os << "dbl_coeffs: ";
    for (auto &c : e.dbl_coeffs) {
        print_dbl_coeff(c);
    }
    std::cout << std::endl;

    os << "add_coeffs: ";
    for (auto &c : e.add_coeffs) {
        print_add_coeff(c);
    }
    std::cout << std::endl;
}

void print_g2_precomp_element(std::ostream &os, const typename pairing::pairing_policy<curves::mnt6<298>>::g2_precomputed_type &e) {
    os << "\"coordinates\": {\"QX\": ";
    print_field_element(os, e.QX);
    os << ", \"QY\": ";
    print_field_element(os, e.QY);
    os << ", \"QY2\": ";
    print_field_element(os, e.QY2);
    os << ", \"QX_over_twist\": ";
    print_field_element(os, e.QX_over_twist);
    os << ", \"QY_over_twist\": ";
    print_field_element(os, e.QY_over_twist);
    os << "}" << std::endl;

    auto print_dbl_coeff = [&os](const auto &c) {
        os << "{\"c_H\": ";
        print_field_element(os, c.c_H);
        os << ", \"c_4C\": ";
        print_field_element(os, c.c_4C);
        os << ", \"c_J\": ";
        print_field_element(os, c.c_J);
        os << ", \"c_L\": ";
        print_field_element(os, c.c_L);
        os << "}" << std::endl;
    };
    auto print_add_coeff = [&os](const auto &c) {
        os << "{\"c_L1\": ";
        print_field_element(os, c.c_L1);
        os << ", \"c_RZ\": ";
        print_field_element(os, c.c_RZ);
        os << "}" << std::endl;
    };

    os << "dbl_coeffs: ";
    for (auto &c : e.dbl_coeffs) {
        print_dbl_coeff(c);
    }
    std::cout << std::endl;

    os << "add_coeffs: ";
    for (auto &c : e.add_coeffs) {
        print_add_coeff(c);
    }
    std::cout << std::endl;
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp<FieldParams>> {
                void operator()(std::ostream &os, typename fields::detail::element_fp<FieldParams> const &e) {
                    print_field_element(os, e);
                    std::cout << std::endl;
                }
            };

            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp2<FieldParams>> {
                void operator()(std::ostream &os, typename fields::detail::element_fp2<FieldParams> const &e) {
                    print_field_element(os, e);
                    std::cout << std::endl;
                }
            };

            template<>
            struct print_log_value<curves::bls12<381>::g1_type<>::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::bls12<381>::g1_type<>::value_type &e) {
                    print_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<curves::bls12<381>::g2_type<>::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::bls12<381>::g2_type<>::value_type &e) {
                    print_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<pairing::pairing_policy<curves::bls12<381>>::g1_precomputed_type> {
                void operator()(std::ostream &os, const typename pairing::pairing_policy<curves::bls12<381>>::g1_precomputed_type &e) {
                    print_g1_precomp_element(os, e);
                }
            };

            template<>
            struct print_log_value<pairing::pairing_policy<curves::bls12<381>>::g2_precomputed_type> {
                void operator()(std::ostream &os, const typename pairing::pairing_policy<curves::bls12<381>>::g2_precomputed_type &e) {
                    print_g2_precomp_element(os, e);
                }
            };

            template<>
            struct print_log_value<curves::bls12<381>::gt_type::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::bls12<381>::gt_type::value_type &e) {
                    print_field_element(os, e);
                    std::cout << std::endl;
                }
            };

            template<>
            struct print_log_value<curves::mnt4<298>::g1_type<>::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::mnt4<298>::g1_type<>::value_type &e) {
                    print_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<curves::mnt4<298>::g2_type<>::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::mnt4<298>::g2_type<>::value_type &e) {
                    print_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<pairing::pairing_policy<curves::mnt4<298>>::g1_precomputed_type> {
                void operator()(std::ostream &os, const typename pairing::pairing_policy<curves::mnt4<298>>::g1_precomputed_type &e) {
                    print_g1_precomp_element(os, e);
                }
            };

            template<>
            struct print_log_value<pairing::pairing_policy<curves::mnt4<298>>::g2_precomputed_type> {
                void operator()(std::ostream &os, const typename pairing::pairing_policy<curves::mnt4<298>>::g2_precomputed_type &e) {
                    print_g2_precomp_element(os, e);
                }
            };

            template<>
            struct print_log_value<curves::mnt4<298>::gt_type::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::mnt4<298>::gt_type::value_type &e) {
                    print_field_element(os, e);
                    std::cout << std::endl;
                }
            };

            template<>
            struct print_log_value<curves::mnt6<298>::g1_type<>::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::mnt6<298>::g1_type<>::value_type &e) {
                    print_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<curves::mnt6<298>::g2_type<>::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::mnt6<298>::g2_type<>::value_type &e) {
                    print_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<pairing::pairing_policy<curves::mnt6<298>>::g1_precomputed_type> {
                void operator()(std::ostream &os, const typename pairing::pairing_policy<curves::mnt6<298>>::g1_precomputed_type &e) {
                    print_g1_precomp_element(os, e);
                }
            };

            template<>
            struct print_log_value<pairing::pairing_policy<curves::mnt6<298>>::g2_precomputed_type> {
                void operator()(std::ostream &os, const typename pairing::pairing_policy<curves::mnt6<298>>::g2_precomputed_type &e) {
                    print_g2_precomp_element(os, e);
                }
            };

            template<>
            struct print_log_value<curves::mnt6<298>::gt_type::value_type> {
                void operator()(std::ostream &os,
                                const typename curves::mnt6<298>::gt_type::value_type &e) {
                    print_field_element(os, e);
                    std::cout << std::endl;
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

const char *test_data = "../../../../libs/algebra/test/data/pairing.json";

boost::property_tree::ptree string_data(const std::string &test_name) {
    boost::property_tree::ptree string_data;
    boost::property_tree::read_json(test_data, string_data);

    return string_data.get_child(test_name);
}

enum Fr_enum : std::size_t { VKx_poly, VKy_poly, VKz_poly, A1_poly, B1_poly, C1_poly, A2_poly, B2_poly, C2_poly };
enum G1_enum : std::size_t { A1, C1, A2, C2, VKx };
enum G2_enum : std::size_t { B1, B2, VKy, VKz };
enum GT_enum : std::size_t {
    pairing_A1_B1,
    pairing_A2_B2,
    pair_reduceding_A1_B1,
    pair_reduceding_A2_B2,
    pair_reduceding_A1_B1_mul_pair_reduceding_A2_B2,
    pair_reduceding_VKx_poly_A1_B1,
    miller_loop_prec_A1_prec_B1,
    miller_loop_prec_A2_prec_B2,
    double_miller_loop_prec_A1_prec_B1_prec_A2_prec_B2
};
enum g1_precomp_enum : std::size_t { prec_A1, prec_A2 };
enum g2_precomp_enum : std::size_t { prec_B1, prec_B2 };

// TODO: add affine_pair_reduceding test
template<typename CurveType, typename Fr_value_type, typename G1_value_type, typename G2_value_type,
         typename GT_value_type, typename g1_precomp_value_type, typename g2_precomp_value_type>
void check_pairing_operations(std::vector<Fr_value_type> &Fr_elements,
                              std::vector<G1_value_type> &G1_elements,
                              std::vector<G2_value_type> &G2_elements,
                              std::vector<GT_value_type> &GT_elements,
                              std::vector<g1_precomp_value_type> &G1_prec_elements,
                              std::vector<g2_precomp_value_type> &G2_prec_elements) {
    std::cout << " * Basic fields and groups tests started..." << std::endl;
    BOOST_CHECK_EQUAL((Fr_elements[A1_poly] * Fr_elements[B1_poly] - Fr_elements[VKx_poly] * Fr_elements[VKy_poly]) *
                          Fr_elements[VKz_poly].inversed(),
                      Fr_elements[C1_poly]);
    BOOST_CHECK_EQUAL((Fr_elements[A2_poly] * Fr_elements[B2_poly] - Fr_elements[VKx_poly] * Fr_elements[VKy_poly]) *
                          Fr_elements[VKz_poly].inversed(),
                      Fr_elements[C2_poly]);
    BOOST_CHECK_EQUAL(Fr_elements[VKx_poly] * G1_value_type::one(), G1_elements[VKx]);
    BOOST_CHECK_EQUAL(Fr_elements[VKy_poly] * G2_value_type::one(), G2_elements[VKy]);
    BOOST_CHECK_EQUAL(Fr_elements[VKz_poly] * G2_value_type::one(), G2_elements[VKz]);
    BOOST_CHECK_EQUAL(Fr_elements[A1_poly] * G1_value_type::one(), G1_elements[A1]);
    BOOST_CHECK_EQUAL(Fr_elements[C1_poly] * G1_value_type::one(), G1_elements[C1]);
    BOOST_CHECK_EQUAL(Fr_elements[A2_poly] * G1_value_type::one(), G1_elements[A2]);
    BOOST_CHECK_EQUAL(Fr_elements[C2_poly] * G1_value_type::one(), G1_elements[C2]);
    BOOST_CHECK_EQUAL(Fr_elements[B1_poly] * G2_value_type::one(), G2_elements[B1]);
    BOOST_CHECK_EQUAL(Fr_elements[B2_poly] * G2_value_type::one(), G2_elements[B2]);
    std::cout << " * Basic fields and groups tests finished." << std::endl << std::endl;

    std::cout << " * Precomputing and pairing tests started..." << std::endl;
    BOOST_CHECK_EQUAL(precompute_g1<CurveType>(G1_elements[A1]), G1_prec_elements[prec_A1]);
    BOOST_CHECK_EQUAL(precompute_g1<CurveType>(G1_elements[A2]), G1_prec_elements[prec_A2]);
    BOOST_CHECK_EQUAL(precompute_g2<CurveType>(G2_elements[B1]), G2_prec_elements[prec_B1]);
    BOOST_CHECK_EQUAL(precompute_g2<CurveType>(G2_elements[B2]), G2_prec_elements[prec_B2]);
    BOOST_CHECK_EQUAL(pair<CurveType>(G1_elements[A1], G2_elements[B1]), GT_elements[pairing_A1_B1]);
    BOOST_CHECK_EQUAL(pair<CurveType>(G1_elements[A2], G2_elements[B2]), GT_elements[pairing_A2_B2]);
    std::cout << " * Precomputing and pairing tests finished." << std::endl << std::endl;

    // TODO: activate after pair_reduceding->cyclotomic_exp fixed. Bugs in final_exponentiation_last_chunk
    std::cout << " * Reduced pairing tests started..." << std::endl;
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(G1_elements[A1], G2_elements[B1]), GT_elements[pair_reduceding_A1_B1]);
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(G1_elements[A1], G2_elements[B1]),
                      pair_reduced<CurveType>(G1_elements[VKx], G2_elements[VKy]) *
                          pair_reduced<CurveType>(G1_elements[C1], G2_elements[VKz]));
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(G1_elements[A2], G2_elements[B2]), GT_elements[pair_reduceding_A2_B2]);
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(G1_elements[A2], G2_elements[B2]),
                      pair_reduced<CurveType>(G1_elements[VKx], G2_elements[VKy]) *
                          pair_reduced<CurveType>(G1_elements[C2], G2_elements[VKz]));
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(G1_elements[A1], G2_elements[B1]) *
                          pair_reduced<CurveType>(G1_elements[A2], G2_elements[B2]),
                      GT_elements[pair_reduceding_A1_B1_mul_pair_reduceding_A2_B2]);
    std::cout << " * Reduced pairing tests finished." << std::endl << std::endl;

    // TODO: activate when scalar multiplication done
    std::cout << " * Reduced pairing tests with scalar multiplication started..." << std::endl;
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(G1_elements[A1], G2_elements[B1]) *
                          pair_reduced<CurveType>(G1_elements[A2], G2_elements[B2]),
                      pair_reduced<CurveType>(Fr_value_type(2) * G1_elements[VKx], G2_elements[VKy]) *
                          pair_reduced<CurveType>(G1_elements[C1] + G1_elements[C2], G2_elements[VKz]));
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(Fr_elements[VKx_poly] * G1_elements[A1], G2_elements[B1]),
                      GT_elements[pair_reduceding_VKx_poly_A1_B1]);
    BOOST_CHECK_EQUAL(pair_reduced<CurveType>(Fr_elements[VKx_poly] * G1_elements[A1], G2_elements[B1]),
                      pair_reduced<CurveType>(G1_elements[A1], Fr_elements[VKx_poly] * G2_elements[B1]));
    std::cout << " * Reduced pairing tests with scalar multiplication finished." << std::endl << std::endl;

    // TODO: activate when pow will be override with field element
    std::cout << " * Reduced pairing tests with pow started..." << std::endl;
    BOOST_CHECK_EQUAL(
        pair_reduced<CurveType>(Fr_elements[VKx_poly] * G1_elements[A1], G2_elements[B1]),
        // TODO: fix pow to accept field element as exponent
        pair_reduced<CurveType>(G1_elements[A1], G2_elements[B1]).pow(cpp_int(Fr_elements[VKx_poly].data)));
    std::cout << " * Reduced pairing tests with pow finished." << std::endl << std::endl;

    std::cout << " * Miller loop tests started..." << std::endl;
    BOOST_CHECK_EQUAL(miller_loop<CurveType>(G1_prec_elements[prec_A1], G2_prec_elements[prec_B1]),
                      GT_elements[miller_loop_prec_A1_prec_B1]);
    BOOST_CHECK_EQUAL(miller_loop<CurveType>(G1_prec_elements[prec_A2], G2_prec_elements[prec_B2]),
                      GT_elements[miller_loop_prec_A2_prec_B2]);
    BOOST_CHECK_EQUAL(double_miller_loop<CurveType>(G1_prec_elements[prec_A1], G2_prec_elements[prec_B1],
                                                   G1_prec_elements[prec_A2], G2_prec_elements[prec_B2]),
                      GT_elements[double_miller_loop_prec_A1_prec_B1_prec_A2_prec_B2]);
    BOOST_CHECK_EQUAL(miller_loop<CurveType>(G1_prec_elements[prec_A1], G2_prec_elements[prec_B1]) *
                          miller_loop<CurveType>(G1_prec_elements[prec_A2], G2_prec_elements[prec_B2]),
                      double_miller_loop<CurveType>(G1_prec_elements[prec_A1], G2_prec_elements[prec_B1],
                                                   G1_prec_elements[prec_A2], G2_prec_elements[prec_B2]));
    std::cout << " * Miller loop tests finished." << std::endl << std::endl;
}

template<typename ElementType>
struct field_element_init;

template<typename FieldParams>
struct field_element_init<fields::detail::element_fp<FieldParams>> {
    using element_type = fields::detail::element_fp<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        return element_type(typename element_type::integral_type(element_data.second.data()));
    }
};

template<typename FieldParams>
struct field_element_init<fields::detail::element_fp2<FieldParams>> {
    using element_type = fields::detail::element_fp2<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        // element_fp
        using underlying_type = typename element_type::underlying_type;

        std::array<underlying_type, 2> element_values;
        auto i = 0;
        for (auto &element_value : element_data.second) {
            element_values[i++] = field_element_init<underlying_type>::process(element_value);
        }
        return element_type(element_values[0], element_values[1]);
    }
};

template<typename FieldParams>
struct field_element_init<fields::detail::element_fp3<FieldParams>> {
    using element_type = fields::detail::element_fp3<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        // element_fp
        using underlying_type = typename element_type::underlying_type;

        std::array<underlying_type, 3> element_values;
        auto i = 0;
        for (auto &element_value : element_data.second) {
            element_values[i++] = field_element_init<underlying_type>::process(element_value);
        }
        return element_type(element_values[0], element_values[1], element_values[2]);
    }
};

template<typename FieldParams>
struct field_element_init<fields::detail::element_fp4<FieldParams>> {
    using element_type = fields::detail::element_fp4<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        // element_fp2 over element_fp
        using underlying_type = typename element_type::underlying_type;

        std::array<underlying_type, 2> element_values;
        auto i = 0;
        for (auto &element_value : element_data.second) {
            element_values[i++] = field_element_init<underlying_type>::process(element_value);
        }
        return element_type(element_values[0], element_values[1]);
    }
};

template<typename FieldParams>
struct field_element_init<fields::detail::element_fp6_2over3<FieldParams>> {
    using element_type = fields::detail::element_fp6_2over3<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        // element_fp3 over element_fp
        using underlying_type = typename element_type::underlying_type;

        std::array<underlying_type, 2> element_values;
        auto i = 0;
        for (auto &element_value : element_data.second) {
            element_values[i++] = field_element_init<underlying_type>::process(element_value);
        }
        return element_type(element_values[0], element_values[1]);
    }
};

template<typename FieldParams>
struct field_element_init<fields::detail::element_fp12_2over3over2<FieldParams>> {
    using element_type = fields::detail::element_fp12_2over3over2<FieldParams>;

    template<typename ElementData>
    static inline element_type process(const ElementData &element_data) {
        // element_fp3 over element_fp2 over element_fp
        using underlying_type_3over2 = typename element_type::underlying_type;
        // element_fp2 over element_fp
        using underlying_type = typename underlying_type_3over2::underlying_type;

        std::array<underlying_type_3over2, 2> element_values;
        std::array<underlying_type, 3> underlying_element_values;
        auto i = 0;
        for (auto &elem_3over2 : element_data.second) {
            auto j = 0;
            for (auto &elem_fp2 : elem_3over2.second) {
                underlying_element_values[j++] = field_element_init<underlying_type>::process(elem_fp2);
            }
            element_values[i++] = underlying_type_3over2(underlying_element_values[0], underlying_element_values[1],
                                                         underlying_element_values[2]);
        }
        return element_type(element_values[0], element_values[1]);
    }
};

template<typename CurveGroupValue, typename PointData>
CurveGroupValue curve_point_init(const PointData &point_data) {
    using group_value_type = CurveGroupValue;
    using field_value_type = typename group_value_type::field_type::value_type;

    std::array<field_value_type, 3> coordinates;
    auto i = 0;
    for (auto &coordinate : point_data.second) {
        coordinates[i++] = field_element_init<field_value_type>::process(coordinate);
    }
    return group_value_type(coordinates[0], coordinates[1], coordinates[2]);
}

template<typename FieldParams, typename TestSet>
void pairing_test_Fr_init(std::vector<typename fields::detail::element_fp<FieldParams>> &elements,
                          const TestSet &test_set) {
    using value_type = typename fields::detail::element_fp<FieldParams>;

    for (auto &elem : test_set.second.get_child("Fr")) {
        elements.emplace_back(field_element_init<value_type>::process(elem));
    }
}

template<typename CurveType, typename TestSet>
void pairing_test_G1_init(std::vector<typename CurveType::template g1_type<>::value_type> &elements, const TestSet &test_set) {

    using value_type = typename CurveType::template g1_type<>::value_type;

    for (auto &elem_coords : test_set.second.get_child("G1")) {
        elements.emplace_back(curve_point_init<value_type>(elem_coords));
    }
}

template<typename CurveType, typename TestSet>
void pairing_test_G2_init(std::vector<typename CurveType::template g2_type<>::value_type> &elements, const TestSet &test_set) {

    using value_type = typename CurveType::template g2_type<>::value_type;

    for (auto &elem_coords : test_set.second.get_child("G2")) {
        elements.emplace_back(curve_point_init<value_type>(elem_coords));
    }
}

template<typename CurveType, typename TestSet>
void pairing_test_GT_init(std::vector<typename CurveType::gt_type::value_type> &elements, const TestSet &test_set) {

    using value_type = typename CurveType::gt_type::value_type;

    for (auto &elem_GT : test_set.second.get_child("GT")) {
        elements.emplace_back(field_element_init<value_type>::process(elem_GT));
    }
}

template<typename TestSet>
void pairing_test_g1_precomp_init(std::vector<typename pairing::pairing_policy<curves::bls12<381>>::g1_precomputed_type> &elements,
                                  const TestSet &test_set) {
    using curve_type = curves::bls12<381>;
    using pairing_policy = typename pairing::pairing_policy<curve_type>;
    using value_type = typename pairing_policy::g1_precomputed_type;

    using g1_field_value_type = typename curve_type::base_field_type::value_type;
    using g2_field_value_type = typename curve_type::template g2_type<>::field_type::value_type;

    for (auto &elem : test_set.second.get_child("g1_precomputed_type")) {
        elements.emplace_back(
            value_type {field_element_init<g1_field_value_type>::process(elem.second.get_child("PX").front()),
                        field_element_init<g1_field_value_type>::process(elem.second.get_child("PY").front())});
    }
}

template<typename TestSet>
void pairing_test_g1_precomp_init(std::vector<typename pairing::pairing_policy<curves::mnt4<298>>::g1_precomputed_type> &elements,
                                  const TestSet &test_set) {
    using curve_type = curves::mnt4<298>;
    using pairing_policy = typename pairing::pairing_policy<curve_type>;
    using value_type = typename pairing_policy::g1_precomputed_type;

    using g1_field_value_type = typename curve_type::base_field_type::value_type;
    using g2_field_value_type = typename curve_type::template g2_type<>::field_type::value_type;

    for (auto &elem : test_set.second.get_child("g1_precomputed_type")) {
        elements.emplace_back(
            value_type {field_element_init<g1_field_value_type>::process(elem.second.get_child("PX").front()),
                        field_element_init<g1_field_value_type>::process(elem.second.get_child("PY").front()),
                        field_element_init<g2_field_value_type>::process(elem.second.get_child("PX_twist").front()),
                        field_element_init<g2_field_value_type>::process(elem.second.get_child("PY_twist").front())});
    }
}

template<typename TestSet>
void pairing_test_g1_precomp_init(std::vector<typename pairing::pairing_policy<curves::mnt6<298>>::g1_precomputed_type> &elements,
                                  const TestSet &test_set) {
    using curve_type = curves::mnt6<298>;
    using pairing_policy = typename pairing::pairing_policy<curve_type>;
    using value_type = typename pairing_policy::g1_precomputed_type;

    using g1_field_value_type = typename curve_type::base_field_type::value_type;
    using g2_field_value_type = typename curve_type::template g2_type<>::field_type::value_type;

    for (auto &elem : test_set.second.get_child("g1_precomputed_type")) {
        elements.emplace_back(
            value_type {field_element_init<g1_field_value_type>::process(elem.second.get_child("PX").front()),
                        field_element_init<g1_field_value_type>::process(elem.second.get_child("PY").front()),
                        field_element_init<g2_field_value_type>::process(elem.second.get_child("PX_twist").front()),
                        field_element_init<g2_field_value_type>::process(elem.second.get_child("PY_twist").front())});
    }
}

template<typename TestSet>
void pairing_test_g2_precomp_init(std::vector<typename pairing::pairing_policy<curves::bls12<381>>::g2_precomputed_type> &elements,
                                  const TestSet &test_set) {
    using curve_type = curves::bls12<381>;
    using pairing_policy = typename pairing::pairing_policy<curve_type>;
    using value_type = typename pairing_policy::g2_precomputed_type;

    using g1_field_value_type = typename curve_type::base_field_type::value_type;
    using g2_field_value_type = typename curve_type::template g2_type<>::field_type::value_type;

    using coeffs_type = value_type::coeffs_type;
    using coeffs_value_type = g2_field_value_type;

    for (auto &elem : test_set.second.get_child("g2_precomputed_type")) {
        elements.emplace_back(value_type());

        elements.back().QX = field_element_init<g2_field_value_type>::process(elem.second.get_child("QX").front());
        elements.back().QY = field_element_init<g2_field_value_type>::process(elem.second.get_child("QY").front());

        for (auto &elem_coeffs : elem.second.get_child("coeffs")) {
            elements.back().coeffs.emplace_back(coeffs_type());

            elements.back().coeffs.back().ell_0 =
                field_element_init<coeffs_value_type>::process(elem_coeffs.second.get_child("ell_0").front());
            elements.back().coeffs.back().ell_VW =
                field_element_init<coeffs_value_type>::process(elem_coeffs.second.get_child("ell_VW").front());
            elements.back().coeffs.back().ell_VV =
                field_element_init<coeffs_value_type>::process(elem_coeffs.second.get_child("ell_VV").front());
        }
    }
}

template<typename TestSet>
void pairing_test_g2_precomp_init(std::vector<typename pairing::pairing_policy<curves::mnt4<298>>::g2_precomputed_type> &elements,
                                  const TestSet &test_set) {
    using curve_type = curves::mnt4<298>;
    using pairing_policy = typename pairing::pairing_policy<curve_type>;
    using value_type = typename pairing_policy::g2_precomputed_type;

    using g1_field_value_type = typename curve_type::base_field_type::value_type;
    using g2_field_value_type = typename curve_type::template g2_type<>::field_type::value_type;

    using dbl_coeffs_type = typename value_type::dbl_coeffs_type;
    using add_coeffs_type = typename value_type::add_coeffs_type;
    using dbl_coeffs_value_type = g2_field_value_type;
    using add_coeffs_value_type = g2_field_value_type;

    for (auto &elem : test_set.second.get_child("g2_precomputed_type")) {
        elements.emplace_back(value_type());

        elements.back().QX = field_element_init<g2_field_value_type>::process(elem.second.get_child("QX").front());
        elements.back().QY = field_element_init<g2_field_value_type>::process(elem.second.get_child("QY").front());
        elements.back().QY2 = field_element_init<g2_field_value_type>::process(elem.second.get_child("QY2").front());
        elements.back().QX_over_twist =
            field_element_init<g2_field_value_type>::process(elem.second.get_child("QX_over_twist").front());
        elements.back().QY_over_twist =
            field_element_init<g2_field_value_type>::process(elem.second.get_child("QY_over_twist").front());

        for (auto &elem_coeffs : elem.second.get_child("dbl_coeffs")) {
            elements.back().dbl_coeffs.emplace_back(dbl_coeffs_type());

            elements.back().dbl_coeffs.back().c_H =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_H").front());
            elements.back().dbl_coeffs.back().c_4C =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_4C").front());
            elements.back().dbl_coeffs.back().c_J =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_J").front());
            elements.back().dbl_coeffs.back().c_L =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_L").front());
        }

        for (auto &elem_coeffs : elem.second.get_child("add_coeffs")) {
            elements.back().add_coeffs.emplace_back(add_coeffs_type());

            elements.back().add_coeffs.back().c_L1 =
                field_element_init<add_coeffs_value_type>::process(elem_coeffs.second.get_child("c_L1").front());
            elements.back().add_coeffs.back().c_RZ =
                field_element_init<add_coeffs_value_type>::process(elem_coeffs.second.get_child("c_RZ").front());
        }
    }
}

template<typename TestSet>
void pairing_test_g2_precomp_init(std::vector<typename pairing::pairing_policy<curves::mnt6<298>>::g2_precomputed_type> &elements,
                                  const TestSet &test_set) {
    using curve_type = curves::mnt6<298>;
    using pairing_policy = typename pairing::pairing_policy<curve_type>;
    using value_type = typename pairing_policy::g2_precomputed_type;

    using g1_field_value_type = typename curve_type::base_field_type::value_type;
    using g2_field_value_type = typename curve_type::template g2_type<>::field_type::value_type;

    using dbl_coeffs_type = typename value_type::dbl_coeffs_type;
    using add_coeffs_type = typename value_type::add_coeffs_type;
    using dbl_coeffs_value_type = g2_field_value_type;
    using add_coeffs_value_type = g2_field_value_type;

    for (auto &elem : test_set.second.get_child("g2_precomputed_type")) {
        elements.emplace_back(value_type());

        elements.back().QX = field_element_init<g2_field_value_type>::process(elem.second.get_child("QX").front());
        elements.back().QY = field_element_init<g2_field_value_type>::process(elem.second.get_child("QY").front());
        elements.back().QY2 = field_element_init<g2_field_value_type>::process(elem.second.get_child("QY2").front());
        elements.back().QX_over_twist =
            field_element_init<g2_field_value_type>::process(elem.second.get_child("QX_over_twist").front());
        elements.back().QY_over_twist =
            field_element_init<g2_field_value_type>::process(elem.second.get_child("QY_over_twist").front());

        for (auto &elem_coeffs : elem.second.get_child("dbl_coeffs")) {
            elements.back().dbl_coeffs.emplace_back(dbl_coeffs_type());

            elements.back().dbl_coeffs.back().c_H =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_H").front());
            elements.back().dbl_coeffs.back().c_4C =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_4C").front());
            elements.back().dbl_coeffs.back().c_J =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_J").front());
            elements.back().dbl_coeffs.back().c_L =
                field_element_init<dbl_coeffs_value_type>::process(elem_coeffs.second.get_child("c_L").front());
        }

        for (auto &elem_coeffs : elem.second.get_child("add_coeffs")) {
            elements.back().add_coeffs.emplace_back(add_coeffs_type());

            elements.back().add_coeffs.back().c_L1 =
                field_element_init<add_coeffs_value_type>::process(elem_coeffs.second.get_child("c_L1").front());
            elements.back().add_coeffs.back().c_RZ =
                field_element_init<add_coeffs_value_type>::process(elem_coeffs.second.get_child("c_RZ").front());
        }
    }
}

template<typename PairingT, typename Fr_value_type, typename G1_value_type, typename G2_value_type,
         typename GT_value_type, typename g1_precomp_value_type, typename g2_precomp_value_type, typename TestSet>
void pairing_test_init(std::vector<Fr_value_type> &Fr_elements,
                       std::vector<G1_value_type> &G1_elements,
                       std::vector<G2_value_type> &G2_elements,
                       std::vector<GT_value_type> &GT_elements,
                       std::vector<g1_precomp_value_type> &G1_prec_elements,
                       std::vector<g2_precomp_value_type> &G2_prec_elements,
                       const TestSet &test_set) {
    pairing_test_Fr_init(Fr_elements, test_set);
    pairing_test_G1_init<PairingT>(G1_elements, test_set);
    pairing_test_G2_init<PairingT>(G2_elements, test_set);
    pairing_test_GT_init<PairingT>(GT_elements, test_set);
    pairing_test_g1_precomp_init(G1_prec_elements, test_set);
    pairing_test_g2_precomp_init(G2_prec_elements, test_set);
}

template<typename CurveType, typename TestSet>
void pairing_operation_test(const TestSet &test_set) {
    std::vector<typename CurveType::scalar_field_type::value_type> Fr_elements;
    std::vector<typename CurveType::template g1_type<>::value_type> G1_elements;
    std::vector<typename CurveType::template g2_type<>::value_type> G2_elements;
    std::vector<typename CurveType::gt_type::value_type> GT_elements;
    std::vector<typename pairing::pairing_policy<CurveType>::g1_precomputed_type> G1_prec_elements;
    std::vector<typename pairing::pairing_policy<CurveType>::g2_precomputed_type> G2_prec_elements;

    pairing_test_init<CurveType>(Fr_elements, G1_elements, G2_elements, GT_elements, G1_prec_elements, G2_prec_elements,
                                test_set);
    check_pairing_operations<CurveType>(Fr_elements, G1_elements, G2_elements, GT_elements, G1_prec_elements,
                                       G2_prec_elements);
}

BOOST_AUTO_TEST_SUITE(curves_manual_tests)

// TODO: fix pair_reduceding
BOOST_DATA_TEST_CASE(pairing_operation_test_bls12_381, string_data("pairing_operation_test_bls12_381"), data_set) {
    using curve_type = typename curves::bls12<381>;

    pairing_operation_test<curve_type>(data_set);
}

BOOST_DATA_TEST_CASE(pairing_operation_test_mnt4_298, string_data("pairing_operation_test_mnt4_298"), data_set) {
    using curve_type = typename curves::mnt4<298>;

    pairing_operation_test<curve_type>(data_set);
}

BOOST_DATA_TEST_CASE(pairing_operation_test_mnt6_298, string_data("pairing_operation_test_mnt6_298"), data_set) {
    using curve_type = typename curves::mnt6<298>;

    pairing_operation_test<curve_type>(data_set);
}

BOOST_AUTO_TEST_SUITE_END()


#include "lelantus_test_fixture.h"

#include "../src/lelantus_prover.h"
#include "../src/lelantus_verifier.h"

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

namespace lelantus {

class ProtocolTests : public LelantusTestingSetup
{
public:
    ProtocolTests()
        : params(Params::get_default())
    {
    }

public:
    std::vector<PublicCoin> ExtractPublicCoins(std::vector<PrivateCoin> const &coins) const {
        std::vector<PublicCoin> pubs;
        pubs.reserve(coins.size());

        for (auto const &c : coins) {
            pubs.push_back(c.getPublicCoin());
        }

        return pubs;
    }

    std::vector<Scalar> ExtractSerials(
        size_t anonymitySets,
        std::vector<std::pair<PrivateCoin, uint32_t>> const &Cin,
        std::vector<uint32_t>& groupIds) const {
        std::vector<Scalar> serials;
        for (auto const &in : Cin) {
            serials.push_back(in.first.getSerialNumber());
            groupIds.push_back(in.second);
        }

        return serials;
    }

    std::map<uint32_t, std::vector<PublicCoin>> GenerateAnonymitySets(std::initializer_list<size_t> sizes) const {
        std::map<uint32_t, std::vector<PublicCoin>> sets;

        uint32_t id = 0;
        for (size_t s : sizes) {
            std::vector<PublicCoin> set;
            GenerateGroupElements(s, std::back_inserter(set));
            sets[id] = set;
            id++;
        }

        return sets;
    }

public:
    Params const *params;
};

BOOST_FIXTURE_TEST_SUITE(lelantus_protocol_tests, ProtocolTests)

BOOST_AUTO_TEST_CASE(prove_verify)
{
    auto* params = Params::get_default();
    size_t N = 100;

    uint64_t v1(5);
    PrivateCoin input_coin1(params ,v1);
    std::vector<std::pair<PrivateCoin, uint32_t>> Cin = {{input_coin1, 0}};

    std::vector <size_t> indexes = {0};

    auto anonymity_sets = GenerateAnonymitySets({N});
    anonymity_sets[0][0] = Cin[0].first.getPublicCoin();

    Scalar Vin(5);
    uint64_t Vout(6);
    std::vector<PrivateCoin> Cout = {{params, 2}, {params, 1}};

    uint64_t f(1);
    LelantusProof proof;
    SchnorrProof qkSchnorrProof;

    LelantusProver prover(params, LELANTUS_TX_VERSION_4_5);
    prover.proof(anonymity_sets, {}, Vin, Cin, indexes, {}, Vout, Cout, f,  proof, qkSchnorrProof);

    std::vector<uint32_t> groupIds;
    auto Sin = ExtractSerials(anonymity_sets.size(), Cin, groupIds);
    auto Cout_Public = ExtractPublicCoins(Cout);

    lelantus::LelantusVerifier verifier(params, LELANTUS_TX_VERSION_4_5);
    BOOST_CHECK(verifier.verify(anonymity_sets, {}, Sin, {}, groupIds, Vin, Vout, f, Cout_Public, proof, qkSchnorrProof));
}

BOOST_AUTO_TEST_CASE(prove_verify_many_coins)
{
    size_t N = 100;

    PrivateCoin input1(params ,2), input2(params, 2), input3(params, 1);
    std::vector<std::pair<PrivateCoin, uint32_t>> Cin = {
        {input1, 0}, {input2, 0}, {input3, 1}
    };

    std::vector <size_t> indexes = {0, 1, 0};

    auto anonymity_sets = GenerateAnonymitySets({N, N});
    anonymity_sets[0][0] = Cin[0].first.getPublicCoin();
    anonymity_sets[0][1] = Cin[1].first.getPublicCoin();
    anonymity_sets[1][0] = Cin[2].first.getPublicCoin();

    Scalar Vin(5);
    uint64_t Vout(6), f(1);
    std::vector<PrivateCoin> Cout = {{params, 2}, {params, 1}};

    LelantusProof proof;
    SchnorrProof qkSchnorrProof;

    LelantusProver prover(params, LELANTUS_TX_VERSION_4_5);
    prover.proof(anonymity_sets, {}, Vin, Cin, indexes, {}, Vout, Cout, f,  proof, qkSchnorrProof);

    std::vector<uint32_t> groupIds;
    auto Sin = ExtractSerials(anonymity_sets.size(), Cin, groupIds);
    auto Cout_Public = ExtractPublicCoins(Cout);

    lelantus::LelantusVerifier verifier(params, LELANTUS_TX_VERSION_4_5);
    BOOST_CHECK(verifier.verify(anonymity_sets, {}, Sin, {}, groupIds, Vin, Vout, f, Cout_Public, proof, qkSchnorrProof));
    //After Lelantus new update (after version LELANTUS_TX_VERSION_4_5) following 2 verifications will fail, as schnorr proof challenge depends also on Vout, Vin  and fee values
//    BOOST_CHECK(verifier.verify(anonymity_sets, {}, Sin, {}, groupIds, Vin + 1, Vout + 1, f, Cout_Public, proof));
//    BOOST_CHECK(verifier.verify(anonymity_sets, {}, Sin, {}, groupIds, Vin, Vout + f, uint64_t(0), Cout_Public, proof));
}

BOOST_AUTO_TEST_CASE(imbalance_proof_should_fail)
{
    size_t N = 100;

    // Input
    PrivateCoin p1(params, 3);
    std::vector<std::pair<PrivateCoin, uint32_t>> Cin = {{p1, 0}};

    std::vector<size_t> indexs = {0};

    auto anonymitySets = GenerateAnonymitySets({N});
    anonymitySets[0][0] = Cin[0].first.getPublicCoin();

    Scalar Vin(2); // Use this to verify
    Scalar FakeVin(4); // Use this to generate proof

    // Output
    std::vector<PrivateCoin> Cout = {{params, 3}};
    uint64_t Vout(3);
    uint64_t f(1);

    // Proof
    LelantusProof proof;
    SchnorrProof qkSchnorrProof;

    // Should be prevent from prover
    LelantusProver prover(params, LELANTUS_TX_VERSION_4_5);
    BOOST_CHECK_THROW(prover.proof(anonymitySets, {}, Vin, Cin, indexs, {}, Vout, Cout, f, proof, qkSchnorrProof), std::runtime_error);

    // Use fake vin
    prover.proof(anonymitySets, {}, FakeVin, Cin, indexs, {}, Vout, Cout, f, proof, qkSchnorrProof);

    // Verify
    std::vector<uint32_t> groupIds;
    auto Sin = ExtractSerials(anonymitySets.size(), Cin, groupIds);
    auto publicCoins = ExtractPublicCoins(Cout);

    LelantusVerifier verifier(params, LELANTUS_TX_VERSION_4_5);

    // input: 2 + 3(anonymous), output: 3 + 3(anonymous) + 1(fee)
    BOOST_CHECK(!verifier.verify(anonymitySets, {}, Sin, {}, groupIds, Vin, Vout, f, publicCoins, proof, qkSchnorrProof));

    // Verify with output which is less than input also should fail
    // input: 99 + 3(anonymous), output: 3 + 3(anonymous) + 1(fee)
    Scalar newVin(99);
    BOOST_CHECK(!verifier.verify(anonymitySets, {}, Sin, {}, groupIds, newVin, Vout, f, publicCoins, proof, qkSchnorrProof));
}

BOOST_AUTO_TEST_CASE(other_fail_to_validate)
{
    size_t N = 100;

    // Input
    PrivateCoin p1(params, 1), p2(params, 2);
    std::vector<std::pair<PrivateCoin, uint32_t>> Cin = {{p1, 0}, {p2, 0}};

    std::vector<size_t> indexs = {0, 1};

    auto anonymitySets = GenerateAnonymitySets({N});
    anonymitySets[0][0] = Cin[0].first.getPublicCoin();
    anonymitySets[0][1] = Cin[1].first.getPublicCoin();

    Scalar Vin(4);

    // Output
    std::vector<PrivateCoin> Cout = {{params, 1}, {params, 2}};
    uint64_t Vout(3);
    uint64_t f(1);

    // Proof
    LelantusProof proof;
    SchnorrProof qkSchnorrProof;

    // Should be prevent from prover
    LelantusProver prover(params, LELANTUS_TX_VERSION_4_5);
    prover.proof(anonymitySets, {}, Vin, Cin, indexs, {}, Vout, Cout, f, proof, qkSchnorrProof);

    // Verify
    std::vector<uint32_t> groupIds;
    auto Sin = ExtractSerials(anonymitySets.size(), Cin, groupIds);
    auto publicCoins = ExtractPublicCoins(Cout);

    LelantusVerifier verifier(params, LELANTUS_TX_VERSION_4_5);

    BOOST_CHECK(verifier.verify(anonymitySets, {}, Sin, {}, groupIds, Vin, Vout, f, publicCoins, proof, qkSchnorrProof));

    // Invalid group
    auto invalidAnonymitySets = anonymitySets;
    invalidAnonymitySets[0].pop_back();
    BOOST_CHECK(!verifier.verify(invalidAnonymitySets, {}, Sin, {}, groupIds, Vin, Vout, f, publicCoins, proof, qkSchnorrProof));

    invalidAnonymitySets = anonymitySets;
    invalidAnonymitySets[0].push_back(PrivateCoin(params, 1).getPublicCoin());
    BOOST_CHECK(!verifier.verify(invalidAnonymitySets, {}, Sin, {}, groupIds, Vin, Vout, f, publicCoins, proof, qkSchnorrProof));

    // Invalid serial
    auto invalidSin = Sin;
    invalidSin[1].randomize();
    BOOST_CHECK(!verifier.verify(anonymitySets, {}, invalidSin, {}, groupIds, Vin, Vout, f, publicCoins, proof, qkSchnorrProof));
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/special_functions/spherical_harmonic.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/special_functions/spherical_harmonic.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
inline void check_result_imp(std::complex<float>, std::complex<float>){}
inline void check_result_imp(std::complex<double>, std::complex<double>){}
inline void check_result_imp(std::complex<long double>, std::complex<long double>){}

#include "test_compile_result.hpp"

void check()
{
   check_result<std::complex<float> >(boost::math::spherical_harmonic<float>(u, i, f, f));
   check_result<std::complex<double> >(boost::math::spherical_harmonic<double>(u, i, d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<std::complex<long double> >(boost::math::spherical_harmonic<long double>(u, i, l, l));
#endif

   check_result<float>(boost::math::spherical_harmonic_r<float>(u, i, f, f));
   check_result<double>(boost::math::spherical_harmonic_r<double>(u, i, d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::spherical_harmonic_r<long double>(u, i, l, l));
#endif

   check_result<float>(boost::math::spherical_harmonic_i<float>(u, i, f, f));
   check_result<double>(boost::math::spherical_harmonic_i<double>(u, i, d, d));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   check_result<long double>(boost::math::spherical_harmonic_i<long double>(u, i, l, l));
#endif
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * 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 The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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.
//
// Please contact the author of this file if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <math.h>
#include <glog/logging.h>
#include <Eigen/Core>
#include <ctime>
#include <random>
#include "gtest/gtest.h"

#include "theia/test/test_utils.h"
#include "theia/sfm/pose/four_point_focal_length.h"

namespace {
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector3d;

void P4pfTestWithNoise(const Matrix3d& gt_rotation,
                       const Vector3d& gt_translation,
                       const double focal_length,
                       const std::vector<Vector3d>& world_points_vector,
                       const double noise,
                       const double reproj_tolerance) {
  Map<const Matrix<double, 3, 4> > world_points(world_points_vector[0].data());

  // Camera intrinsics matrix.
  const Matrix3d camera_matrix =
      Eigen::DiagonalMatrix<double, 3>(focal_length, focal_length, 1.0);
  // Create the projection matrix P = K * [R t].
  Matrix<double, 3, 4> gt_projection;
  gt_projection << gt_rotation, gt_translation;
  gt_projection = camera_matrix * gt_projection;

  // Reproject 3D points to get undistorted image points.
  std::vector<Eigen::Vector2d> image_points_vector(5);
  Map<Matrix<double, 2, 4> > image_point(image_points_vector[0].data());
  image_point = (gt_projection * world_points.colwise().homogeneous()).colwise()
      .hnormalized();

  // Add noise to distorted image points.
  if (noise) {
    std::default_random_engine generator;
    std::normal_distribution<double> distribution(0.0, noise);
    for (int i = 0; i < 4; i++) {
      image_point.col(i).x() += distribution(generator);
      image_point.col(i).y() += distribution(generator);
    }
  }

  // Run P5pf algorithm.
  std::vector<Matrix<double, 3, 4> > soln_projection;
  int num_solns = theia::FourPointPoseAndFocalLength(
      image_points_vector, world_points_vector, &soln_projection);
  ASSERT_GT(num_solns, 0);

  bool matched_transform = false;
  for (int i = 0; i < num_solns; ++i) {
    matched_transform = true;
    // Check that the reprojection error is very small.
    for (int n = 0; n < 4; n++) {
      Vector3d reproj_point =
          soln_projection[i] * world_points.col(n).homogeneous();
      const double reproj_error =
          (reproj_point.hnormalized() - image_point.col(n)).norm();
      if (reproj_error > reproj_tolerance) {
        matched_transform = false;
        break;
      }
    }
    if (matched_transform) {
      break;
    }
  }
  // One of the solutions must have been a valid solution.
  EXPECT_TRUE(matched_transform);
}

void BasicTest(const double noise, const double reproj_tolerance) {
  const double focal_length = 800;

  const double x = -0.10;  // rotation of the view around x axis
  const double y = -0.20;  // rotation of the view around y axis
  const double z = 0.30;   // rotation of the view around z axis

  // Create a ground truth pose.
  Matrix3d Rz, Ry, Rx;
  Rz << cos(z), sin(z), 0,
        -sin(z), cos(z), 0,
        0, 0, 1;
  Ry << cos(y), 0, -sin(y),
        0, 1, 0,
        sin(y), 0, cos(y);
  Rx << 1, 0, 0,
        0, cos(x), sin(x),
        0, -sin(x), cos(x);
  const Matrix3d gt_rotation = Rz * Ry * Rx;
  const Vector3d gt_translation = Vector3d(-0.00950692, 0.0171496, 0.0508743);

  // Create 3D world points that are viable based on the camera intrinsics and
  // extrinsics.
  std::vector<Vector3d> world_points_vector = { Vector3d(-1.0, 0.5, 1.2),
                                                Vector3d(-0.79, -0.68, 1.9),
                                                Vector3d(1.42, 1.01, 2.19),
                                                Vector3d(0.87, -0.49, 0.89) };

  P4pfTestWithNoise(gt_rotation, gt_translation, focal_length,
                    world_points_vector, noise, reproj_tolerance);
}

void RandomTestWithNoise(const double noise, const double reproj_tolerance) {
  // Seed random number generator.
  srand(time(NULL));

  const double kBaseline = 0.25;

  // focal length (values used in the ICCV paper)
  std::default_random_engine generator;
  std::uniform_real_distribution<double> distribution(0.0, 1.0);
  const double focal_length = distribution(generator) * 50.0 + 600;

  // Rotation areound x, y, z axis.
  const double x = distribution(generator) * 0.5 - 0.25;
  const double y = distribution(generator) * 0.5 - 0.25;
  const double z = distribution(generator) * 0.5 - 0.25;

  // Create a ground truth pose.
  Matrix3d Rz, Ry, Rx;
  Rz << cos(z), sin(z), 0,
        -sin(z), cos(z), 0,
        0, 0, 1;
  Ry << cos(y), 0, -sin(y),
        0, 1, 0,
        sin(y), 0, cos(y);
  Rx << 1, 0, 0,
        0, cos(x), sin(x),
        0, -sin(x), cos(x);
  const Matrix3d gt_rotation = Rz * Ry * Rx;
  const Vector3d gt_translation = Vector3d::Random() * kBaseline;

  // Create 3D world points that are viable based on the camera intrinsics and
  // extrinsics.
  std::vector<Vector3d> world_points_vector(4);
  Map<Matrix<double, 3, 4> > world_points(world_points_vector[0].data());
  world_points.row(2) = 2.0 * Matrix<double, 1, 4>::Random().array() + 2.0;
  world_points.row(1) = 2.0 * Matrix<double, 1, 4>::Random();
  world_points.row(0) = 2.0 * Matrix<double, 1, 4>::Random();

  P4pfTestWithNoise(gt_rotation, gt_translation, focal_length,
                    world_points_vector, noise, reproj_tolerance);
}

TEST(P4pf, BasicTest) {
  BasicTest(0.0, 1e-4);
}

TEST(P4pf, BasicNoiseTest) {
  BasicTest(0.5, 10.0);
}

TEST(P4pf, RandomTest) {
  RandomTestWithNoise(0.0, 0.1);
}

}  // namespace