//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing successor capabilities

    Returns the n-th least element strictly greater than the parameter

    @par Semantic:

    @code
    auto r = successor(x,n);
    @endcode

    If n is null returns x else computes the @c n-th least representable value strictly greater than x in its type.
    n must be positive or null.

    For integer it saturate at @ref Valmax. For floating point numbers, all @ref Inf
    strict successors are @ref Nan.

  **/
  Value successor(Value const& x, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/successor.hpp>
#include <boost/simd/function/simd/successor.hpp>

#endif


#pragma once

#include <Eigen/Dense>
#include <unordered_map>
#include <vector>
#include "TriangleMesh.hpp"

namespace LayoutMaker {

std::vector<uint32_t> get_mapping_optimized_layout(
	const std::shared_ptr<TriangleMesh> p_mesh,
	const uint32_t max_depth,
	const uint32_t max_cluster_size,
	const uint32_t max_spectral_size,
	const uint32_t max_number_interations_eigen,
	const float eigen_error
);
}


#include <iostream>

#include <boost/dynamic_bitset.hpp>
#include "crc.hpp"

//  Simple CRC class function definitions  -----------------------------------//

my_crc_basic::my_crc_basic( unsigned int width,
			    my_crc_basic::value_type  truncated_polynominal,
			    my_crc_basic::value_type  initial_remainder,      // = 0
			    my_crc_basic::value_type  final_xor_value,        // = 0
			    bool                                  reflect_input,          // = false
			    bool                                  reflect_remainder       // = false
			    )
  : width_(width), rem_( initial_remainder ), poly_( truncated_polynominal )
  , init_( initial_remainder ), final_( final_xor_value )
  , rft_in_( reflect_input ), rft_out_( reflect_remainder )
{
  high_bit_mask = 1ul << ( width - 1u );
  sig_bits = (~( ~( 0ul ) << width )) ;

}

void my_crc_basic::set(my_crc_basic::value_type  truncated_polynominal,
		       my_crc_basic::value_type  initial_remainder,      // = 0
		       my_crc_basic::value_type  final_xor_value,        // = 0
		       bool                                  reflect_input,          // = false
		       bool                                  reflect_remainder       // = false
		       ) {
  rem_ = initial_remainder;
  poly_ =  truncated_polynominal;
  init_ = initial_remainder;
  final_ = final_xor_value;
  rft_in_ = reflect_input;
  rft_out_ = reflect_remainder;
}

void my_crc_basic::reset(my_crc_basic::value_type  new_rem) {
  rem_ = new_rem;
}

void my_crc_basic::process_bit (bool bit) {

  // compare the new bit with the remainder's highest
  rem_ ^= ( bit ? high_bit_mask : 0u );

  // a full polynominal division step is done when the highest bit is one
  bool const  do_poly_div = static_cast<bool>( rem_ & high_bit_mask );

  // shift out the highest bit
  rem_ <<= 1;

  // carry out the division, if needed
  if ( do_poly_div ) rem_ ^= poly_;

}

my_crc_basic::value_type my_crc_basic::checksum() const {
  return ( (rft_out_ ? reflect( rem_ ) : rem_)
	   ^ final_ ) & sig_bits;
}

void my_crc_basic::calc_crc(value_type const use_initial,
			    boost::dynamic_bitset<> const & msg) {
  this->calc_crc(use_initial, msg, 0, AUTO);
}

bool my_crc_basic::calc_crc(value_type const use_initial,
			    boost::dynamic_bitset<> const& msg,
			    value_type const expected_crc,
			    FEED_TYPE feed_type) {

  reset(use_initial);

  if(feed_type == AUTO) {
    if(rft_in_) {
      if (msg.size() % 8 != 0) feed_type = LINEAR_REVERSED;
      else feed_type = BYTEWISE_REVERSED;
    }
    else {
      feed_type = LINEAR_FORWARD;
    }
  }

  if(feed_type == LINEAR_FORWARD) {
    for (size_t i = 0; i < msg.size(); i++) {
      process_bit(msg[i]);
    }
  }
  else if(feed_type == LINEAR_REVERSED) {
    for (int i = msg.size()-1; i >= 0; i--) {
      process_bit(msg[i]);
    }
  }
  else if(feed_type == BYTEWISE_REVERSED) {

      for(size_t i = 0; i < msg.size(); i+=8) {
	// inverse feeding
	for(int j = 1; j <= 8; j++)
	  process_bit(msg[i + 8 - j]);
      }
  }

  return checksum() == expected_crc;
}

bool my_crc_basic::calc_crc(value_type const use_initial,
			    uint8_t msg[], size_t msg_length,
			    value_type const expected_crc) {

	reset(use_initial);

	//process_bytes(msg, msg_length);

	return checksum() == expected_crc;

}

// Function that reflects its argument
my_crc_basic::value_type my_crc_basic::reflect(value_type  x ) const {

  value_type        reflection = 0;
  value_type const  one = 1;

  for( std::size_t i = 0 ; i < width_ ; ++i, x >>= 1 ) {
    if ( x & one ) {
      reflection |= ( one << (width_ - 1u - i) );
    }
  }

  return reflection;
}


#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "IO/readPLY.h"
#include "IO/writePLY.h"

#include "compute_sdf.h"
#include "print_tensor.h"
#include "polyscope/polyscope.h"

int main() {
    bool visualization = true;
    int grid_resolution = 100;    // The code goes nuts if the resolution is too high, this is probably a memory issue
    double bounding_box_scale = 1;

    // IO: load files
    std::cout << "Progress: load data\n";
    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    Eigen::MatrixXd N;
    Eigen::MatrixXi RGB;
    readPLY("../data/Lucy100k.ply", V, F, N, RGB);
    //readPLY("../data/david.ply", V, F, N, RGB);
    std::cout << "Progress: data loaded\n";

    // put it into rowWise
    Eigen::MatrixXf Vf;
    Eigen::MatrixXi Ff;
    Vf = V.transpose().cast <float> ();
    Ff = F.transpose();
    std::cout << "size of V: [" << Vf.rows() << ", " << Vf.cols() << "]\n";
    std::cout << "size of F: [" << Ff.rows() << ", " << Ff.cols() << "]\n";

    // get the sdf
    SDF sdf(Vf.data(), Vf.rows(), Ff.data(), Ff.rows(), grid_resolution);

    std::cout << "Progress: class created\n";

    int Dx, Dy, Dz;
    sdf.get_tensor_size(Dx, Dy, Dz);
    std::cout << "Tensor size: \n";
    std::cout << "Dx: " << Dx << "\n";
    std::cout << "Dy: " << Dy << "\n";
    std::cout << "Dz: " << Dz << "\n";

    float* sdf_array = (float*) malloc( Dx * Dy * Dz * sizeof(float));
    sdf.get_tensor(sdf_array);

    std::cout << "Progress: array collected\n";

    // transform back to eigen tensor (this is not the smartest way to do it, probably some cast thingy would be better)
    Eigen::Tensor<float, 3> sdf_tensor;
    sdf_tensor.resize(Dx, Dy, Dz);
    for (int x = 0; x < Dx; x++)
        for (int y = 0; y < Dy; y++)
            for (int z = 0; z < Dz; z++)
                sdf_tensor(x, y, z) = sdf_array[x + y*Dx + z*Dx*Dy];

    print_to_folder("../data/image_stack/", sdf_tensor);

    free(sdf_array);
    sdf.clear_memory();

}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 exponential toolbox - pow/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// Test behavior of exponential components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 08/12/2010
/// modified by jt the 23/03/2011
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/infinites.hpp>
#include <nt2/include/functions/ulpdist.hpp>
#include <nt2/toolbox/exponential/include/pow.hpp>
// specific includes for arity 2 tests
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/sqrt.hpp>
extern "C" { long double cephes_powl(long double,long double); }
// specific includes for arity 2 tests
extern "C" { long double cephes_powil(long double,int); }

NT2_TEST_CASE_TPL ( pow_real__2_0,  NT2_REAL_TYPES)
{

  using nt2::pow;
  using nt2::tag::pow_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<pow_(T,T)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::floating(T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<T>(), nt2::Inf<T>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<T>(), nt2::Minf<T>()), nt2::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<T>(), nt2::Mone<T>()), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<T>(), nt2::Nan<T>()), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<r_t>(), 0);
  // random verifications
  static const uint32_t NR = NT2_NB_RANDOM_TEST;
  {
    NT2_CREATE_BUF(tab_a0,T, NR, T(0), T(10));
    NT2_CREATE_BUF(tab_a1,T, NR, T(0), T(10));
    double ulp0, ulpd ; ulpd=ulp0=0.0;
    T a0;
    T a1;
    for (uint32_t j =0; j < NR; ++j )
      {
        std::cout << "for params "
                  << "  a0 = "<< u_t(a0 = tab_a0[j])
                  << ", a1 = "<< u_t(a1 = tab_a1[j])
                  << std::endl;
        NT2_TEST_ULP_EQUAL( nt2::pow(a0,a1),::cephes_powl(a0,a1),9);
        ulp0=nt2::max(ulpd,ulp0);
        NT2_TEST_ULP_EQUAL( nt2::pow(a0,nt2::Two<T>()),nt2::sqr(a0),1);
        ulp0=nt2::max(ulpd,ulp0);
        NT2_TEST_ULP_EQUAL( nt2::pow(a0,nt2::Half<T>()),nt2::sqrt(a0),1);
        ulp0=nt2::max(ulpd,ulp0);
     }
     std::cout << "max ulp found is: " << ulp0 << std::endl;
   }
} // end of test for real_

NT2_TEST_CASE_TPL ( pow_unsigned_int__2_0,  NT2_UNSIGNED_TYPES)
{

  using nt2::pow;
  using nt2::tag::pow_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<pow_(T,T)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::floating(T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<r_t>(), 0);
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( pow_signed_int__2_0,  NT2_INTEGRAL_SIGNED_TYPES)
{

  using nt2::pow;
  using nt2::tag::pow_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<pow_(T,T)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::floating(T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<T>(), nt2::Mone<T>()), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(), nt2::Zero<T>()), nt2::One<r_t>(), 0);
} // end of test for signed_int_

NT2_TEST_CASE_TPL ( pow_real__2_1,  NT2_REAL_TYPES)
{

  using nt2::pow;
  using nt2::tag::pow_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<pow_(T,iT)>::type r_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;
  typedef typename boost::result_of<nt2::meta::floating(T)>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;
  double ulpd;
  ulpd=0.0;

  // specific values tests
  NT2_TEST_ULP_EQUAL(pow(nt2::Inf<T>(),3), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Minf<T>(),3), nt2::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Mone<T>(),3), nt2::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Nan<T>(),3), nt2::Nan<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::One<T>(),3), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(pow(nt2::Zero<T>(),3), nt2::Zero<r_t>(), 0);
  // random verifications
  static const uint32_t NR = NT2_NB_RANDOM_TEST;
  {
    NT2_CREATE_BUF(tab_a0,T, NR, T(-10), T(10));
    NT2_CREATE_BUF(tab_a1,iT, NR, T(-10), T(10));
    double ulp0, ulpd ; ulpd=ulp0=0.0;
    T a0;
    iT a1;
    for (uint32_t j =0; j < NR; ++j )
      {
        std::cout << "for params "
                  << "  a0 = "<< u_t(a0 = tab_a0[j])
                  << ", a1 = "<< u_t(a1 = tab_a1[j])
                  << std::endl;
        NT2_TEST_ULP_EQUAL( nt2::powi(a0,a1),cephes_powil(a0,a1),2);
        ulp0=nt2::max(ulpd,ulp0);
        NT2_TEST_ULP_EQUAL( nt2::powi(a0,nt2::Two<iT>()),nt2::sqr(a0),2);
        ulp0=nt2::max(ulpd,ulp0);
        NT2_TEST_ULP_EQUAL( nt2::powi(a0,nt2::Three<iT>()),a0*nt2::sqr(a0),2);
        ulp0=nt2::max(ulpd,ulp0);
     }
     std::cout << "max ulp found is: " << ulp0 << std::endl;
   }
} // end of test for real_


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014-2016
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_ALGORITHMS_CAMERA_RESPONSE_FUNCTION_HPP
#define PIC_ALGORITHMS_CAMERA_RESPONSE_FUNCTION_HPP

#include <algorithm>

#include "../image.hpp"
#include "../point_samplers/sampler_random.hpp"
#include "../histogram.hpp"
#include "../filtering/filter_mean.hpp"
#include "../util/polynomial.hpp"

#include "../algorithms/sub_sample_stack.hpp"
#include "../algorithms/weight_function.hpp"
#include "../algorithms/mitsunaga_nayar_crf.hpp"

#ifndef PIC_DISABLE_EIGEN
    #ifndef PIC_EIGEN_NOT_BUNDLED
        #include "../externals/Eigen/SVD"
    #else
        #include <Eigen/SVD>
    #endif
#endif

namespace pic {

enum IMG_LIN {IL_LIN, IL_2_2, IL_LUT_8_BIT, IL_POLYNOMIAL};

/**
 * @brief The CameraResponseFunction class
 */
class CameraResponseFunction
{
protected:

    /**
    * \brief gsolve computes the inverse CRF of a camera.
    */
    float *gsolve(int *samples, std::vector< float > &log_exposure, float lambda, int nSamples)
    {
        #ifndef PIC_DISABLE_EIGEN

        int nExposure = int(log_exposure.size());

        int n = 256;
        int rows = nSamples * nExposure + n + 1;
        int cols = n + nSamples;

        #ifdef PIC_DEBUG
            printf("Matrix size: (%d, %d)\n", rows, cols);
        #endif

        Eigen::MatrixXf A = Eigen::MatrixXf::Zero(rows, cols);
        Eigen::VectorXf b = Eigen::VectorXf::Zero(rows);

        int k = 0;

        for(int i = 0; i < nSamples; i++) {
            for(int j = 0; j < nExposure; j++) {
                int tmp = samples[i * nExposure + j];

                float w_ij = w[tmp];

                A.coeffRef(k, tmp) =  w_ij;
                A.coeffRef(k, n + i) = -w_ij;

                b[k] =  w_ij * log_exposure[j];

                k++;
            }
        }

        A.coeffRef(k, 128) = 1.0f;
        k++;

        //smoothness term
        for(int i = 0; i < (n - 2); i++) {
            float w_l = lambda * w[i + 1];
            A.coeffRef(k, i)     =         w_l;
            A.coeffRef(k, i + 1) = -2.0f * w_l;
            A.coeffRef(k, i + 2) =         w_l;
            k++;
        }

        //solve the linear system
        Eigen::JacobiSVD< Eigen::MatrixXf > svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

        Eigen::VectorXf x = svd.solve(b);

        float *ret = new float[n];

        for(int i = 0; i < n; i++) {
            ret[i] = expf(x[i]);
        }

        #else
            float *ret = NULL;
        #endif

        return ret;
    }

    /**
     * @brief release frees memory.
     */
    void release()
    {
        stackOut.release();

        for(unsigned int i = 0; i < icrf.size(); i++) {
            if(icrf[i] != NULL) {
                delete[] icrf[i];
                icrf[i] = NULL;
            }
        }

        for(unsigned int i = 0; i < crf.size(); i++) {
            if(crf[i] != NULL) {
                delete[] crf[i];
                crf[i] = NULL;
            }
        }

        icrf.clear();
        crf.clear();
        poly.clear();
    }

    /**
     * @brief createTabledICRF
     */
    void createTabledICRF()
    {
        if(type_linearization != IL_POLYNOMIAL) {
            return;
        }

        for(unsigned int i = 0; i < icrf.size(); i++) {
            if(icrf[i] != NULL) {
                delete[] icrf[i];
            }
        }

        icrf.clear();

        for(unsigned int i = 0; i < poly.size(); i++) {
            float *tmp = new float[256];

            for(int j = 0; j < 256; j++) {
                float x = float(j) / 255.0f;

                tmp[j] = poly[i].eval(x);
            }

            crf.push_back(tmp);
        }
    }

    SubSampleStack stackOut;
    IMG_LIN type_linearization;
    float w[256];

public:

    std::vector<float *> icrf;
    std::vector<float *> crf;

    std::vector< Polynomial > poly;

    /**
     * @brief CameraResponseFunction
     */
    CameraResponseFunction()
    {
        type_linearization = IL_LIN;
    }

    ~CameraResponseFunction()
    {
        release();
    }

    /**
     * @brief remove linearizes a camera value using the inverse CRF.
     * @param x is an intensity value in [0,1].
     * @param channel
     * @return It returns x in the linear domain.
     */
    inline float remove(float x, int channel)
    {
        switch(type_linearization) {
            case IL_LIN: {
                return x;
            }
            break;

            case IL_LUT_8_BIT: {
                int index =  CLAMP(int(round(x * 255.0f)), 256);
                return icrf.at(channel)[index];
            }
            break;

            case IL_2_2: {
                return powf(x, 2.2f);
            }
            break;

            case IL_POLYNOMIAL: {
                return poly[channel].eval(x);
            }
            break;

            default:
                break;
        }

        return x;
    }

    /**
     * @brief apply
     * @param x a value in [0, 1]
     * @param channel
     * @return
     */
    inline float apply(float x, int channel)
    {
        switch(type_linearization) {
            case IL_LIN: {
                return x;
            }
            break;

            case IL_LUT_8_BIT: {
                float *ptr = std::lower_bound(&icrf[channel][0], &icrf[channel][255], x);
                int offset = CLAMPi((int)(ptr - icrf[channel]), 0, 255);

                return float(offset) / 255.0f;
            }
            break;

            case IL_2_2: {
               #ifdef PIC_WIN32
                  float inv_gamma = 1.0f / 2.2f;
               #else
                  constexpr float inv_gamma = 1.0f / 2.2f;
               #endif

               return powf(x, inv_gamma);
            }
            break;

            case IL_POLYNOMIAL: {
                float *ptr = std::lower_bound(&icrf[channel][0], &icrf[channel][255], x);
                int offset = CLAMPi((int)(ptr - icrf[channel]), 0, 255);

                return float(offset) / 255.0f;
            }
            break;

            default:
                break;
        }

        return x;
    }

    /**
     * @brief setCRFtoGamma2_2
     */
    void setCRFtoGamma2_2()
    {
        type_linearization = IL_2_2;
    }

    /**
     * @brief setCRFtoLinear
     */
    void setCRFtoLinear()
    {
        type_linearization = IL_LIN;
    }

    /**
     * @brief FromRAWJPEG computes the CRF by exploiting the couple RAW/JPEG from cameras.
     * @param img_raw is a RAW image.
     * @param img_jpg is a JPEG compressed image.
     * @param filteringSize
     */
    void fromRAWJPEG(Image *img_raw, Image *img_jpg, int filteringSize = 11)
    {
        if((img_raw == NULL) || (img_jpg == NULL))
            return;

        if(!img_raw->isSimilarType(img_jpg))
            return;

        icrf.clear();

        int width    = img_raw->width;
        int height   = img_raw->height;
        int channels = img_raw->channels;

        int crf_size = 256 * 256 * channels;
        unsigned int *crf = new unsigned int[crf_size];

        for(int i=0;i<crf_size;i++) {
            crf[i] = 0;
        }

        for(int i=0; i<height; i++) {
            for(int j=0; j<width; j++) {

                float *data_raw = (*img_raw)(j, i);
                float *data_jpg = (*img_jpg)(j, i);

                for(int k=0;k<channels;k++) {
                    int i_raw = CLAMPi(int(255.0f * data_raw[k]), 0, 255);
                    int i_jpg = CLAMPi(int(255.0f * data_jpg[k]), 0, 255);

                    int addr = (i_raw * 256 + i_jpg ) * channels;

                    crf[addr + k ]++;
                }
            }
        }

        //compute the result
        std::vector< int > coords;

        for(int k=0;k<channels;k++) {

            float *ret_c = new float[256];

            for(int j=0;j<256;j++) {
                coords.clear();

                for(int i=0;i<256;i++) {

                    int addr = (i * 256 + j ) * channels + k;

                    if(crf[addr] > 0) {
                        coords.push_back(i);
                    }

                }

                if(!coords.empty()) {//get the median value
                    std::sort (coords.begin(), coords.end());
                    ret_c[j] = float(coords[coords.size() >> 1]) / 255.0f;
                }
            }

            if(filteringSize > 0) {
                Image toBeFiltered(1, 256, 1, 1, ret_c);

                Image *filtered = FilterMean::execute(&toBeFiltered, NULL, filteringSize);

                icrf.push_back(filtered->data);

            } else {
                icrf.push_back(ret_c);
            }
        }
    }

    /**
     * @brief DebevecMalik computes the CRF of a camera using multiple exposures value following Debevec and Malik
    1997's method.
     * @param stack
     * @param exposure
     * @param type
     * @param nSamples
     * @param lambda
     */
    void DebevecMalik(ImageVec stack, CRF_WEIGHT type = CW_DEB97, int nSamples = 256, float lambda = 20.0f)
    {
        release();

        if(!ImageVecCheckSimilarType(stack)) {
            return;
        }

        if(nSamples < 1) {
            nSamples = 256;
        }

        this->type_linearization = IL_LUT_8_BIT;

        //subsample the image stack
        stackOut.execute(stack, nSamples);

        int *samples = stackOut.get();
        nSamples = stackOut.getNSamples();

        //compute CRF using Debevec and Malik
        int channels = stack[0]->channels;

        //pre-compute the weight function
        for(int i = 0; i < 256; i++) {
            w[i] = weightFunction(float(i) / 255.0f, type);
        }

        int nExposure = int(stack.size());

        //log domain exposure time
        std::vector< float > log_exposures;
        ImaveVecGetExposureTimesAsArray(stack, log_exposures, true);

        #ifdef PIC_DEBUG
            printf("nSamples: %d\n", nSamples);
        #endif

        int stride = nSamples * nExposure;
        for(int i = 0; i < channels; i++) {
            float *icrf_channel = gsolve(&samples[i * stride], log_exposures, lambda, nSamples);

            icrf.push_back(icrf_channel);
        }
    }

    /**
     * @brief MitsunagaNayar computes the inverse CRF of a camera as a polynomial function.
     * @param stack             Array of images with associated exposure. Note that this array will be sorted with increasing exposure.
     * @param polynomial_degree Degree of the polynomial. If negative, the best degree will be selected in [1, -polynomial_degree] for each channel.
     * @param nSamples          Number of samples to extract from each image.
     * @param full              true for computing all exposure ratios (as in book "High Dynamic Range Imaging", second edition, Reinhard et al.),
     *                          false as in the original paper (only among successive exposures).
     * @param alpha             Threshold for removing samples with values not in [alpha, 1-alpha].
     * @param computeRatios     false if exact exposures are passed, true to approximate exposure ratios as in the paper.
     * @param eps               Threshold on the difference among successive approximations for stopping the computation.
     * @param max_iterations    Stop the computation after this number of iterations.
     * @return true if successfully computed, false otherwise.
     */
    bool MitsunagaNayar(ImageVec &stack, int polynomial_degree = -3, int nSamples = 256, const bool full = false,
                        const float alpha = 0.04f, const bool computeRatios = false, const float eps = 0.0001f,
                        const std::size_t max_iterations = 100)
    {
        release();

        if(!ImageVecCheckSimilarType(stack)) {
            return false;
        }

        if(nSamples < 1) {
            nSamples = 256;
        }

        type_linearization = IL_POLYNOMIAL;

        //sort the array by exposure
        ImaveVecSortByExposureTime(stack);

        //subsample the image stack
        stackOut.execute(stack, nSamples, alpha);
        int *samples = stackOut.get();
        nSamples = stackOut.getNSamples();

        if (nSamples < 1) {
            return false;
        }

        //compute the CRF using Mitsunaga and Nayar
        int channels = stack[0]->channels;

        std::size_t nExposures = stack.size();

        std::vector< float > exposures;
        ImaveVecGetExposureTimesAsArray(stack, exposures, false);

        int stride = nSamples * int(nExposures);

        float error = std::numeric_limits<float>::infinity();
        std::vector<float> R(nExposures - 1);
        std::vector<std::vector<float>> RR(nExposures - 1, std::vector<float>(nExposures - 1));

        poly.resize(channels);

        if (polynomial_degree > 0) {
            error = 0.f;
            for (int i = 0; i < channels; ++i) {
                poly[i].coeff.assign(polynomial_degree + 1, 0.f);
                if (full) {
                    error += MitsunagaNayarFull(&samples[i * stride], nSamples, exposures, poly[i].coeff, computeRatios, RR, eps, max_iterations);
                } else {
                    error += MitsunagaNayarClassic(&samples[i * stride], nSamples, exposures, poly[i].coeff, computeRatios, R, eps, max_iterations);
                }
            }
        } else if (polynomial_degree < 0) {
            error = std::numeric_limits<float>::infinity();
            std::vector<Polynomial> tmpCoefficients(channels);
            for (int degree = 1; degree <= -polynomial_degree; ++degree) {
                float tmpError = 0.f;
                for (int i = 0; i < channels; ++i) {
                    tmpCoefficients[i].coeff.resize(degree + 1);
                    if (full) {
                        tmpError += MitsunagaNayarFull(&samples[i * stride], nSamples, exposures, tmpCoefficients[i].coeff, computeRatios, RR, eps, max_iterations);
                    } else {
                        tmpError += MitsunagaNayarClassic(&samples[i * stride], nSamples, exposures, tmpCoefficients[i].coeff, computeRatios, R, eps, max_iterations);
                    }
                }

                if (tmpError < error) {
                    error = tmpError;
                    poly = std::move(tmpCoefficients);
                    tmpCoefficients.resize(channels);
                }
            }
        }

        bool bOk = error < std::numeric_limits<float>::infinity();

        if(bOk) {
            createTabledICRF();
        }

        return bOk;
    }

    /**
     * @brief Robertson computes the CRF of a camera using all multiple exposures value Robertson et al
       1999's method (Dynamic range improvement through multiple exposures).
     * @param stack
     * @param maxIterations
     */
    void Robertson(ImageVec &stack, const size_t maxIterations = 50)
    {
        release();

        if(!ImageVecCheckSimilarType(stack)) {
            return;
        }

        this->type_linearization = IL_LUT_8_BIT;

        const int channels   = stack[0]->channels;
        const int pixelcount = stack[0]->nPixels();

        // precompute robertson weighting function
        for (size_t i=0; i<256; i++) {
            this->w[i] = weightFunction(float(i) / 255.0f, CW_ROBERTSON);
        }

        // avoid saturation
        int minM = 0;
        int maxM = 255;
        for (int m = 0; m < 256; m++) {
            if (this->w[m] > 0) {
                minM = m;
                break;
            }
        }

        for (int m=255; m>=0; m--) {
            if (this->w[m] > 0) {
                maxM = m;
                break;
            }
        }

        // avoid ghosting (for each exposure get the index for the immediately higher and lower exposure)
        int *lower = new int [stack.size()];
        int *higher = new int[stack.size()];

        for (size_t i=0; i<stack.size(); i++) {
            lower[i]  = -1;
            higher[i] = -1;
            float t = stack[i]->exposure;
            float tHigh = stack[0]->exposure;
            float tLow  = tHigh;

            for (size_t j=0; j<stack.size(); j++) {
                if (i != j) {
                    float tj = stack[j]->exposure;

                    if (tj > t && tj < tHigh) {
                        tHigh = tj;
                        higher[i] = int(j);
                    }
                    if (tj < t && tj > tLow) {
                        tLow = tj;
                        lower[i] = int(j);
                    }
                }
            }

            if (lower[i]  == -1) {
                lower[i]  = int(i);
            }

            if (higher[i] == -1) {
                higher[i] = int(i);
            }
        }

        // create initial inv response function
        {
            float * lin = new float[256];
            for (int i=0; i<256; i++) {
                lin[i] = float(2.0 * i / 255.0);
            }
            this->icrf.push_back(lin);

            for (int i=1; i<channels; i++) {
                float * col = new float[256];
                Buffer<float>::assign(col, lin, 256);
                this->icrf.push_back(col);
            }
        }

        // create quantized stack
        std::vector<unsigned char *> qstack;
        for (Image * slice : stack) {
            assert(slice->frames == 1);
            unsigned char * q = convertHDR2LDR(slice->data, NULL, slice->size(), LT_NOR);
            qstack.push_back(q);
        }

        // iterative gauss-seidel
        for (int ch=0; ch<channels; ch++) {
            float * fun = this->icrf[ch];
            float funPrev[256];
            Buffer<float>::assign(funPrev, fun, 256);

            std::vector<float> x(pixelcount);

            float prevDelta = 0.0f;
            for (size_t iter=0; iter<maxIterations; iter++) {
                // Normalize inv crf to midpoint
                {
                    // find min max
                    size_t minIdx, maxIdx;
                    for (minIdx = 0   ; minIdx < 255 && fun[minIdx]==0 ; minIdx++);
                    for (maxIdx = 255 ; maxIdx > 0   && fun[maxIdx]==0 ; maxIdx--);

                    size_t midIdx = minIdx+(maxIdx-minIdx)/2;
                    float  mid = fun[midIdx];

                    if (mid == 0.0f) {
                        // find first non-zero middle response
                        while (midIdx < maxIdx && fun[midIdx] == 0.0f) {
                            midIdx++;
                        }
                        mid = fun[midIdx];
                    }

                    if (mid != 0.0f) {
                        Buffer<float>::div(fun, 256, mid);
                    }
                }

                // Update x
                for (int i=0; i<pixelcount; i++) {
                    float sum     = 0.0f;
                    float divisor = 0.0f;

                    float maxt = -1.0f;
                    float mint = FLT_MAX;

                    int ind = i * channels + ch;

                    for (size_t s=0; s<qstack.size(); s++) {
                        unsigned char * qslice = qstack[s];
                        const float     t      = stack[s]->exposure;

                        int m = qslice[ind];

                        // compute max/min time for under/over exposed pixels
                        if (m > maxM) {
                            mint = std::min(mint, t);
                        }

                        if (m < minM) {
                            maxt = std::max(maxt, t);
                        }

                        // to avoid ghosting
                        int mLow  = qstack[lower [s]][ind];
                        int mHigh = qstack[higher[s]][ind];
                        if (mLow > m || mHigh < m) {
                            continue;
                        }

                        const float wm = this->w[m];

                        sum     += wm * t * fun[m];
                        divisor += wm * t * t;
                    }

                    if (divisor == 0.0f) {
                        // avoid saturation
                        if (maxt > -1.0f) {
                            x[i] = fun[minM] / maxt;
                        }

                        if (mint < FLT_MAX) {
                            x[i] = fun[maxM] / mint;
                        }
                    } else if (divisor < 1e-4f) {
                        x[i] = -1.0f;
                    } else {
                        x[i] = sum / divisor;
                    }
                }

                // Update inv crf
                {
                    size_t cardEm[256] = { 0 };
                    float  sum[256]    = { 0.0f };
                    float minSatTime = FLT_MAX;
                    for (size_t s=0; s<qstack.size(); s++) {
                        unsigned char * qslice = qstack[s];
                        const float     t      = stack[s]->exposure;

                        for (int i=0; i<pixelcount; i++) {
                            if (x[i] < 0.0f) {
                                continue;
                            }

                            const int m = int(qslice[i*channels+ch]);

                            if (m == 255) {
                                if (t < minSatTime) {
                                    minSatTime = t;
                                    sum[m] = t * x[i];
                                    cardEm[m] = 1;
                                }
                                else if (t == minSatTime)
                                {
                                    sum[m] = std::min(sum[m], t * x[i]);
                                }
                            } else {
                                sum[m] += t * x[i];
                                cardEm[m]++;
                            }
                        }
                    }

                    // compute average and fill undefined values with previous one
                    float prev = 0.0f;
                    for (int m=0; m<256; m++) {
                        if (cardEm[m] != 0) {
                            fun[m] = prev = sum[m] / cardEm[m];
                        } else {
                            fun[m] = prev;
                        }
                    }
                }

                // check residuals
                {
                    static const float MaxDelta = 1e-7f;

                    float delta = 0.0f;
                    int count   = 0;
                    for (int m=0; m<256; m++) {
                        if( fun[m] != 0.0f ) {
                            float diff = fun[m] - funPrev[m];
                            delta += diff * diff;
                            funPrev[m] = fun[m];
                            count++;
                        }
                    }
                    delta /= count;

                    if (delta < MaxDelta) {
                        break;
                    }

                    prevDelta = delta;
                }
            }
        }
        // estimation complete!

        // normalize response function keeping relative scale between colors
        float maxV = -1.0f;
        for (int ch=0; ch<channels; ch++) {
            int ind;
            maxV = std::max(Arrayf::getMax(this->icrf[ch], 256, ind), maxV);
        }

        for (int ch=0; ch<channels; ch++) {
            Buffer<float>::div(this->icrf[ch], 256, maxV);
            this->icrf[ch][255] = 1.0f;
        }

        // clean quantized stack
        for (unsigned char * qslice : qstack) {
            delete qslice;
        }

        delete[] lower;
        delete[] higher;
    }
};

} // end namespace pic

#endif /* PIC_ALGORITHMS_CAMERA_RESPONSE_FUNCTION_HPP */


#include <boost/multiprecision/mpfr.hpp>


#pragma once

#include "calotypes/DataSelector.hpp"
#include "calotypes/SubsetSamplers.hpp"

#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

namespace calotypes
{

template <class Data>
class RandomDataSelector
: public DataSelector<Data>
{
public:

	typedef typename DataSelector<Data>::Dataset Dataset;

	RandomDataSelector()
	{
		boost::random::random_device rng;
		generator.seed( rng );
	}

	virtual void SelectData( const Dataset& data, unsigned int subsetSize, Dataset& subset )
	{
		std::vector< unsigned int > subsetInds;
		BitmapSampling( data.size(), subsetSize, subsetInds, generator );
		subset.resize( subsetSize );
		for( unsigned int i = 0; i < subsetSize; i++ )
		{
			subset[i] = data[ subsetInds[i] ];
		}
	}

private:

	boost::random::mt19937 generator; // Seeded by a true random_device
    boost::random::uniform_int_distribution<> index_dist;

};

} // end namespace calotypes


/*
	@author herumi

	tiny calculator
	This program generates a function to calc the value of
	polynomial given by user in run-time.
	use boost::spirit::classic
	see calc2.cpp for new version of boost::spirit
*/
#include <stdio.h>
#include <sstream>
#include <map>
#define XBYAK_NO_OP_NAMES
#include "xbyak/xbyak.h"
#ifdef _MSC_VER
	#pragma warning(disable : 4127) // for boost(constant condition)
	#pragma warning(disable : 4512) // for boost
#endif
#include <boost/spirit/include/classic_file_iterator.hpp>
#include <boost/spirit/include/classic_core.hpp>
#include <boost/bind.hpp>

enum Error {
	UNDEFINED_VARIABLE = 1
};

/*
	JIT assemble of given polynomial for VC or gcc
*/
class FuncGen : public Xbyak::CodeGenerator {
public:
	typedef std::map<std::string, int> Map;
private:
	enum {
		MAX_CONST_NUM = 32
	};
	double constTbl_[MAX_CONST_NUM];
	size_t constTblPos_;
	int regIdx_;
	Map varMap_; // map var name to index
#ifdef XBYAK32
	const Xbyak::Reg32& valTbl_;
	const Xbyak::Reg32& tbl_;
#else
	const Xbyak::Reg64& valTbl_;
	const Xbyak::Reg64& tbl_;
#endif
public:
	/*
		@param y [out] the value of f(var)
		@param var [in] table of input variables
		func(double *y, const double var[]);
		@note func does not return double to avoid difference of compiler
	*/
	FuncGen(const std::vector<std::string>& varTbl)
		: constTblPos_(0)
		, regIdx_(-1)
#ifdef XBYAK32
		, valTbl_(eax)
		, tbl_(edx)
#elif defined(XBYAK64_WIN)
		, valTbl_(rcx)
		, tbl_(rdx)
#else
		, valTbl_(rdi)
		, tbl_(rsi)
#endif
	{
#ifdef XBYAK32
		mov(valTbl_, ptr[esp+8]); // eax == varTbl
		mov(tbl_, (size_t)constTbl_);
#else
#ifdef XBYAK64_WIN
		movaps(ptr [rsp + 8], xm6); // save xm6, xm7
		movaps(ptr [rsp + 8 + 16], xm7);
#endif
		mov(tbl_, (size_t)constTbl_);
#endif
		for (int i = 0, n = static_cast<int>(varTbl.size()); i < n; i++) {
			varMap_[varTbl[i]] = i;
		}
	}
	// use edx
	void genPush(double n)
	{
		if (constTblPos_ >= MAX_CONST_NUM) throw;
		constTbl_[constTblPos_] = n;
		if (regIdx_ == 7) throw;
		movsd(Xbyak::Xmm(++regIdx_), ptr[tbl_ + (int)(constTblPos_ * sizeof(double))]);
		constTblPos_++;
	}
	// use eax
	void genVal(const char *begin, const char *end)
	{
		std::string var(begin, end);
		if (varMap_.find(var) == varMap_.end()) throw UNDEFINED_VARIABLE;
		if (regIdx_ == 7) throw;
		movsd(Xbyak::Xmm(++regIdx_), ptr[valTbl_ + varMap_[var] * sizeof(double)]);
	}
	void genAdd(const char*, const char*)
	{
		addsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genSub(const char*, const char*)
	{
		subsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genMul(const char*, const char*)
	{
		mulsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void genDiv(const char*, const char*)
	{
		divsd(Xbyak::Xmm(regIdx_ - 1), Xbyak::Xmm(regIdx_)); regIdx_--;
	}
	void complete()
	{
#ifdef XBYAK32
		mov(eax, ptr [esp + 4]); // eax = valTbl
		movsd(ptr [eax], xm0);
#else
#ifdef XBYAK64_WIN
		movaps(xm6, ptr [rsp + 8]);
		movaps(xm7, ptr [rsp + 8 + 16]);
#endif
#endif
		ret();
	}
};

struct Grammar : public boost::spirit::classic::grammar<Grammar> {
	FuncGen& f_;
	Grammar(FuncGen& f) : f_(f) { }
	template<typename ScannerT>
	struct definition {
		boost::spirit::classic::rule<ScannerT> poly0, poly1, poly2, var;

		definition(const Grammar& self)
		{
			using namespace boost;
			using namespace boost::spirit::classic;

			poly0 = poly1 >> *(('+' >> poly1)[bind(&FuncGen::genAdd, ref(self.f_), _1, _2)]
			                 | ('-' >> poly1)[bind(&FuncGen::genSub, ref(self.f_), _1, _2)]);
			poly1 = poly2 >> *(('*' >> poly2)[bind(&FuncGen::genMul, ref(self.f_), _1, _2)]
			                 | ('/' >> poly2)[bind(&FuncGen::genDiv, ref(self.f_), _1, _2)]);
			var = (+alpha_p)[bind(&FuncGen::genVal, ref(self.f_), _1, _2)];
			poly2 = real_p[bind(&FuncGen::genPush, ref(self.f_), _1)]
				| var
				| '(' >> poly0 >> ')';
		}
		const boost::spirit::classic::rule<ScannerT>& start() const { return poly0; }
	};
};

void put(const std::vector<double>& x)
{
	for (size_t i = 0, n = x.size(); i < n; i++) {
		if (i > 0) printf(", ");
		printf("%f", x[i]);
	}
}

int main(int argc, char *argv[])
{
	if (argc <= 2) {
		fprintf(stderr, "calc \"var1 var2 ...\" \"function of var\"\n");
		fprintf(stderr, "eg. calc x \"x*x\"\n");
		fprintf(stderr, "eg. calc \"x y z\"  \"x*x + y - z\"\n");
		return 1;
	}
	const char *poly = argv[2];
	try {
		std::vector<std::string> varTbl;

		// get varTbl from argv[1]
		{
			std::istringstream is(argv[1]);
			int i = 0;
			printf("varTbl = { ");
			while (is) {
				std::string var;
				is >> var;
				if (var.empty()) break;
				printf("%s:%d, ", var.c_str(), i);
				varTbl.push_back(var);
				i++;
			}
			printf("}\n");
		}
		FuncGen funcGen(varTbl);
		Grammar calc(funcGen);
		boost::spirit::classic::parse_info<> r = parse(poly, calc, boost::spirit::classic::space_p);
		if (!r.full) {
			printf("err poly=%s\n", poly);
			return 1;
		}
		funcGen.complete();
		std::vector<double> valTbl;
		valTbl.resize(varTbl.size());
#ifdef XBYAK32
		puts("32bit mode");
		void (*func)(double *ret, const double *valTbl) = funcGen.getCode<void (*)(double *, const double*)>();
#else
		puts("64bit mode");
		double (*func)(const double *valTbl) = funcGen.getCode<double (*)(const double*)>();
#endif
		for (int i = 0; i < 10; i++) {
			for (size_t j = 0, n = valTbl.size(); j < n; j++) {
				valTbl[j] = rand() % 7;
			}
			double y;
#ifdef XBYAK32
			func(&y, &valTbl[0]);
#else
			y = func(&valTbl[0]);
#endif
			printf("f("); put(valTbl); printf(")=%f\n", y);
		}
	} catch (std::exception& e) {
		printf("ERR:%s\n", e.what());
	} catch (Error err) {
		printf("ERR:%d\n", err);
	} catch (...) {
		printf("unknown error\n");
	}

	return 0;
}


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

#ifndef CRYPTO3_ALGEBRA_FFT_KRONECKER_SUBSTITUTION_HPP
#define CRYPTO3_ALGEBRA_FFT_KRONECKER_SUBSTITUTION_HPP

#include <vector>
#include <algorithm>
#include <cmath>

#include <boost/math/tools/polynomial.hpp>

namespace nil {
    namespace crypto3 {
        namespace fft {
            /*!
             * @brief Given two polynomial vectors, A and B, the function performs
             * polynomial multiplication and returns the resulting polynomial vector.
             * The implementation makes use of
             * [Harvey 07, Multipoint Kronecker Substitution, Section 2.1] and
             * [Gathen and Gerhard, Modern Computer Algebra 3rd Ed., Section 8.4].
             */
            template<typename FieldType>
            void kronecker_substitution(std::vector<typename FieldType::value_type>& v3,
                                        const std::vector<typename FieldType::value_type>& v1,
                                        const std::vector<typename FieldType::value_type>& v2) {
                typedef typename FieldType::value_type value_type;

                // Initialize
                bool square = (v1 == v2) ? 1 : 0;

                // Polynomial length
                std::size_t n1 = v1.size();
                std::size_t n2 = v2.size();
                std::size_t n3 = n1 + n2 - 1;

                // Determine number of bits needed
                value_type v1_max = *max_element(std::begin(v1), std::end(v1));
                value_type v2_max = *max_element(std::begin(v2), std::end(v2));
                /*size_t b = 2 * (v1_max * v2_max).as_bigint().num_bits() + 1;

                // Number of limbs needed in total
                std::size_t k1 = (n1 * b + (GMP_NUMB_BITS)-1) / GMP_NUMB_BITS;
                std::size_t k2 = (n2 * b + (GMP_NUMB_BITS)-1) / GMP_NUMB_BITS;

                // Output polynomial
                v3.resize(n3, value_type::zero());

                // Allocate all MP_LIMB_T space once and store the reference pointer M1
                // to free memory afterwards. P1, P2, and P3 will remain fixed pointers
                // to the start of their respective polynomials as reference.
                mp_limb_t* m1 = (mp_limb_t*)malloc(sizeof(mp_limb_t) * 2 * (k1 + k2));
                mp_limb_t* p1 = m1;
                mp_limb_t* p2 = p1 + k1;
                mp_limb_t* p3 = p2 + k2;

                // Helper variables
                mp_limb_t* ref;
                mp_limb_t limb;
                unsigned long val;
                unsigned long mask;
                unsigned long limb_b;
                unsigned long delta;
                unsigned long delta_b;

                // Construct P1 limb
                ref = p1;
                limb = 0;
                limb_b = 0;
                for (std::size_t i = 0; i < n1; i++) {
                    val = v1[i].as_ulong();
                    limb += (val << limb_b);

                     // If the next iteration of LIMB_B is >= to the GMP_LIMB_BITS, then
                     // write it out to MP_LIMB_T* and reset LIMB. If VAL has remaining
                     // bits due to GMP_LIMB_BITS boundary, set it in LIMB and proceed.
                    if (limb_b + b >= GMP_LIMB_BITS) {
                        *ref++ = limb;
                        limb = limb_b ? (val >> (GMP_LIMB_BITS - limb_b)) : 0;
                        limb_b -= GMP_LIMB_BITS;
                    }
                    limb_b += b;
                }
                if (limb_b)
                    *ref++ = limb;

                // Construct P2 limb. If V2 == V1, then P2 = P1 - square case.
                if (square)
                    p2 = p1;
                else {
                    ref = p2;
                    limb = 0;
                    limb_b = 0;
                    for (std::size_t i = 0; i < n2; i++) {
                        val = v2[i].as_ulong();
                        limb += (val << limb_b);

                         // If the next iteration of LIMB_B is >= to the GMP_LIMB_BITS, then
                         // write it out to MP_LIMB_T* and reset LIMB. If VAL has remaining
                         // bits due to GMP_LIMB_BITS boundary, set it in LIMB and proceed.
                        if (limb_b + b >= GMP_LIMB_BITS) {
                            *ref++ = limb;
                            limb = limb_b ? (val >> (GMP_LIMB_BITS - limb_b)) : 0;
                            limb_b -= GMP_LIMB_BITS;
                        }
                        limb_b += b;
                    }
                    if (limb_b)
                        *ref++ = limb;
                }

                // Multiply P1 and P2 limbs and store result in P3 limb.
                mpn_mul(p3, p1, k1, p2, k2);

                // Perfect alignment case: bits B is equivalent to GMP_LIMB_BITS
                if (b == GMP_LIMB_BITS)
                    for (std::size_t i = 0; i < n3; i++)
                        v3[i] = value_type(*p3++);
                // Non-alignment case
                else {
                    // Mask of 2^b - 1
                    mask = (1UL << b) - 1;

                    limb = 0;
                    limb_b = 0;
                    for (std::size_t i = 0; i < n3; i++) {

                         // If the coefficient's bit length is contained in LIMB, then
                         // write the masked value out to vector V3 and decrement LIMB
                         // by B bits.
                        if (b <= limb_b) {
                            v3[i] = value_type(limb & mask);

                            delta = b;
                            delta_b = limb_b - delta;
                        }

                         // If the remaining coefficient is across two LIMBs, then write
                         // to vector V3 the current limb's value and add upper bits from
                         // the second part. Lastly, decrement LIMB by the coefficient's
                         // upper portion bit length.
                        else {
                            v3[i] = value_type(limb);
                            v3[i] += value_type(((limb = *p3++) << limb_b) & mask);

                            delta = b - limb_b;
                            delta_b = GMP_LIMB_BITS - delta;
                        }

                        limb >>= delta;
                        limb_b = delta_b;
                    }
                }

                // Free memory
                free(m1);

                _condense(v3);*/
            }

            /**
             * Perform the multiplication of two polynomials, polynomial A * polynomial B, using Kronecker Substitution,
             * and stores result in polynomial C.
             */
            template<typename FieldType>
            void _polynomial_multiplication_on_kronecker(std::vector<typename FieldType::value_type>& c,
                                                         const std::vector<typename FieldType::value_type>& a,
                                                         const std::vector<typename FieldType::value_type>& b) {
                kronecker_substitution<FieldType>(c, a, b);
            }

        }    // namespace fft
    }        // namespace crypto3
}    // namespace nil

#endif    // ALGEBRA_FFT_KRONECKER_SUBSTITUTION_HPP


#include <sstream>
#include <string>
#include <vector>
#include <iostream>
#include <time.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/filesystem.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <yaml-cpp/yaml.h>
#include <glog/logging.h>

#include "YamlFileIO.h"
#include "FileSystemTools.h"

#define BACKPACK_1 1
#define BACKPACK_2 2
#define BACKPACK_3 3
#define BACKPACK_4 4
#define BACKPACK_5 5

namespace YAML {
template <>
struct convert<Eigen::Matrix3d> {
  static Node encode(const Eigen::Matrix3d& rhs) {
    Node node;

    for (int i = 0; i < rhs.rows(); ++i) {
      for (int j = 0; j < rhs.cols(); ++j) {
        node.push_back(rhs(i, j));
      }
    }
    return node;
  }

  static bool decode(const Node& node, Eigen::Matrix3d& rhs) {
    if (!node.IsSequence() || node.size() != 9) {
      return false;
    }

    rhs(0, 0) = node[0].as<double>();
    rhs(0, 1) = node[1].as<double>();
    rhs(0, 2) = node[2].as<double>();
    rhs(1, 0) = node[3].as<double>();
    rhs(1, 1) = node[4].as<double>();
    rhs(1, 2) = node[5].as<double>();
    rhs(2, 0) = node[6].as<double>();
    rhs(2, 1) = node[7].as<double>();
    rhs(2, 2) = node[8].as<double>();
    return true;
  }
};

template <>
struct convert<Eigen::Vector3d> {
  static Node encode(const Eigen::Vector3d& rhs) {
    Node node;

    for (int i = 0; i < rhs.rows(); ++i) {
      for (int j = 0; j < rhs.cols(); ++j) {
        node.push_back(rhs(i, j));
      }
    }
    return node;
  }

  static bool decode(const Node& node, Eigen::Vector3d& rhs) {
    if (!node.IsSequence() || node.size() != 4) {
      return false;
    }

    rhs(0, 0) = node[0].as<double>();
    rhs(1, 0) = node[1].as<double>();
    rhs(2, 0) = node[2].as<double>();
    return true;
  }
};

template <>
struct convert<Eigen::Vector4d> {
  static Node encode(const Eigen::Vector4d& rhs) {
    Node node;

    for (int i = 0; i < rhs.rows(); ++i) {
      for (int j = 0; j < rhs.cols(); ++j) {
        node.push_back(rhs(i, j));
      }
    }
    return node;
  }

  static bool decode(const Node& node, Eigen::Vector4d& rhs) {
    if (!node.IsSequence() || node.size() != 4) {
      return false;
    }

    rhs(0, 0) = node[0].as<double>();
    rhs(1, 0) = node[1].as<double>();
    rhs(2, 0) = node[2].as<double>();
    rhs(3, 0) = node[3].as<double>();
    return true;
  }
};

template <>
struct convert<Eigen::Matrix4d> {
  static Node encode(const Eigen::Matrix4d& rhs) {
    Node node;

    for (int i = 0; i < rhs.rows(); ++i) {
      for (int j = 0; j < rhs.cols(); ++j) {
        node.push_back(rhs(i, j));
      }
    }
    return node;
  }

  static bool decode(const Node& node, Eigen::Matrix4d& rhs) {
    if (!node.IsSequence() || node.size() != 16) {
      return false;
    }

    rhs(0, 0) = node[0].as<double>();
    rhs(0, 1) = node[1].as<double>();
    rhs(0, 2) = node[2].as<double>();
    rhs(0, 3) = node[3].as<double>();
    rhs(1, 0) = node[4].as<double>();
    rhs(1, 1) = node[5].as<double>();
    rhs(1, 2) = node[6].as<double>();
    rhs(1, 3) = node[7].as<double>();
    rhs(2, 0) = node[8].as<double>();
    rhs(2, 1) = node[9].as<double>();
    rhs(2, 2) = node[10].as<double>();
    rhs(2, 3) = node[11].as<double>();
    rhs(3, 0) = node[12].as<double>();
    rhs(3, 1) = node[13].as<double>();
    rhs(3, 2) = node[14].as<double>();
    rhs(3, 3) = node[15].as<double>();
    return true;
  }
};
}  // namespace YAML

/**
 * @brief Stereo camera parameters
 *
 */
struct StereoParam {
  // extrinsic matrix, transform right camera frame into left camera frame
  Eigen::Matrix4d T_rl;
  // left camera intrinsic
  Eigen::Matrix3d cam0_intrinsic;
  // left camera distortion_coeffs
  double cam0_distortions[4];

  // right camera intrinsic
  Eigen::Matrix3d cam1_intrinsic;
  // right camera distortion_coeffs
  double cam1_distortions[4];

  int img_width;
  int img_height;
  // camera model
  std::string camera_type;
  // assume cam0 and cam1 use identical distortion type
  std::string distortion_type;

  StereoParam()
      : T_rl(Eigen::Matrix4d::Identity()),
        cam0_intrinsic(Eigen::Matrix3d::Identity()),
        cam1_intrinsic(Eigen::Matrix3d::Identity()),
        img_width(2592),
        img_height(2048) {
    memset(cam0_distortions, 0, 4 * sizeof(double));
    memset(cam1_distortions, 0, 4 * sizeof(double));
    camera_type = "pinhole";
    distortion_type = "equidistant";
  }

  void getExtrinsic(cv::Mat& R_rl, cv::Mat& t_rl) {
    Eigen::Matrix3d R = T_rl.block<3, 3>(0, 0);
    Eigen::Vector3d t = T_rl.block<3, 1>(0, 3);

    cv::eigen2cv(R, R_rl);
    cv::eigen2cv(t, t_rl);
  }

  void getCam0Param(cv::Mat& K, cv::Mat& D) {
    cv::eigen2cv(cam0_intrinsic, K);

    cv::Mat distortion =
        (cv::Mat_<double>(1, 4) << cam0_distortions[0], cam0_distortions[1],
         cam0_distortions[2], cam0_distortions[3]);
    D = distortion.clone();
  }

  void getCam1Param(cv::Mat& K, cv::Mat& D) {
    cv::eigen2cv(cam1_intrinsic, K);

    cv::Mat distortion =
        (cv::Mat_<double>(1, 4) << cam1_distortions[0], cam1_distortions[1],
         cam1_distortions[2], cam1_distortions[3]);
    D = distortion.clone();
  }
};

/**
 * @brief Stereo camera parameters
 *
 */
struct MonoCamParam {
  // extrinsic: from reference camera to this camera
  Eigen::Matrix4d T_ref_cam_;
  // camera intrinsic
  Eigen::Matrix3d intrinsic_;
  // camera distortion_coeffs
  Eigen::Vector4d distortions_;

  int img_width_;
  int img_height_;
  // camera model
  std::string camera_type;
  // lens distortion type
  std::string distortion_type;

  MonoCamParam()
      : T_ref_cam_(Eigen::Matrix4d::Identity()),
        intrinsic_(Eigen::Matrix3d::Identity()),
        distortions_(Eigen::Vector4d::Zero()),
        img_width_(2592),
        img_height_(2048) {
    camera_type = "pinhole";
    distortion_type = "equidistant";
  }

  void getExtrinsic(cv::Mat& R_ref_cam, cv::Mat& t_ref_cam) {
    Eigen::Matrix3d R = T_ref_cam_.block<3, 3>(0, 0);
    Eigen::Vector3d t = T_ref_cam_.block<3, 1>(0, 3);

    cv::eigen2cv(R, R_ref_cam);
    cv::eigen2cv(t, t_ref_cam);
  }

  void getCamParam(cv::Mat& K, cv::Mat& D) {
    cv::eigen2cv(intrinsic_, K);

    cv::Mat distortion;
    cv::eigen2cv(distortions_, distortion);
    D = distortion.clone();
  }

};

/**
 * @brief Mono-IMU parameters
 *
 */
struct MonoIMUParam {
  // cam0 2 imu
  Eigen::Matrix4d T_cam0_imu;

  // acc bias(m/s^2)
  Eigen::Vector3d acc_bias;
  // gyro (rad/s)
  Eigen::Vector3d gyro_bias;

  // noise rms
  double avg_acc_noise_density;
  double avg_gyro_noise_density;
  // random walk noise rms
  double avg_acc_bias_noise_density;
  double avg_gyro_bias_noise_density;

  MonoIMUParam()
      : T_cam0_imu(Eigen::Matrix4d::Identity()),
        acc_bias(Eigen::Vector3d::Zero()),
        gyro_bias(Eigen::Vector3d::Zero()),
        avg_acc_noise_density(0.0),
        avg_gyro_noise_density(0.0),
        avg_acc_bias_noise_density(0.0),
        avg_gyro_bias_noise_density(0.0) {}

  void getExtrinsic(cv::Mat& R_cam0_imu, cv::Mat& t_cam0_imu) {
    Eigen::Matrix3d R = T_cam0_imu.block<3, 3>(0, 0);
    Eigen::Vector3d t = T_cam0_imu.block<3, 1>(0, 3);

    cv::eigen2cv(R, R_cam0_imu);
    cv::eigen2cv(t, t_cam0_imu);
  }

  void getAccBias(cv::Mat& acc_bias_mat) {
    cv::eigen2cv(acc_bias, acc_bias_mat);
  }

  void getGyroBias(cv::Mat& gyro_bias_mat) {
    cv::eigen2cv(gyro_bias, gyro_bias_mat);
  }

  void getNoiseDensity(double& acc_nd, double& gyro_nd) {
    acc_nd = avg_acc_noise_density;
    gyro_nd = avg_gyro_noise_density;
  }

  void getBiasNoiseDensity(double& acc_bias_nd, double& gyro_bias_nd) {
    acc_bias_nd = avg_acc_bias_noise_density;
    gyro_bias_nd = avg_gyro_bias_noise_density;
  }
};

/**
 * @brief Stereo-IMU parameters
 *
 */
struct LidarParam {
  //
  double max_dist;
  //
  double min_dist;
  //
  double starting_offset;
  //
  int num_rays;
  double frequency;
  // transform w.r.t lidar0
  Eigen::Matrix4d T;

  LidarParam()
      : max_dist(30.0),
        min_dist(0.1),
        starting_offset(-6.0),
        num_rays(16),
        frequency(10),
        T(Eigen::Matrix4d::Identity()) {}
};

/**
 * @brief Teche parameters
 * Contain 4 camera intrinsics and extrinsics
 */
struct TecheParam {
  // intrinsic parameters
  Eigen::Matrix3d cam0_intrinsic;
  Eigen::Matrix3d cam1_intrinsic;
  Eigen::Matrix3d cam2_intrinsic;
  Eigen::Matrix3d cam3_intrinsic;
  // distortion parameters
  Eigen::Vector4d cam0_distortion;
  Eigen::Vector4d cam1_distortion;
  Eigen::Vector4d cam2_distortion;
  Eigen::Vector4d cam3_distortion;
  // image size
  int img_width;
  int img_height;
  // camera model
  std::string camera_type;
  // distortion type
  std::string distortion_type;
  // extrinsic: rotation axis 2 camera
  Eigen::Matrix4d T_r_c0;
  Eigen::Matrix4d T_r_c1;
  Eigen::Matrix4d T_r_c2;
  Eigen::Matrix4d T_r_c3;

  // extrinsic: lidar0 2 rotation axis
  Eigen::Matrix4d T_lidar0_r;
};

/**
 * @brief parse kalibr yaml file support: pinhole + equidistant
 *
 * @param kalib_intrin_file
 * @param camera Camera Model param
 */
bool readKalibrStereoOutputFile(const std::string& kalib_result_file,
                                StereoParam& stereo_param) {
  if (kalib_result_file.empty()) {
    std::cout << "[readKalibrOutputFile] Empty input file name!\n";
    return false;
  }

  std::ifstream fs(kalib_result_file);
  if (!fs.is_open()) {
    std::cout << "[readKalibrOutputFile] Error read " << kalib_result_file
              << "\n";
    return false;
  }

  YAML::Node root_node;
  YAML::Node cam0_node;
  YAML::Node cam1_node;

  try {
    root_node = YAML::LoadFile(kalib_result_file);
  } catch (YAML::BadFile& e) {
    std::cout << "[readKalibrOutputFile] Could not open file: "
              << kalib_result_file << "\n";
    return false;
  } catch (YAML::ParserException& e) {
    std::cout << "[readKalibrOutputFile] Invalid file format: "
              << kalib_result_file << "\n";
    return false;
  }
  if (root_node.IsNull()) {
    std::cout << "[loadKalibrResult] Could not open file: " << kalib_result_file
              << "\n";
    return false;
  }

  std::cout << "[readKalibrOutputFile] read " << kalib_result_file << ":\n";
  int img_width = 0, img_height = 0;

  cam0_node = root_node["cam0"];
  double cam0_distortion[4] = {0.0};
  cam0_distortion[0] = cam0_node["distortion_coeffs"][0].as<double>();
  cam0_distortion[1] = cam0_node["distortion_coeffs"][1].as<double>();
  cam0_distortion[2] = cam0_node["distortion_coeffs"][2].as<double>();
  cam0_distortion[3] = cam0_node["distortion_coeffs"][3].as<double>();

  Eigen::Matrix3d cam0_intrinsic = Eigen::Matrix3d::Identity();
  cam0_intrinsic(0, 0) = cam0_node["intrinsics"][0].as<double>();
  cam0_intrinsic(1, 1) = cam0_node["intrinsics"][1].as<double>();
  cam0_intrinsic(0, 2) = cam0_node["intrinsics"][2].as<double>();
  cam0_intrinsic(1, 2) = cam0_node["intrinsics"][3].as<double>();

  cam1_node = root_node["cam1"];
  double cam1_distortion[4] = {0.0};
  cam1_distortion[0] = cam1_node["distortion_coeffs"][0].as<double>();
  cam1_distortion[1] = cam1_node["distortion_coeffs"][1].as<double>();
  cam1_distortion[2] = cam1_node["distortion_coeffs"][2].as<double>();
  cam1_distortion[3] = cam1_node["distortion_coeffs"][3].as<double>();

  Eigen::Matrix3d cam1_intrinsic = Eigen::Matrix3d::Identity();
  cam1_intrinsic(0, 0) = cam1_node["intrinsics"][0].as<double>();
  cam1_intrinsic(1, 1) = cam1_node["intrinsics"][1].as<double>();
  cam1_intrinsic(0, 2) = cam1_node["intrinsics"][2].as<double>();
  cam1_intrinsic(1, 2) = cam1_node["intrinsics"][3].as<double>();

  img_width = cam1_node["resolution"][0].as<int>();
  img_height = cam1_node["resolution"][1].as<int>();

  std::string distortion_type = cam0_node["distortion_model"].as<std::string>();
  std::string camera_type = cam0_node["camera_model"].as<std::string>();

  Eigen::Matrix4d extrinsic = Eigen::Matrix4d::Identity();
  for (size_t i = 0; i < cam1_node["T_cn_cnm1"].size(); ++i) {
    extrinsic(i, 0) = cam1_node["T_cn_cnm1"][i][0].as<double>();
    extrinsic(i, 1) = cam1_node["T_cn_cnm1"][i][1].as<double>();
    extrinsic(i, 2) = cam1_node["T_cn_cnm1"][i][2].as<double>();
    extrinsic(i, 3) = cam1_node["T_cn_cnm1"][i][3].as<double>();
  }

  stereo_param.T_rl = extrinsic;
  std::cout << "[readKalibrOutputFile] T_rl : \n" << stereo_param.T_rl << "\n";
  Eigen::Vector3d baseline = stereo_param.T_rl.block<3, 1>(0, 3);
  std::cout << "[readKalibrOutputFile] baseline norm : " << baseline.norm()
            << "\n";

  std::cout << "[loadKalibrResult] baseline * fx = "
            << (baseline.norm() * cam0_intrinsic(0, 0)) << "\n";

  stereo_param.cam0_intrinsic = cam0_intrinsic;
  std::cout << "[loadKalibrResult] cam0 intrinsic : \n"
            << stereo_param.cam0_intrinsic << "\n";
  memcpy(stereo_param.cam0_distortions, cam0_distortion, 4 * sizeof(double));
  std::cout << "[loadKalibrResult] cam0 distrotion : "
            << stereo_param.cam0_distortions[0] << ", "
            << stereo_param.cam0_distortions[3] << "\n";
  stereo_param.cam1_intrinsic = cam1_intrinsic;
  std::cout << "[loadKalibrResult] cam1 intrinsic : \n"
            << stereo_param.cam1_intrinsic << "\n";
  memcpy(stereo_param.cam1_distortions, cam1_distortion, 4 * sizeof(double));
  std::cout << "[loadKalibrResult] cam1 distrotion : "
            << stereo_param.cam1_distortions[0] << ", "
            << stereo_param.cam1_distortions[3] << "\n";
  stereo_param.img_width = img_width;
  stereo_param.img_height = img_height;
  stereo_param.distortion_type = distortion_type;
  stereo_param.camera_type = camera_type;

  return true;
}

bool loadMonoIMUExtFileKalibr(const std::string& kalib_result_file,
                                   MonoIMUParam& monocam_imu_param) {
  if (!common::fileExists(kalib_result_file)) {
    LOG(FATAL) << " File doesnt exist: " << kalib_result_file;
    return false;
  }

  std::ifstream fs(kalib_result_file);
  if (!fs.is_open()) {
    LOG(INFO) << " Error read " << kalib_result_file << "\n";
    return false;
  }

  YAML::Node root_node;
  try {
    root_node = YAML::LoadFile(kalib_result_file);
  } catch (YAML::BadFile& e) {
    LOG(INFO) << " Could not open file: " << kalib_result_file << "\n";
    return false;
  } catch (YAML::ParserException& e) {
    LOG(INFO) << " Invalid file format: " << kalib_result_file << "\n";
    return false;
  }
  if (root_node.IsNull()) {
    LOG(INFO) << " Could not open file: " << kalib_result_file << "\n";
    return false;
  }

  YAML::Node cam0_node = root_node["cam0"];
  Eigen::Matrix4d extrinsic = Eigen::Matrix4d::Identity();
  for (size_t i = 0; i < cam0_node["T_cam_imu"].size(); ++i) {
    extrinsic(i, 0) = cam0_node["T_cam_imu"][i][0].as<double>();
    extrinsic(i, 1) = cam0_node["T_cam_imu"][i][1].as<double>();
    extrinsic(i, 2) = cam0_node["T_cam_imu"][i][2].as<double>();
    extrinsic(i, 3) = cam0_node["T_cam_imu"][i][3].as<double>();
  }

  monocam_imu_param.T_cam0_imu = extrinsic;
  std::cout << "[loadMonoIMUExtFileKalibr] camera-IMU : \n"
            << extrinsic << "\n";
  return true;
}

bool readOpencvOutputFile(const std::string& file_name, Eigen::Matrix4d& T) {
  if (file_name.empty()) {
    std::cerr << "Empty file name!\n";
    return false;
  }

  cv::FileStorage fs(file_name, cv::FileStorage::READ);
  if (!fs.isOpened()) {
    std::cerr << "Fail to open " << file_name << "\n";
    return false;
  }

  cv::Mat rotation, trans;
  fs["extrinsic_rotation"] >> rotation;
  fs["extrinsic_translation"] >> trans;
  fs.release();

  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Zero();
  cv::cv2eigen(rotation, R);
  cv::cv2eigen(trans, t);

  T.block<3, 3>(0, 0) = R;
  T.block<3, 1>(0, 3) = t;

  return true;
}

bool readOpencvOutputFile(const std::string& cam0_intrinsic_fn,
                          const std::string& cam1_intrinsic_fn,
                          const std::string& cam2_intrinsic_fn,
                          const std::string& cam3_intrinsic_fn,
                          TecheParam& teche_param) {
  if (cam0_intrinsic_fn.empty() || cam1_intrinsic_fn.empty() ||
      cam2_intrinsic_fn.empty() || cam3_intrinsic_fn.empty()) {
    std::cerr << "[readTecheOutputFile] Empty file name!\n";
    return false;
  }

  teche_param.camera_type = "pinhole";
  {
    cv::FileStorage fs(cam0_intrinsic_fn, cv::FileStorage::READ);
    if (!fs.isOpened()) {
      std::cerr << "[readTecheOutputFile] Fail to open " << cam0_intrinsic_fn
                << "\n";
      return false;
    }

    int img_width, img_height;
    cv::Mat intrinsic, distortion;
    Eigen::Matrix3d intrinsic_matrix;
    Eigen::Vector4d distortion_vector;
    std::string distortion_type;

    img_width = (int)fs["image_width"];
    img_height = (int)fs["image_height"];

    distortion_type = (std::string)fs["camera_model"];

    fs["camera_matrix"] >> intrinsic;
    fs["distortion_coefficients"] >> distortion;
    cv::cv2eigen(intrinsic, intrinsic_matrix);
    distortion_vector[0] = distortion.at<double>(0, 0);
    distortion_vector[1] = distortion.at<double>(0, 1);
    distortion_vector[2] = distortion.at<double>(0, 2);
    distortion_vector[3] = distortion.at<double>(0, 3);
    fs.release();

    teche_param.img_width = img_width;
    teche_param.img_height = img_height;

    teche_param.cam0_intrinsic = intrinsic_matrix;
    teche_param.cam0_distortion = distortion_vector;
    teche_param.distortion_type = distortion_type;

    std::cout << "Load teche0 intrinsic parameters\n";
  }

  {
    cv::FileStorage fs(cam1_intrinsic_fn, cv::FileStorage::READ);
    if (!fs.isOpened()) {
      std::cerr << "[readTecheOutputFile] Fail to open " << cam1_intrinsic_fn
                << "\n";
      return false;
    }

    cv::Mat intrinsic, distortion;
    Eigen::Matrix3d intrinsic_matrix;
    Eigen::Vector4d distortion_vector;

    fs["camera_matrix"] >> intrinsic;
    fs["distortion_coefficients"] >> distortion;
    cv::cv2eigen(intrinsic, intrinsic_matrix);
    distortion_vector[0] = distortion.at<double>(0, 0);
    distortion_vector[1] = distortion.at<double>(0, 1);
    distortion_vector[2] = distortion.at<double>(0, 2);
    distortion_vector[3] = distortion.at<double>(0, 3);
    fs.release();

    teche_param.cam1_intrinsic = intrinsic_matrix;
    teche_param.cam1_distortion = distortion_vector;
  }

  {
    cv::FileStorage fs(cam2_intrinsic_fn, cv::FileStorage::READ);
    if (!fs.isOpened()) {
      std::cerr << "[readTecheOutputFile] Fail to open " << cam2_intrinsic_fn
                << "\n";
      return false;
    }

    cv::Mat intrinsic, distortion;
    Eigen::Matrix3d intrinsic_matrix;
    Eigen::Vector4d distortion_vector;

    fs["camera_matrix"] >> intrinsic;
    fs["distortion_coefficients"] >> distortion;
    cv::cv2eigen(intrinsic, intrinsic_matrix);
    distortion_vector[0] = distortion.at<double>(0, 0);
    distortion_vector[1] = distortion.at<double>(0, 1);
    distortion_vector[2] = distortion.at<double>(0, 2);
    distortion_vector[3] = distortion.at<double>(0, 3);
    fs.release();

    teche_param.cam2_intrinsic = intrinsic_matrix;
    teche_param.cam2_distortion = distortion_vector;
  }

  {
    cv::FileStorage fs(cam3_intrinsic_fn, cv::FileStorage::READ);
    if (!fs.isOpened()) {
      std::cerr << "[readTecheOutputFile] Fail to open " << cam3_intrinsic_fn
                << "\n";
      return false;
    }

    cv::Mat intrinsic, distortion;
    Eigen::Matrix3d intrinsic_matrix;
    Eigen::Vector4d distortion_vector;

    fs["camera_matrix"] >> intrinsic;
    fs["distortion_coefficients"] >> distortion;
    cv::cv2eigen(intrinsic, intrinsic_matrix);
    distortion_vector[0] = distortion.at<double>(0, 0);
    distortion_vector[1] = distortion.at<double>(0, 1);
    distortion_vector[2] = distortion.at<double>(0, 2);
    distortion_vector[3] = distortion.at<double>(0, 3);
    fs.release();

    teche_param.cam3_intrinsic = intrinsic_matrix;
    teche_param.cam3_distortion = distortion_vector;
  }

  return true;
}

bool writeYAMLOutputFile(const std::string& file_name, const MonoCamParam& cam0, const MonoCamParam& cam1,
                         const MonoCamParam& cam2, const MonoCamParam& cam3, const MonoIMUParam& mono_imu,
                         const Eigen::Matrix4d& T_lidar0_lidar1,
                         const Eigen::Matrix4d& T_lidar1_cam0, int device_id) {
    if (file_name.empty()) {
        std::cerr << "Empty file name!\n";
        return false;
    }

    std::ofstream yaml_out_file(file_name);
    if (!yaml_out_file.is_open()) {
        LOG(FATAL) << " Fail to open yaml file! \n";
        return false;
    }

    YAML::Node root_node;
    YAML::Node lidar0_node;
    YAML::Node lidar1_node;
    YAML::Node camera0_node;
    YAML::Node camera1_node;
    YAML::Node camera2_node;
    YAML::Node camera3_node;
    YAML::Node imu_node;

    // serial nunmber
    root_node["serial_num"] = device_id;

    // sensor bucket
    std::vector<std::vector<std::string> > v_sensor_buckets = {
    {"camera_0", "pinhole"}, {"camera_1", "pinhole"}, {"camera_2", "pinhole"}, {"camera_3", "pinhole"},
    {"horizon_lidar", "multi"},  {"vertical_lidar", "multi"}, {"imu_config"}};

    for (size_t i = 0; i < v_sensor_buckets.size(); ++i) {
        root_node["sensor_bucket"].push_back(v_sensor_buckets[i]);
        root_node["sensor_bucket"][i].SetStyle(YAML::EmitterStyle::Flow);
    }

    time_t tt;
    time(&tt);
    struct tm* t2 = localtime(&tt);
    char buf[1024];
    strftime(buf, sizeof(buf) - 1, "%c", t2);
    root_node["calibration_time"] = buf;

    Eigen::Matrix4d T_cam0_cam1 = cam1.T_ref_cam_;
    Eigen::Matrix4d T_cam0_cam2 = cam2.T_ref_cam_;
    Eigen::Matrix4d T_cam0_cam3 = cam3.T_ref_cam_;
    Eigen::Matrix4d T_cam0_base = mono_imu.T_cam0_imu;
    Eigen::Matrix4d T_base_cam0 = T_cam0_base.inverse();
    Eigen::Matrix4d T_base_cam1 = T_base_cam0 * T_cam0_cam1;
    Eigen::Matrix4d T_base_cam2 = T_base_cam0 * T_cam0_cam2;
    Eigen::Matrix4d T_base_cam3 = T_base_cam0 * T_cam0_cam3;
    Eigen::Matrix4d T_base_l1 = T_base_cam0 * T_lidar1_cam0.inverse();
    Eigen::Matrix4d T_base_l0 = T_base_l1 * T_lidar0_lidar1.inverse();
    Eigen::Matrix4d T_base_imu = Eigen::Matrix4d::Identity();
    // cameras
    {
        {
            camera0_node["type"] = cam0.camera_type;
            camera0_node["distort_type"] = cam0.distortion_type;
            camera0_node["image_height"] = cam0.img_height_;
            camera0_node["image_width"] = cam0.img_width_;
            camera0_node["intrinsics"]["cols"] = cam0.intrinsic_.cols();
            camera0_node["intrinsics"]["rows"] = cam0.intrinsic_.rows();
            camera0_node["intrinsics"]["data"] = cam0.intrinsic_;
            camera0_node["intrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);

            camera0_node["distort_coefficient"]["cols"] = cam0.distortions_.cols();
            camera0_node["distort_coefficient"]["rows"] = cam0.distortions_.rows();
            camera0_node["distort_coefficient"]["data"] = cam0.distortions_;
            camera0_node["distort_coefficient"]["data"].SetStyle(YAML::EmitterStyle::Flow);
            camera0_node["extrinsics"]["cols"] = T_base_cam0.cols();
            camera0_node["extrinsics"]["rows"] = T_base_cam0.rows();
            camera0_node["extrinsics"]["data"] = T_base_cam0;
            camera0_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
        }
        {
            camera1_node["type"] = cam1.camera_type;
            camera1_node["distort_type"] = cam1.distortion_type;
            camera1_node["image_height"] = cam1.img_height_;
            camera1_node["image_width"] = cam1.img_width_;
            camera1_node["intrinsics"]["cols"] = cam1.intrinsic_.cols();
            camera1_node["intrinsics"]["rows"] = cam1.intrinsic_.rows();
            camera1_node["intrinsics"]["data"] = cam1.intrinsic_;
            camera1_node["intrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);

            camera1_node["distort_coefficient"]["cols"] = cam1.distortions_.cols();
            camera1_node["distort_coefficient"]["rows"] = cam1.distortions_.rows();
            camera1_node["distort_coefficient"]["data"] = cam1.distortions_;
            camera1_node["distort_coefficient"]["data"].SetStyle(YAML::EmitterStyle::Flow);
            camera1_node["extrinsics"]["cols"] = T_base_cam1.cols();
            camera1_node["extrinsics"]["rows"] = T_base_cam1.rows();
            camera1_node["extrinsics"]["data"] = T_base_cam1;
            camera1_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
        }
        {
            camera2_node["type"] = cam2.camera_type;
            camera2_node["distort_type"] = cam2.distortion_type;
            camera2_node["image_height"] = cam2.img_height_;
            camera2_node["image_width"] = cam2.img_width_;
            camera2_node["intrinsics"]["cols"] = cam2.intrinsic_.cols();
            camera2_node["intrinsics"]["rows"] = cam2.intrinsic_.rows();
            camera2_node["intrinsics"]["data"] = cam2.intrinsic_;
            camera2_node["intrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);

            camera2_node["distort_coefficient"]["cols"] = cam2.distortions_.cols();
            camera2_node["distort_coefficient"]["rows"] = cam2.distortions_.rows();
            camera2_node["distort_coefficient"]["data"] = cam2.distortions_;
            camera2_node["distort_coefficient"]["data"].SetStyle(YAML::EmitterStyle::Flow);
            camera2_node["extrinsics"]["cols"] = T_base_cam2.cols();
            camera2_node["extrinsics"]["rows"] = T_base_cam2.rows();
            camera2_node["extrinsics"]["data"] = T_base_cam2;
            camera2_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
        }
        {
            camera3_node["type"] = cam3.camera_type;
            camera3_node["distort_type"] = cam3.distortion_type;
            camera3_node["image_height"] = cam3.img_height_;
            camera3_node["image_width"] = cam3.img_width_;
            camera3_node["intrinsics"]["cols"] = cam3.intrinsic_.cols();
            camera3_node["intrinsics"]["rows"] = cam3.intrinsic_.rows();
            camera3_node["intrinsics"]["data"] = cam3.intrinsic_;
            camera3_node["intrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);

            camera3_node["distort_coefficient"]["cols"] = cam3.distortions_.cols();
            camera3_node["distort_coefficient"]["rows"] = cam3.distortions_.rows();
            camera3_node["distort_coefficient"]["data"] = cam3.distortions_;
            camera3_node["distort_coefficient"]["data"].SetStyle(YAML::EmitterStyle::Flow);
            camera3_node["extrinsics"]["cols"] = T_base_cam3.cols();
            camera3_node["extrinsics"]["rows"] = T_base_cam3.rows();
            camera3_node["extrinsics"]["data"] = T_base_cam3;
            camera3_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
        }
    }

    // lidar 0
    {
        lidar0_node["max_dist"] = 30.0;
        lidar0_node["min_dist"] = 0.1;
        lidar0_node["starting_offset"] = -6.0;
        lidar0_node["num_rays"] = 16;
        lidar0_node["frequency"] = 10;
        lidar0_node["extrinsics"]["cols"] = T_base_l0.cols();
        lidar0_node["extrinsics"]["rows"] = T_base_l0.rows();
        lidar0_node["extrinsics"]["data"] = T_base_l0;
        lidar0_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
    }

    // lidar 1
    {
        lidar1_node["max_dist"] = 30.0;
        lidar1_node["min_dist"] = 0.1;
        lidar1_node["starting_offset"] = -6.0;
        lidar1_node["num_rays"] = 16;
        lidar1_node["frequency"] = 10;
        lidar1_node["extrinsics"]["cols"] = T_base_l1.cols();
        lidar1_node["extrinsics"]["rows"] = T_base_l1.rows();
        lidar1_node["extrinsics"]["data"] = T_base_l1;
        lidar1_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
    }

    // imu
    {
        imu_node["name"] = "xsens";
        imu_node["gyro_meas_noise"] = mono_imu.avg_gyro_noise_density;
        imu_node["gyro_bias_noise"] = mono_imu.avg_gyro_bias_noise_density;
        imu_node["acc_meas_noise"] = mono_imu.avg_acc_noise_density;
        imu_node["acc_bias_noise"] = mono_imu.avg_acc_bias_noise_density;
        imu_node["acc_bias"]["cols"] = mono_imu.acc_bias.cols();
        imu_node["acc_bias"]["rows"] = mono_imu.acc_bias.rows();
        imu_node["acc_bias"]["data"] = mono_imu.acc_bias;
        imu_node["acc_bias"]["data"].SetStyle(YAML::EmitterStyle::Flow);
        imu_node["gyro_bias"]["cols"] = mono_imu.gyro_bias.cols();
        imu_node["gyro_bias"]["rows"] = mono_imu.gyro_bias.rows();
        imu_node["gyro_bias"]["data"] = mono_imu.gyro_bias;
        imu_node["gyro_bias"]["data"].SetStyle(YAML::EmitterStyle::Flow);

        imu_node["extrinsics"]["cols"] = T_base_imu.cols();
        imu_node["extrinsics"]["rows"] = T_base_imu.rows();
        imu_node["extrinsics"]["data"] = T_base_imu;
        imu_node["extrinsics"]["data"].SetStyle(YAML::EmitterStyle::Flow);
    }

    root_node["camera_0"] = camera0_node;
    root_node["camera_1"] = camera1_node;
    root_node["camera_2"] = camera2_node;
    root_node["camera_3"] = camera3_node;
    root_node["horizon_lidar"] = lidar0_node;
    root_node["vertical_lidar"] = lidar1_node;
    root_node["imu_config"] = imu_node;

    yaml_out_file << root_node;
    yaml_out_file.close();

    return true;
}

bool readOutputFileTest(const std::string& file_name) {
  if (file_name.empty()) {
    std::cerr << "[readOutputFileTest] Empty input file name";
    return false;
  }

  std::ifstream fs(file_name);
  if (!fs.is_open()) {
    std::cout << "[readOutputFileTest] Error read " << file_name << "\n";
    return false;
  }

  YAML::Node root_node;

  try {
    root_node = YAML::LoadFile(file_name);
  } catch (YAML::BadFile& e) {
    std::cout << "[readOutputFileTest] Could not open file: " << file_name
              << "\n";
    return false;
  } catch (YAML::ParserException& e) {
    std::cout << "[readOutputFileTest] Invalid file format: " << file_name
              << "\n";
    return false;
  }
  if (root_node.IsNull()) {
    std::cout << "[readOutputFileTest] Could not open file: " << file_name
              << "\n";
    return false;
  }

  std::cout << "[readOutputFileTest] read " << file_name << "\n";

  YAML::Node left_stereo_node = root_node["left_stereo"];

  Eigen::Matrix3d stereo0_cam0_intrinsic =
      left_stereo_node["cam0"]["intrinsics"]["data"].as<Eigen::Matrix3d>();
  std::cout << "[readOutputFileTest] left_stereo-cam0-intrinsics:\n"
            << stereo0_cam0_intrinsic << "\n";

  Eigen::Vector4d stereo0_cam0_distortion =
      left_stereo_node["cam0"]["distort_coefficient"]["data"]
          .as<Eigen::Vector4d>();
  std::cout << "[readOutputFileTest] left_stereo-cam0-distort:\n"
            << stereo0_cam0_distortion << "\n";

  Eigen::Matrix4d stereo0_extrinsics =
      left_stereo_node["cam_extrinsics"]["data"].as<Eigen::Matrix4d>();
  std::cout << "[readOutputFileTest] left_stereo cam-extrinsic:\n"
            << stereo0_extrinsics << "\n";

  return true;
}

bool loadMonoCamParam(const std::string &cam_intrin_filepath, MonoCamParam &cam, std::string cam_ext_filepath){
    if (!common::fileExists(cam_intrin_filepath)){
        LOG(FATAL) << "file " << cam_intrin_filepath << " doesn't exist!";
        return false;
    }

    cv::Mat K, D;
    std::string dist_type;
    int width, height;
    if( !common::loadIntrinFileOpencv(cam_intrin_filepath, K, D, dist_type, width, height)){
        if (!common::loadIntrinFileKalibr(cam_intrin_filepath, K, D, dist_type, width, height))
            return false;
    }

    cv::cv2eigen(K, cam.intrinsic_);
    // D is type of (1, 4) matrix, while distortions_ is (4,1) matrix
    cv::cv2eigen(D.t(), cam.distortions_);

    cam.img_width_ = width;
    cam.img_height_ = height;
    cam.distortion_type = dist_type;

    LOG(INFO) << "camera distortion type: " << dist_type;
    if (cam_ext_filepath.empty()){
        cam.T_ref_cam_ = Eigen::Matrix4d::Identity();
    }else{
        if (!common::loadExtFileOpencv(cam_ext_filepath, cam.T_ref_cam_))
            return false;
    }

    return true;
}

bool loadIMUIntrinFileKalibr(const std::string &imu_intrin_filepath, MonoIMUParam &cam0_imu){
    if (!common::fileExists(imu_intrin_filepath)){
        LOG(FATAL) << "File doesnt exist : " << imu_intrin_filepath;
        return false;
    }

    std::ifstream fs(imu_intrin_filepath);
    if (!fs.is_open()) {
    LOG(INFO) << " Error read " << imu_intrin_filepath << "\n";
    return false;
    }

    YAML::Node root_node;
    try {
    root_node = YAML::LoadFile(imu_intrin_filepath);
    } catch (YAML::BadFile& e) {
    LOG(INFO) << " Could not open file: " << imu_intrin_filepath << "\n";
    return false;
    } catch (YAML::ParserException& e) {
    LOG(INFO) << " Invalid file format: " << imu_intrin_filepath << "\n";
    return false;
    }
    if (root_node.IsNull()) {
    LOG(INFO) << " Could not open file: " << imu_intrin_filepath << "\n";
    return false;
    }

    double acc_noise = root_node["accelerometer_noise_density"].as<double>();
    double gyro_noise = root_node["gyroscope_noise_density"].as<double>();
    double acc_bias_noise = root_node["accelerometer_random_walk"].as<double>();
    double gyro_bias_noise = root_node["gyroscope_random_walk"].as<double>();
    LOG(INFO) << "acc_noise: " << acc_noise;
    LOG(INFO) << "gyro_noise: " << gyro_noise;
    LOG(INFO) << "acc_bias_noise: " << acc_bias_noise;
    LOG(INFO) << "gyro_bias_noise: " << gyro_bias_noise;
    cam0_imu.avg_acc_noise_density = acc_noise;
    cam0_imu.avg_acc_bias_noise_density = acc_bias_noise;
    cam0_imu.avg_gyro_noise_density = gyro_noise;
    cam0_imu.avg_gyro_bias_noise_density = gyro_bias_noise;
    cam0_imu.acc_bias[0] = root_node["acc_bias"][0].as<double>();
    cam0_imu.acc_bias[1] = root_node["acc_bias"][1].as<double>();
    cam0_imu.acc_bias[2] = root_node["acc_bias"][2].as<double>();
    cam0_imu.gyro_bias[0] = root_node["gyro_bias"][0].as<double>();
    cam0_imu.gyro_bias[1] = root_node["gyro_bias"][1].as<double>();
    cam0_imu.gyro_bias[2] = root_node["gyro_bias"][2].as<double>();
    LOG(INFO) << "acc_bias : " << cam0_imu.acc_bias.transpose();
    LOG(INFO) << "gyro_bias : " << cam0_imu.gyro_bias.transpose();

    return true;
}

int main(int argc, char* argv[]) {
    if (argc < 4) {
        std::cerr << " Usage : test_gen_backpack_yaml [calib_dataset_folder] [output_folder] [device_id]\n";
        return -1;
    }

    // dataset folder must contains  extrinsics : camera0_to_camera1.yml camera0_to_camera2.yml camera0_to_camera3.yml lidar0_to_lidar1.yml lidar1_to_camera0.yml camera0_to_imu.yaml
    // intrinsics: cam0.yml cam1.yml cam2.yml cam3.yml
    std::string input_folder = argv[1];
    std::string output_folder = argv[2];
    std::string device_id = argv[3];
    std::string backpack_device_name = "backpack"+device_id;

    // extrinsic files
    std::string lidar0_2_lidar1_filepath = common::concatenateFolderAndFileName(input_folder, "lidar0_to_lidar1.yml");
    std::string lidar1_2_cam0_filepath = common::concatenateFolderAndFileName(input_folder, "lidar1_to_camera0.yml");
    std::string cam0_2_cam1_filepath = common::concatenateFolderAndFileName(input_folder, "camera0_to_camera1.yml");
    std::string cam0_2_cam2_filepath = common::concatenateFolderAndFileName(input_folder, "camera0_to_camera2.yml");
    std::string cam0_2_cam3_filepath = common::concatenateFolderAndFileName(input_folder, "camera0_to_camera3.yml");
    std::string cam0_2_imu_filepath = common::concatenateFolderAndFileName(input_folder, "camera0_to_imu.yaml");
    // intrinsic files
    std::string cam0_intrin_filepath = common::concatenateFolderAndFileName(input_folder, "cam0.yml");
    std::string cam1_intrin_filepath = common::concatenateFolderAndFileName(input_folder, "cam1.yml");
    std::string cam2_intrin_filepath = common::concatenateFolderAndFileName(input_folder, "cam2.yml");
    std::string cam3_intrin_filepath = common::concatenateFolderAndFileName(input_folder, "cam3.yml");
    std::string imu_intrin_filepath = common::concatenateFolderAndFileName(input_folder, backpack_device_name+"_imu.yaml");

    std::string output_filepath =  common::concatenateFolderAndFileName(output_folder, "raw_backpack.yaml");

    MonoCamParam cam0, cam1, cam2, cam3;
    MonoIMUParam cam0_imu;
    bool sts = loadMonoCamParam(cam0_intrin_filepath, cam0, "");
    if (!sts){
        LOG(ERROR) << "Fail to parse cam0 intrinsic file and extrinsic file " << cam0_intrin_filepath << ", ";
        return -1;
    }
    sts = loadMonoCamParam(cam1_intrin_filepath, cam1, cam0_2_cam1_filepath);
    if (!sts){
        LOG(ERROR) << "Fail to parse cam1 intrinsic file and extrinsic file " << cam1_intrin_filepath << ", " << cam0_2_cam1_filepath;
        return -1;
    }
    sts = loadMonoCamParam(cam2_intrin_filepath, cam2, cam0_2_cam2_filepath);
    if (!sts){
        LOG(ERROR) << "Fail to parse cam2 intrinsic file and extrinsic file " << cam2_intrin_filepath << ", " << cam0_2_cam2_filepath;
        return -1;
    }
    sts = loadMonoCamParam(cam3_intrin_filepath, cam3, cam0_2_cam3_filepath);
    if (!sts){
        LOG(ERROR) << "Fail to parse cam3 intrinsic file and extrinsic file " << cam3_intrin_filepath << ", " << cam0_2_cam3_filepath;
        return -1;
    }
    // load cam2imu extrinsic
    // sts = loadMonoIMUExtFileKalibr(cam0_2_imu_filepath, cam0_imu);
    sts = common::loadExtFileOpencv(cam0_2_imu_filepath, cam0_imu.T_cam0_imu);
    if (!sts) {
          LOG(ERROR) << " Fail to read "<< cam0_2_imu_filepath;
          return -1;
    }
    // load imu intrinsic parameters
    sts = loadIMUIntrinFileKalibr(imu_intrin_filepath, cam0_imu);
    if (!sts) {
        LOG(ERROR) << " Fail to read "<< imu_intrin_filepath;
        return -1;
    }

    // lidar0 2 lidar1
    Eigen::Matrix4d T_lidar0_lidar1 = Eigen::Matrix4d::Identity();
    sts = common::loadExtFileOpencv(lidar0_2_lidar1_filepath, T_lidar0_lidar1);
    if (!sts) {
        LOG(FATAL) << " Fail to read " << lidar0_2_lidar1_filepath << "\n";
        return -1;
    }

    // lidar1 2 cam0
    Eigen::Matrix4d T_lidar1_cam0 = Eigen::Matrix4d::Identity();
    sts = common::loadExtFileOpencv(lidar1_2_cam0_filepath, T_lidar1_cam0);
    if (!sts) {
        LOG(FATAL) << " Fail to read " << lidar1_2_cam0_filepath << "\n";
        return -1;
    }
    // define imu frame as body frame
    sts = writeYAMLOutputFile(output_filepath, cam0, cam1, cam2, cam3,
                            cam0_imu, T_lidar0_lidar1, T_lidar1_cam0, std::stoi(device_id));
    if (!sts) {
        LOG(ERROR) << " Fail to write " << output_filepath << "\n";
        return -1;
    }

    //    sts = readOutputFileTest(output_fn);

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_CBRT_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_CBRT_HPP_INCLUDED

#include <nt2/exponential/functions/cbrt.hpp>
#include <boost/simd/sdk/math.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <nt2/include/functions/scalar/is_invalid.hpp>
#include <nt2/sdk/error/policies.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::cbrt_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::cbrt(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::cbrt_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0)) return a0;
#ifdef BOOST_SIMD_HAS_CBRT
      return ::cbrt(a0);
#else
      return boost::math::cbrt(a0, nt2_policy());
#endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::cbrt_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_invalid(a0)) return a0;
#ifdef BOOST_SIMD_HAS_CBRTF
      return ::cbrtf(a0);
#else
      return boost::math::cbrt(a0, nt2_policy());
#endif
    }
  };
} }

#endif


// Fucntions for computing the initial and updated barrier stiffnesses.

#pragma once

#include <Eigen/Core>

#include <ipc/collision_constraint.hpp>

namespace ipc {

/// Compute an inital barrier stiffness using the barrier potential gradient.
double initial_barrier_stiffness(
    double bbox_diagonal,
    double dhat,
    double average_mass,
    const Eigen::VectorXd& grad_energy,
    const Eigen::VectorXd& grad_barrier,
    double& max_barrier_stiffness,
    double min_barrier_stiffness_scale = 1e11,
    double dmin = 0);

/// Update the barrier stiffness if the distance is decreasing and less than
/// dhat_epsilon_scale * diag.
void update_barrier_stiffness(
    double prev_min_distance,
    double min_distance,
    double max_barrier_stiffness,
    double& barrier_stiffness,
    double bbox_diagonal,
    double dhat_epsilon_scale = 1e-9,
    double dmin = 0);

/// Update the barrier stiffness if the distance is decreasing and less than
/// dhat_epsilon_scale * diag.
void update_barrier_stiffness(
    const Constraints& constraint_set,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    double prev_min_distance,
    double& min_distance,
    double max_barrier_stiffness,
    double& barrier_stiffness,
    double dhat_epsilon_scale = 1e-9,
    double dmin = 0);

} // namespace ipc