/*
This file is a part of Raman-Scattering-Code-Conversion.
<https://github.com/Kirbologist/Raman-Scattering-Code-Conversion>

Written by Siwan Li for the UQ School of Maths and Physics.
Copyright (C) 2021-2022 Siwan Li

This source code form is subject to the terms of the MIT License.
If a copy of the MIT License was not distributed with this file,
you can obtain one at <https://opensource.org/licenses/MIT>.

This code is used to calculate Raman scattering off of spheroids.
None of the code was included in the original SMARTIES package.
Many functions are used to provide I/O support to the main function.
*/

#ifndef RAMAN_ELASTIC_SCATTERING_HPP
#define RAMAN_ELASTIC_SCATTERING_HPP

#include "core.hpp"
#include "smarties.hpp"
#include <boost/lexical_cast.hpp>
#include <chrono>
#include <filesystem>
#include <fstream>
#include <exception>

using namespace std;
using namespace Eigen;
using namespace Smarties;
using boost::lexical_cast;
using boost::bad_lexical_cast;

/* Possible floating-point types used in calculations */
enum CalcType {SINGLE, DOUBLE, QUAD, CUSTOM, NONE};

/*
Controllable parameters that are used for calculations.
_rm suffix denotes parameters used for excitation T-matrix,
absense of _rm suffix denotes parameters used for Raman T-matrix.
*/
template <class Real>
struct RamanParams {
  // Describes the minimum and maximum of the particle diameter
  Real dia_min = 1000;
  Real dia_max = 2000;

  int N_rad = 100; // Number of particle sizes to calculate for
  int N_theta_p = 19; // Number of particle orientations to calculate for
  Real phi_p = 0; // Angle of rotation of particle about the z semiaxis

  // Number of spherical coordinates used throughout the volume of the particle
  int N_r = 100;
  int N_theta = 320;
  int N_phi = 320;

  // Values of h used, where h = a/c. N_h is the number of h values
  int N_h = 1;
  Real h_min = static_cast<Real>(1.0)/3;
  Real h_max = static_cast<Real>(1.0)/3;

  // Incident light parameters. Used for stParams struct
  Real epsilon1 = 1;
  Real epsilon2 = pow(static_cast<Real>(1.35), 2);
  Real epsilon2_rm = pow(static_cast<Real>(1.344), 2);
  Real lambda = 355;
  Real lambda_rm = 403.7;

  // Used for stParams struct
  int Nb_theta = 1000;
  int Nb_theta_pst = 1;

  // Used for stOptions struct
  int delta = 0;

  // Vectors of all possible radii/theta/h values
  ArrayXr<Real> rad_var;
  ArrayXr<Real> theta_p_var;
  ArrayXr<Real> h_var;
};

// All non-template functions defined in raman_elastic_scattering.cpp

/*
Get the 2 types to be used for floating-point calculations from a text file.
Inputs:
  in_file_name: path of the text file which contains the calculation type
Output:
  String array of size 2 that specifies 2 calculation types.
*/
std::array<CalcType, 2> GetCalcType(string in_file_name);

/*
Get the value given for some parameter from a text file.
Inputs:
  in_file_name: path of the text file which contains the option
  option: The exact string of the specific option/parameter to look for
Output:
  The user-defined value given to the option, as a string
*/
string GetOption(string in_file_name, string option);

/*
Determine if it's okay to write calculation info to a file from an option in a text file.
I.e. it checks the "Print output to file:" parameter.
Inputs:
  in_file_name: path of the text file which specifies if it's okay to write to output or not
Output:
  True if the file says "yes", i.e. it is okay, false otherwise
*/
bool CanWriteOutput(string in_file_name);

/*
Determine number of CPUs to use from a text file. I.e. it checks the "No. of CPUs" parameter.
Inputs:
  in_file_name: path of the text file which specifies the number of CPUs.
Output:
  The number of CPUs that are allowed
*/
int GetNumCPUs(string in_file_name);

/*
Determine number of threads to split the particle radii for from a text file.
I.e. it checks the "No. of CPUs to partition particle radii for:" parameter.
Note that any one of these threads can still fork into more threads if there are any available.
Inputs:
  in_file_name: path of the text file which specifies the number of CPUs.
Output:
  The number of CPUs to partition particle radii for
*/
int GetNumParticleCPUs(string in_file_name);

/*
Prints a string to both standard output (the terminal) and appends it to a text ilfe (if allowed).
Inputs:
  out_string: the string to print/write
  out_file_name: path of the text file to write to
  write_output: true enables writing to output, false disables it.
*/
void MultiPrint(string out_string, string out_file_name, bool write_output = false);

/*
Converts a string that's a fraction of two floats into a single float value
Throws a bad_lexical_cast error if string can't be read or converted.
Input:
  `frac` - must either be a floating-point literal, or two floating-point literals deliminated by a '/' character.
Output:
  the calculated value as a floating-point variable.
*/
template <class Real>
Real Frac2Float(string frac) {
  Real output;
  size_t offset = frac.find("/");
  if (offset == string::npos) {
    output = static_cast<Real>(lexical_cast<Real>(frac));
    return output;
  }
  output = static_cast<Real>(lexical_cast<Real>(frac.substr(0, offset)));
  output /= static_cast<Real>(lexical_cast<Real>(frac.substr(offset + 1)));
  return output;
}

/*
Creates RamanParams struct by reading values from text file.
The file can be opened or unopened, but must not be modified during this function call.
Inputs:
  `in_file_name` - path to the file
Output:
  A unique pointer to a new RamanParams struct with member values based on the parameters given in `in_file_name`.
Dependencies:
  GetOption, Frac2Float
*/
template <class Real>
unique_ptr<RamanParams<Real>> LoadParams(string in_file_name) {
  ifstream in_file;
  in_file.open(in_file_name, ios::in);
  if (!in_file.is_open())
    throw runtime_error("Error: cannot open " + in_file_name);

  auto output = make_unique<RamanParams<Real>>();

  string line;
  // Lists all lines/options to check for. Options with floating-point values are listed first,
  // then options with integral values are checked.
  vector<string> options {
    "Minimum diameter:", "Maximum diameter:", "Particle phi:", "Minimum h:", "Maximum h:",
    "epsilon1:", "epsilon2:", "Raman epsilon2:", "lambda:", "Raman lambda:",
    "No. of particle radii:", "No. of particle thetas:", "No. of h ratios:",
    "No. of r-coordinates:", "No. of theta-coordinates:", "No. of phi-coordinates:",
    "Nb_theta:", "Nb_theta_pst:", "Delta:"
  };
  std::array<Real, 10> float_params = {
    output->dia_min, output->dia_max, output->phi_p, output->h_min, output->h_max,
    output->epsilon1, output->epsilon2, output->epsilon2_rm, output->lambda, output->lambda_rm
  };
  std::array<int, 9> int_params = {
    output->N_rad, output->N_theta_p, output->N_h, output->N_r, output->N_theta, output->N_phi,
    output->Nb_theta, output->Nb_theta_pst, output->delta
  };
  for (size_t i = 0; i < options.size(); i++) {
    string option = options[i];
    string value = GetOption(in_file_name, option);

    try {
      if (i < float_params.size())
        float_params[i] = Frac2Float<Real>(value);
      else if (i < float_params.size() + int_params.size())
        int_params[i - float_params.size()] = lexical_cast<int>(value);
    } catch(bad_lexical_cast&) {
      cerr << "Cannot read value of option \"" << option << "\". Using default value.";
    }

    output->dia_min = float_params[0];
    output->dia_max = float_params[1];
    output->phi_p = float_params[2];
    output->h_min = float_params[3];
    output->h_max = float_params[4];
    output->epsilon1 = float_params[5];
    output->epsilon2 = float_params[6];
    output->epsilon2_rm = float_params[7];
    output->lambda = float_params[8];
    output->lambda_rm = float_params[9];
    output->N_rad = int_params[0];
    output->N_theta_p = int_params[1];
    output->N_h = int_params[2];
    output->N_r = int_params[3];
    output->N_theta = int_params[4];
    output->N_phi = int_params[5];
    output->Nb_theta = int_params[6];
    output->Nb_theta_pst = int_params[7];
    output->delta = int_params[8];
  }
  ArrayXr<Real> dia_var = ArrayXr<Real>::LinSpaced(output->N_rad, output->dia_min, output->dia_max);
  output->rad_var = dia_var/2;
  output->theta_p_var = ArrayXr<Real>::LinSpaced(output->N_theta_p, 0, mp_pi<Real>()/2);
  output->h_var = ArrayXr<Real>::LinSpaced(output->N_h, output->h_min, output->h_max);
  return output;
}

/*
Takes a RamanParams struct and uses it to generate an stParams struct for use with SMARTIES functions.
stParams member values are based on RamanParams member values, and on the radius and h value
at rad_ind and h_ind of rad_var and h_var respectively.
Inputs:
  `raman_params` - unique pointer to a RamanParams struct
  `rad_ind` - the index of the entry to use in `raman_params`->rad_var
  `h_ind` - the index of the entry to use in `raman_params`->h_var
  `type` - the type of values the output struct should have. If it's "rm",
           then the values denote the parameters of the Raman T-matrix. Otherwise they denote the parameters
           of the excitation T-matrix.
Output:
  returns a unique pointer to a new stParams struct
Dependencies:
  mp_pi
*/
template <class Real>
unique_ptr<stParams<Real>> Raman2SmartiesParams(
    const unique_ptr<RamanParams<Real>>& raman_params, int rad_ind, int h_ind, string type = string()) {
  auto params = make_unique<stParams<Real>>();
  params->epsilon1 = raman_params->epsilon1;
  params->lambda = ArrayXr<Real>(1);
  params->epsilon2 = ArrayXr<Real>(1);
  params->k1 = ArrayXr<Real>(1);
  params->s = ArrayXr<Real>(1);
  if (type == "rm") {
    params->lambda(0) = raman_params->lambda_rm;
    params->epsilon2(0) = raman_params->epsilon2_rm;
    params->k1(0) = 2*mp_pi<Real>() / raman_params->lambda_rm * sqrt(raman_params->epsilon1);
    params->s(0) = sqrt(raman_params->epsilon2_rm) / sqrt(raman_params->epsilon1);
  } else {
    params->lambda(0) = raman_params->lambda;
    params->epsilon2(0) = raman_params->epsilon2;
    params->k1(0) = 2*mp_pi<Real>() / raman_params->lambda * sqrt(raman_params->epsilon1);
    params->s(0) = sqrt(raman_params->epsilon2) / sqrt(raman_params->epsilon1);
  }
  params->Nb_theta = raman_params->Nb_theta;
  params->Nb_theta_pst = raman_params->Nb_theta_pst;

  Real h = raman_params->h_var(h_ind);
  Real radius = raman_params->rad_var(rad_ind);
  if (h > 1) {
    params->a = radius;
    params->c = radius / h;
  } else {
    params->a = radius * h;
    params->c = radius;
  }
  // The greater N is, the better the T-matrix converges.
  params->N = 6 + 2*static_cast<int>(ceil(max(params->a, params->c)/40));
  return params;
}

/*
Appends a stamp with time and parameter details of Raman scattering calculations at the end of a text file.
Function opens the file itself. It does not check for any errors.
`Real1` and `Real2` are the two selectable calculation types used for Raman scattering calculations.
DEV NOTE: Can make this into a non-template function using c++20 'concepts' keyword
Inputs:
  - out_file_name: text file to write stamp to
  - raman_params: unique pointer to a RamanParams struct containing the parameters to be written
Dependencies:
  - GetTypeName
*/
template <class Real1, class Real2>
void CreateTimeStamp(string out_file_name, const unique_ptr<RamanParams<Real1>>& raman_params) {
  string main_calc_type = GetTypeName<Real1>();
  string second_calc_type = GetTypeName<Real2>();

  ofstream out_file(out_file_name, ios::out | ios::app);
  auto sys_time = chrono::system_clock::now();
  time_t sys_time_date = chrono::system_clock::to_time_t(sys_time);
  out_file << "Session began at system time: " << ctime(&sys_time_date);
  out_file << "Running RamanElasticScattering with type " << main_calc_type;
  out_file << " and " << second_calc_type << endl;
  out_file.flush();
  out_file.close();
}

/*
Converts an existing stTR struct containing entries of one type into a new stTR struct
with the same entries converted into another type. This could theoretically be
implemented as a template cast operator or template copy constructor.
`From` is the entry type of the source struct and `To` is the entry type of the new struct.
Inputs:
  - st_TR_list: the source stTR struct, with entries of type `From`
Outputs:
  - Returns a new stTR struct, with entries of type `To`.
Dependencies:
  mp_im_unit
*/
template <class From, class To>
vector<unique_ptr<stTR<To>>> ConvertStTRList(const vector<unique_ptr<stTR<From>>>& st_TR_list) {
  vector<unique_ptr<stTR<To>>> output(st_TR_list.size());
  complex<To> I = mp_im_unit<To>();
  for (size_t i = 0; i < st_TR_list.size(); i++) {
    assert(st_TR_list[i]);
    assert(st_TR_list[i]->mat_list.size() == 2);
    assert(st_TR_list[i]->mat_list[0] == "st_4M_T");
    assert(st_TR_list[i]->mat_list[1] == "st_4M_R");
    output[i] = make_unique<stTR<To>>();
    output[i]->mat_list = st_TR_list[i]->mat_list;

    // Direct casting from a complex type to a complex type containing a Boost MPFR type isn't supported.
    output[i]->st_4M_T_eo().M11 = st_TR_list[i]->st_4M_T_eo().M11.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_eo().M11.imag().template cast<To>();
    output[i]->st_4M_T_eo().M12 = st_TR_list[i]->st_4M_T_eo().M12.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_eo().M12.imag().template cast<To>();
    output[i]->st_4M_T_eo().M21 = st_TR_list[i]->st_4M_T_eo().M21.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_eo().M21.imag().template cast<To>();
    output[i]->st_4M_T_eo().M22 = st_TR_list[i]->st_4M_T_eo().M22.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_eo().M22.imag().template cast<To>();
    output[i]->st_4M_T_eo().m = st_TR_list[i]->st_4M_T_eo().m;
    output[i]->st_4M_T_eo().ind1 = st_TR_list[i]->st_4M_T_eo().ind1;
    output[i]->st_4M_T_eo().ind2 = st_TR_list[i]->st_4M_T_eo().ind2;

    output[i]->st_4M_T_oe().M11 = st_TR_list[i]->st_4M_T_oe().M11.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_oe().M11.imag().template cast<To>();
    output[i]->st_4M_T_oe().M12 = st_TR_list[i]->st_4M_T_oe().M12.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_oe().M12.imag().template cast<To>();
    output[i]->st_4M_T_oe().M21 = st_TR_list[i]->st_4M_T_oe().M21.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_oe().M21.imag().template cast<To>();
    output[i]->st_4M_T_oe().M22 = st_TR_list[i]->st_4M_T_oe().M22.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_T_oe().M22.imag().template cast<To>();
    output[i]->st_4M_T_oe().m = st_TR_list[i]->st_4M_T_oe().m;
    output[i]->st_4M_T_oe().ind1 = st_TR_list[i]->st_4M_T_oe().ind1;
    output[i]->st_4M_T_oe().ind2 = st_TR_list[i]->st_4M_T_oe().ind2;

    output[i]->st_4M_R_eo().M11 = st_TR_list[i]->st_4M_R_eo().M11.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_eo().M11.imag().template cast<To>();
    output[i]->st_4M_R_eo().M12 = st_TR_list[i]->st_4M_R_eo().M12.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_eo().M12.imag().template cast<To>();
    output[i]->st_4M_R_eo().M21 = st_TR_list[i]->st_4M_R_eo().M21.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_eo().M21.imag().template cast<To>();
    output[i]->st_4M_R_eo().M22 = st_TR_list[i]->st_4M_R_eo().M22.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_eo().M22.imag().template cast<To>();
    output[i]->st_4M_R_eo().m = st_TR_list[i]->st_4M_R_eo().m;
    output[i]->st_4M_R_eo().ind1 = st_TR_list[i]->st_4M_R_eo().ind1;
    output[i]->st_4M_R_eo().ind2 = st_TR_list[i]->st_4M_R_eo().ind2;

    output[i]->st_4M_R_oe().M11 = st_TR_list[i]->st_4M_R_oe().M11.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_oe().M11.imag().template cast<To>();
    output[i]->st_4M_R_oe().M12 = st_TR_list[i]->st_4M_R_oe().M12.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_oe().M12.imag().template cast<To>();
    output[i]->st_4M_R_oe().M21 = st_TR_list[i]->st_4M_R_oe().M21.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_oe().M21.imag().template cast<To>();
    output[i]->st_4M_R_oe().M22 = st_TR_list[i]->st_4M_R_oe().M22.real().template cast<To>() +
        I * st_TR_list[i]->st_4M_R_oe().M22.imag().template cast<To>();
    output[i]->st_4M_R_oe().m = st_TR_list[i]->st_4M_R_oe().m;
    output[i]->st_4M_R_oe().ind1 = st_TR_list[i]->st_4M_R_oe().ind1;
    output[i]->st_4M_R_oe().ind2 = st_TR_list[i]->st_4M_R_oe().ind2;
  }
  return output;
}

/*
Performs calculations of the Raman scattering of light off of spheroids with multiprocessing.
If calculations are done for multiple spheroid radii, the calculations for each radii may be
allocated to the threads using dynamic scheduling.
The parameters are based on some input file, the T-matrices are calculated using floating-point type `Real1`,
and the field calculations are calculated using floating-point type `Real2`.
It then prints a summary of results to the standard output (terminal) and in a set of files in a given directory.
Each thread prints its own calculations to its own output file, so each output file is named
based on the parameters on the number of the thread.
Inputs:
  - in_file_name: path to the file containing parameters for use in the Raman scattering calculations.
                  Parameter details are given in the README file.
  - out_dir: directory to write output files to. If empty, output files are written to.
Dependencies:
  mp_pi, ArrayMap, TensorCast, TensorConj, LoadParams, CreateTimeStamp, MultiPrint,
  Raman2SmartiesParams, ConvertStTRList, slvForT, pstScatteringMatrixOA, vshMakeIncidentParams,
  rvhGetFieldCoefficients, pstMakeStructForField, vshEgenThetaAllPhi, vshEthetaForPhi
*/
template <class Real1, class Real2>
void RamanElasticScattering(string in_file_name, string out_dir = "", int cpus = 1) {
  // Initialise all parameters and variables
  Real2 PI = mp_pi<Real2>();

  unique_ptr<RamanParams<Real1>> raman_params1 = LoadParams<Real1>(in_file_name);
  unique_ptr<RamanParams<Real2>> raman_params2 = LoadParams<Real2>(in_file_name);
  bool write_output = (out_dir != "");

  Tensor3r<Real2> sigma_yz(raman_params2->N_rad, raman_params2->N_h, raman_params2->N_theta_p);
  Tensor3r<Real2> sigma_zy(raman_params2->N_rad, raman_params2->N_h, raman_params2->N_theta_p);
  Tensor3r<Real2> sigma_zz(raman_params2->N_rad, raman_params2->N_h, raman_params2->N_theta_p);
  Tensor3r<Real2> sigma_yy(raman_params2->N_rad, raman_params2->N_h, raman_params2->N_theta_p);
  ArrayXr<Real1> C_sca = ArrayXr<Real1>::Zero(raman_params1->N_rad);
  ArrayXr<Real1> C_ext = ArrayXr<Real1>::Zero(raman_params1->N_rad);
  ArrayXr<Real1> C_abs = ArrayXr<Real1>::Zero(raman_params1->N_rad);
  vector<unique_ptr<stSM<Real2>>> stSM_list(raman_params2->N_rad);

  auto options = make_unique<stOptions>();
  options->get_R = true; // Needed for near fields and will be overridden in any case
  options->delta = raman_params1->delta; // Use delta=-1 to estimate it automatically.
  options->NB = 0; // NB will be estimated automatically
  options->get_symmetric_T = false;
  Real2 phi_p = raman_params2->phi_p;

  int N_r = raman_params2->N_r;
  int N_theta = raman_params2->N_theta;
  int N_phi = raman_params2->N_phi;

  // Initialises a set regularly spaced theta coordinates that are used for internal field calculations
  RowArrayXr<Real2> theta_surf = RowArrayXr<Real2>::LinSpaced(N_theta, 0, PI);
  // Initialises a non-uniform set of N_r radii inside a unit ball
  // such that spherical shells between adjacent radii are equal volume
  ArrayXr<Real2> r_surf_u = ArrayXr<Real2>::LinSpaced(
      N_r, 1/static_cast<Real2>(N_r), 1).pow(static_cast<Real2>(1.0)/3);
  ArrayXXr<Real2> r_mat_u = r_surf_u.replicate(1, N_theta);
  ArrayXXr<Real2> theta_mat = theta_surf.replicate(N_r, 1);

  // Initialise output files
  if (write_output)
    std::filesystem::create_directory(out_dir);

  for (int h_ind = 0; h_ind < raman_params1->N_h; h_ind++) {
    // Split the iterations of the next for-loop among threads for parallel processing.
    #pragma omp parallel for num_threads(cpus) schedule(dynamic)
    for (int k = 0; k < raman_params1->N_rad; k++) {
      string out_file_name = out_dir + "/a2c_" + to_string(raman_params1->h_var(0)) +
          "_to_" + to_string(raman_params1->h_var(last)) + "_dia_" + to_string(raman_params1->dia_min) +
          "_to_" + to_string(raman_params1->dia_max) + "_range_" + to_string(omp_get_thread_num()) + ".txt";
      if (write_output) {
        ofstream out_file(out_file_name, ios::out | ios::app);
        out_file << endl;
        if (!out_file.good()) {
          cerr << "Warning: cannot create or open output file. Some output won't be written." << endl;
          out_file.close();
          write_output = false;
        }
      }
      if (write_output)
        CreateTimeStamp<Real1, Real2>(out_file_name, raman_params1);
      stringstream out_stream;

      out_stream.precision(4);
      out_stream << "aspect ratio " << raman_params1->h_var(h_ind) << endl;
      MultiPrint(out_stream.str(), out_file_name, write_output);
      out_stream.str(string());

      unique_ptr<stParams<Real1>> params1 = Raman2SmartiesParams(raman_params1, k, h_ind);
      unique_ptr<stParams<Real1>> params_rm1 = Raman2SmartiesParams(raman_params1, k, h_ind, "rm");
      unique_ptr<stParams<Real2>> params2 = Raman2SmartiesParams(raman_params2, k, h_ind);
      unique_ptr<stParams<Real2>> params_rm2 = Raman2SmartiesParams(raman_params2, k, h_ind, "rm");
      Real2 a = params2->a;
      Real2 c = params2->c;

      int N = params2->N;
      // Initialises array of radii coordinates that are used for internal field calculations
      // by conforming r_mat_u to the spheroid shape. The ratios of radii along the same angle theta is kept invariant.
      ArrayXXr<Real2> r_mat = a*c*r_mat_u/sqrt(pow(c, 2)*sin(theta_mat).pow(2) + pow(a, 2)*cos(theta_mat).pow(2));
      ArrayXXr<Real2> d_theta = theta_mat(all, seq(1, last)) - theta_mat(all, seq(0, last - 1));
      ArrayXXr<Real2> dr = r_mat(seq(1, last), all) - r_mat(seq(0, last - 1), all);
      ArrayXXr<Real2> dt_dr = d_theta(seq(1, last), all) * dr(all, seq(1, last)); // Jacobian of the integral
      RowArrayXr<Real2> r_row = r_mat.reshaped().transpose();
      RowArrayXr<Real2> theta_row = theta_mat.reshaped().transpose();

      chrono::steady_clock::time_point begin, end;
      chrono::duration<double> elapsed_seconds;

      // Calculating T-matrix for excitation
      begin = chrono::steady_clock::now();
      unique_ptr<stTmatrix<Real1>> T_mat = slvForT(params1, options);
      end = chrono::steady_clock::now();
      elapsed_seconds = end - begin;
      out_stream << elapsed_seconds.count() << endl;
      out_stream << "T-matrix calculatred for excitation" << endl;
      MultiPrint(out_stream.str(), out_file_name, write_output);
      out_stream.str(string());

      // Calculating T-matrix for Raman
      begin = chrono::steady_clock::now();
      unique_ptr<stTmatrix<Real1>> T_mat_rm = slvForT(params_rm1, options);
      end = chrono::steady_clock::now();
      elapsed_seconds = end - begin;
      out_stream << elapsed_seconds.count() << endl;
      out_stream << "T-matrix calculatred for Raman" << endl;
      MultiPrint(out_stream.str(), out_file_name, write_output);
      out_stream.str(string());

      // Converting variables to secondary calculation type
      vector<unique_ptr<stTR<Real2>>> st_TR_list = ConvertStTRList<Real1, Real2>(T_mat->st_TR_list);
      vector<unique_ptr<stTR<Real2>>> st_TR_list_rm = ConvertStTRList<Real1, Real2>(T_mat_rm->st_TR_list);

      C_sca(k) = T_mat->st_C_oa->C_sca(0); // st_C_oa->sca should only contain 1 element
      C_ext(k) = T_mat->st_C_oa->C_ext(0); // st_C_oa->ext should only contain 1 element
      C_abs(k) = T_mat->st_C_oa->C_abs(0); // st_C_oa->abs should only contain 1 element
      out_stream.precision(10);
      out_stream << "C_sca " << C_sca(k) << endl;
      out_stream << "C_ext " << C_ext(k) << endl;
      out_stream << "C_abs " << C_abs(k) << endl;
      MultiPrint(out_stream.str(), out_file_name, write_output);
      out_stream.str(string());

      // Calculating scattering matrix for 2 scattering angles 0 and pi
      stSM_list[k] = pstScatteringMatrixOA(st_TR_list, params2->lambda(0), static_cast<Real2>(C_sca(k)));

      for (int t = 0; t < raman_params2->N_theta_p; t++) {
        begin = chrono::steady_clock::now();
        Real2 theta_p = raman_params2->theta_p_var(t);
        std::array<long int, 3> new_dims = {N_r, N_theta, 3};
        ArrayXr<Real2> phi_var = ArrayXr<Real2>::LinSpaced(N_phi + 1, 0, 2*PI);

        // Calculate field at excitation wavelength
        Real2 alpha_p = 0; // Defines the orientation of the electric field, in the plane orthogonal to wavevector k.
        params2->inc_par = vshMakeIncidentParams(sIncType::GENERAL, N, theta_p, phi_p, alpha_p);
        // Internal field calculations for the incident field defined by stIncPar
        unique_ptr<stAbcdnm<Real2>> st_abcdnm = rvhGetFieldCoefficients(N, st_TR_list, params2->inc_par);
        unique_ptr<stRes<Real2>> st_res_E = pstMakeStructForField(st_abcdnm, params2);
        ArrayXXc<Real2> c_nm = Map<RowArrayXc<Real2>>(st_res_E->c_nm.transpose().data(), st_res_E->c_nm.size());
        ArrayXXc<Real2> d_nm = Map<RowArrayXc<Real2>>(st_res_E->d_nm.transpose().data(), st_res_E->d_nm.size());
        unique_ptr<stEAllPhi<Real2>> st_E_surf = vshEgenThetaAllPhi(st_res_E->lambda, st_res_E->epsilon2,
            c_nm, d_nm, r_row, theta_row, sBessel::J);
        // The field is calculated for a set of phi, from 0 and 2*pi
        Tensor4c<Real2> E_field_z(N_r, N_theta, N_phi + 1, 3);
        E_field_z.setZero();
        for (int m = 0; m <= N_phi; m++) {
          Real2 phi = phi_var(m);
          unique_ptr<stEforPhi<Real2>> st_E_for_phi = vshEthetaForPhi(st_E_surf, phi);
          long int dims[2] = {st_E_for_phi->E_r.cols(), 3*st_E_for_phi->E_r.rows()};
          ArrayXXc<Real2> E_field_phi(dims[0], dims[1]);
          E_field_phi << st_E_for_phi->E_r.matrix().adjoint(),
              st_E_for_phi->E_t.matrix().adjoint(), st_E_for_phi->E_f.matrix().adjoint();
          E_field_z.chip(m, 2) = TensorCast(E_field_phi).reshape(new_dims);
        }

        alpha_p = PI/2; // Internal field calculations for different polarisation alpha_p
        params2->inc_par = vshMakeIncidentParams(sIncType::GENERAL, N, theta_p, phi_p, alpha_p);
        st_abcdnm = rvhGetFieldCoefficients(N, st_TR_list, params2->inc_par);
        st_res_E = pstMakeStructForField(st_abcdnm, params2);
        c_nm = Map<RowArrayXc<Real2>>(st_res_E->c_nm.transpose().data(), st_res_E->c_nm.size());
        d_nm = Map<RowArrayXc<Real2>>(st_res_E->d_nm.transpose().data(), st_res_E->d_nm.size());
        st_E_surf = vshEgenThetaAllPhi(st_res_E->lambda, st_res_E->epsilon2,
            c_nm, d_nm, r_row, theta_row, sBessel::J);
        // The field is calculated for a set of phi, from 0 and 2*pi
        Tensor4c<Real2> E_field_y(N_r, N_theta, N_phi + 1, 3);
        E_field_y.setZero();
        for (int m = 0; m <= N_phi; m++) {
          Real2 phi = phi_var(m);
          unique_ptr<stEforPhi<Real2>> st_E_for_phi = vshEthetaForPhi(st_E_surf, phi);
          long int dims[2] = {st_E_for_phi->E_r.cols(), 3*st_E_for_phi->E_r.rows()};
          ArrayXXc<Real2> E_field_phi(dims[0], dims[1]);
          E_field_phi << st_E_for_phi->E_r.matrix().adjoint(),
              st_E_for_phi->E_t.matrix().adjoint(), st_E_for_phi->E_f.matrix().adjoint();
          E_field_y.chip(m, 2) = TensorCast(E_field_phi).reshape(new_dims);
        }

        // Calculate field at Raman wavelength
        alpha_p = 0;
        params_rm2->inc_par = vshMakeIncidentParams(sIncType::GENERAL, N, theta_p, phi_p, alpha_p);
        st_abcdnm = rvhGetFieldCoefficients(N, st_TR_list_rm, params_rm2->inc_par);
        st_res_E = pstMakeStructForField(st_abcdnm, params_rm2);
        c_nm = Map<RowArrayXc<Real2>>(st_res_E->c_nm.transpose().data(), st_res_E->c_nm.size());
        d_nm = Map<RowArrayXc<Real2>>(st_res_E->d_nm.transpose().data(), st_res_E->d_nm.size());
        st_E_surf = vshEgenThetaAllPhi(st_res_E->lambda, st_res_E->epsilon2,
            c_nm, d_nm, r_row, theta_row, sBessel::J);
        // The field is calculated for a set of phi, from 0 and 2*pi
        Tensor4c<Real2> E_field_rm_z(N_r, N_theta, N_phi + 1, 3);
        E_field_rm_z.setZero();
        for (int m = 0; m <= N_phi; m++) {
          Real2 phi = phi_var(m);
          unique_ptr<stEforPhi<Real2>> st_E_for_phi = vshEthetaForPhi(st_E_surf, phi);
          long int dims[2] = {st_E_for_phi->E_r.cols(), 3*st_E_for_phi->E_r.rows()};
          ArrayXXc<Real2> E_field_phi(dims[0], dims[1]);
          E_field_phi << st_E_for_phi->E_r.matrix().adjoint(),
              st_E_for_phi->E_t.matrix().adjoint(), st_E_for_phi->E_f.matrix().adjoint();
          E_field_rm_z.chip(m, 2) = TensorCast(E_field_phi).reshape(new_dims);
        }

        alpha_p = PI/2;
        params_rm2->inc_par = vshMakeIncidentParams(sIncType::GENERAL, N, theta_p, phi_p, alpha_p);
        st_abcdnm = rvhGetFieldCoefficients(N, st_TR_list_rm, params_rm2->inc_par);
        st_res_E = pstMakeStructForField(st_abcdnm, params_rm2);
        c_nm = Map<RowArrayXc<Real2>>(st_res_E->c_nm.transpose().data(), st_res_E->c_nm.size());
        d_nm = Map<RowArrayXc<Real2>>(st_res_E->d_nm.transpose().data(), st_res_E->d_nm.size());
        st_E_surf = vshEgenThetaAllPhi(st_res_E->lambda, st_res_E->epsilon2,
            c_nm, d_nm, r_row, theta_row, sBessel::J);
        // The field is calculated for a set of phi, from 0 and 2*pi
        Tensor4c<Real2> E_field_rm_y(N_r, N_theta, N_phi + 1, 3);
        E_field_rm_y.setZero();
        for (int m = 0; m <= N_phi; m++) {
          Real2 phi = phi_var(m);
          unique_ptr<stEforPhi<Real2>> st_E_for_phi = vshEthetaForPhi(st_E_surf, phi);
          long int dims[2] = {st_E_for_phi->E_r.cols(), 3*st_E_for_phi->E_r.rows()};
          ArrayXXc<Real2> E_field_phi(dims[0], dims[1]);
          E_field_phi << st_E_for_phi->E_r.matrix().adjoint(),
              st_E_for_phi->E_t.matrix().adjoint(), st_E_for_phi->E_f.matrix().adjoint();
          E_field_rm_y.chip(m, 2) = TensorCast(E_field_phi).reshape(new_dims);
        }

        // Calculate scattering cross-sections
        std::array<int, 1> dim3 = {3}, dim2 = {2};
        Tensor<Real2, 2> M_tens = 2*PI/N_phi*(E_field_rm_z * TensorConj(E_field_z))
            .sum(dim3).abs().pow(static_cast<Real2>(2.0)).sum(dim2);
        Map<ArrayXXr<Real2>> M_mat = ArrayMap(M_tens);
        ArrayXXr<Real2> F = M_mat * r_mat.pow(2) * sin(theta_mat);
        ArrayXXr<Real2> intergrand = (F(seq(1, last), seq(1, last)) + F(seq(0, last - 1), seq(1, last)) +
            F(seq(1, last), seq(0, last - 1)) + F(seq(0, last - 1), seq(0, last - 1))) / 4 * dt_dr;
        sigma_zz(k, h_ind, t) = (intergrand).sum()/(PI*a*a*c*4/3);

        M_tens = 2*PI/N_phi*(E_field_rm_y * TensorConj(E_field_z))
            .sum(dim3).abs().pow(static_cast<Real2>(2.0)).sum(dim2);
        M_mat = ArrayMap(M_tens);
        F = M_mat * r_mat.pow(2) * sin(theta_mat);
        intergrand = (F(seq(1, last), seq(1, last)) + F(seq(0, last - 1), seq(1, last)) +
            F(seq(1, last), seq(0, last - 1)) + F(seq(0, last - 1), seq(0, last - 1))) / 4 * dt_dr;
        sigma_yz(k, h_ind, t) = (intergrand).sum()/(PI*a*a*c*4/3);

        M_tens = 2*PI/N_phi*(E_field_rm_y * TensorConj(E_field_y))
            .sum(dim3).abs().pow(static_cast<Real2>(2.0)).sum(dim2);
        M_mat = ArrayMap(M_tens);
        F = M_mat * r_mat.pow(2) * sin(theta_mat);
        intergrand = (F(seq(1, last), seq(1, last)) + F(seq(0, last - 1), seq(1, last)) +
            F(seq(1, last), seq(0, last - 1)) + F(seq(0, last - 1), seq(0, last - 1))) / 4 * dt_dr;
        sigma_yy(k, h_ind, t) = (intergrand).sum()/(PI*a*a*c*4/3);

        M_tens = 2*PI/N_phi*(E_field_rm_z * TensorConj(E_field_y))
            .sum(dim3).abs().pow(static_cast<Real2>(2.0)).sum(dim2);
        M_mat = ArrayMap(M_tens);
        F = M_mat * r_mat.pow(2) * sin(theta_mat);
        intergrand = (F(seq(1, last), seq(1, last)) + F(seq(0, last - 1), seq(1, last)) +
            F(seq(1, last), seq(0, last - 1)) + F(seq(0, last - 1), seq(0, last - 1))) / 4 * dt_dr;
        sigma_zy(k, h_ind, t) = (intergrand).sum()/(PI*a*a*c*4/3);

        out_stream.precision(5);
        out_stream << "max diameter = " << max(a, c)*2e-3;
        out_stream.precision(10);
        out_stream << " µm, theta = " << theta_p * 180/PI << " degrees" << endl;
        out_stream.precision(11);
        out_stream << "--- relative raman zz = " << sigma_zz(k, h_ind, t) << endl;
        out_stream << "--- relative raman yz = " << sigma_yz(k, h_ind, t) << endl;
        out_stream << "--- relative raman zy = " << sigma_zy(k, h_ind, t) << endl;
        out_stream << "--- relative raman yy = " << sigma_yy(k, h_ind, t) << endl;

        end = chrono::steady_clock::now();
        elapsed_seconds = end - begin;
        out_stream << elapsed_seconds.count() << endl;

        MultiPrint(out_stream.str(), out_file_name, write_output);
        out_stream.str(string());
      }
    }
  }
}

#endif


#include <vector>
#include <boost/math/tools/polynomial.hpp>
#include <fstream>
#include <immintrin.h>
#include <map>
#include "cpp-btree/btree_map.h"
//---------------------------------------------------------------------------
typedef std::pair<double,double> Coord;
typedef std::pair<double, unsigned> segCoord;
typedef btree::btree_map<double, double> btree_map;
typedef btree::btree_map<double, unsigned> btree_map_segments;
//---------------------------------------------------------------------------
class SplineEvaluation;
//---------------------------------------------------------------------------
/// A linear spline for evaluation
class SplineEvaluation
{
    private:
    static const unsigned countOfBuckets = 2 + 1;
    static const unsigned limitSplineSize = 100000;
    int32_t offset;
    int32_t length;

    void transformSpline();
    double selfInterpolate(unsigned segment, double& x) const;
    unsigned searchLeft(int segment, double& x) const;
    unsigned searchRight(int segment, double& x) const;
    unsigned binarySearch(unsigned lower, unsigned upper, double& x) const;
    void changeIntoArray(long double** moveHere, boost::math::tools::polynomial<long double> poly);
    void convertValues();
    void splitSegmentSpline();
    public:
    static const unsigned polyGrade = 2;
    /// Constructor
    SplineEvaluation();
    /// Deconstructor
    ~SplineEvaluation();
    /// Constructor with the function
    SplineEvaluation(const std::vector<Coord>& function, unsigned desiredSize, const unsigned useSplineMode, const unsigned searchMode, const unsigned testMode);
    /// Constructor with given data
    SplineEvaluation(unsigned size, const double* x, const double* y);

    void save(std::ofstream& file_out);
    void load(std::ifstream& file_in);

    unsigned size() const;
    unsigned findExactSegment(double segment, double& x) const;
    /// Return the approximate value of function(x)
    double evaluate(double& x) const;
    double hornerEvaluate(double& x) const;

    double splittedHornerEvaluate(double& x) const;
    double hornerEvaluateByBuckets(unsigned bucketIndex, double& x) const;

    double chebyshevEvaluate(double& x) const;
    double splittedChebyshevEvaluate(double& x) const;

    double mapEvaluate(double& x) const;
    std::map<double, double> coordinates;

    double btreeEvaluate(double& x) const;
    btree_map* btreeMap;

    double btreeSegmentsEvaluate(double& x) const;
    btree_map_segments* btreeSegments;

    std::vector<Coord> spline;
    /// Spline which has to be fitted
    std::vector<segCoord> segmentSpline;
    /// Fitted polynomial to the spline
    long double* poly;
    double* values;
    unsigned splineSize;

    std::vector<segCoord>* splineBuckets;
    long double **polyBuckets;
    double splittedPositions[countOfBuckets];
    __m128d saveLimits;

    /// Fit a polynomial to spline
    boost::math::tools::polynomial<long double> fitSpline(const std::vector<segCoord>& spline);
};
//---------------------------------------------------------------------------


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_dot"

#include <boost/test/unit_test.hpp>
#include "nanocv/tensor.h"
#include "nanocv/math/dot.hpp"
#include "nanocv/math/abs.hpp"
#include "nanocv/math/epsilon.hpp"
#include "nanocv/tensor/dot.hpp"

namespace test
{
        using namespace ncv;

        template
        <
                typename top,
                typename tvector,
                typename tscalar = typename tvector::Scalar
        >
        tscalar test_dot(top op, const tvector& vec1, const tvector& vec2)
        {
                return op(vec1.data(), vec2.data(), vec1.size());
        }

        void test_dot(size_t size)
        {
                vector_t vec1(size), vec2(size);
                vec1.setRandom();
                vec2.setRandom();

                vec1.array() *= scalar_t(1) / std::cbrt(scalar_t(size));
                vec2.array() *= scalar_t(1) / std::cbrt(scalar_t(size));

                const scalar_t dot    = test_dot(ncv::math::dot<scalar_t>, vec1, vec2);
                const scalar_t dotul2 = test_dot(ncv::math::dot_unroll<scalar_t, 2>, vec1, vec2);
                const scalar_t dotul3 = test_dot(ncv::math::dot_unroll<scalar_t, 3>, vec1, vec2);
                const scalar_t dotul4 = test_dot(ncv::math::dot_unroll<scalar_t, 4>, vec1, vec2);
                const scalar_t dotul5 = test_dot(ncv::math::dot_unroll<scalar_t, 5>, vec1, vec2);
                const scalar_t dotul6 = test_dot(ncv::math::dot_unroll<scalar_t, 6>, vec1, vec2);
                const scalar_t dotul7 = test_dot(ncv::math::dot_unroll<scalar_t, 7>, vec1, vec2);
                const scalar_t dotul8 = test_dot(ncv::math::dot_unroll<scalar_t, 8>, vec1, vec2);
                const scalar_t doteig = test_dot(ncv::tensor::dot<scalar_t>, vec1, vec2);

                BOOST_CHECK_LE(math::abs(dot - dot), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul2), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul3), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul4), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul5), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul6), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul7), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - dotul8), math::epsilon1<scalar_t>());
                BOOST_CHECK_LE(math::abs(dot - doteig), math::epsilon1<scalar_t>());
        }
}

BOOST_AUTO_TEST_CASE(test_dot)
{
        using namespace ncv;

        static const size_t min_size = 1024;
        static const size_t max_size = 1024 * 1024;

        for (size_t size = min_size; size <= max_size; size *= 2)
        {
                test::test_dot(size);
        }
}


#include "ddCRP.h"
#include <boost/random/discrete_distribution.hpp>
#include <vector>
#include <iostream>

ddCRP::ddCRP(const Eigen::MatrixXd &log_decay_values, unsigned int seed)
    : c_(log_decay_values.rows()),
      log_decay_values_(log_decay_values),
      likelihood_(NULL),
      rng_( seed )
{
}

void ddCRP::setLikelihood(const std::shared_ptr<LikelihoodFcn> &l)
{
    likelihood_ = l;
}

void ddCRP::iterate()
{
    std::vector<double> p_link(c_.num_customers(), 0.0);
    for ( std::size_t source = 0; source < c_.num_customers(); ++source)
    {
        c_.unlink(source);
        get_link_likelihoods(source, p_link);
        std::transform(p_link.begin(), p_link.end(), p_link.begin(), [](double p){return std::exp(p); } );
        boost::random::discrete_distribution<std::size_t> d(p_link.begin(), p_link.end());
        c_.link(source, d(rng_));
    }
}

void ddCRP::get_link_likelihoods(std::size_t source, std::vector<double>& p) const
{
    p.resize(c_.num_customers());
    for (std::size_t target = 0; target < p.size(); ++target)
    {
        p[target] = get_link_likelihood(source, target);
    }
}

double ddCRP::get_link_likelihood(std::size_t source, std::size_t target) const
{
    double p = log_decay_values_(source, target);
    if ((source != target) && !std::isinf(p))
    {
        // If this is NOT a self link and the link is possible, we examine if tables are connected
        std::size_t k(0), l(0);
        if (c_.joins_tables(source, target, k, l))
        {
            std::set<std::size_t> table_l = c_.get_table_members(l);
            p -= likelihood_->get_marginal_log_likelihood(table_l);

            std::set<std::size_t> table_k = c_.get_table_members(k);
            p -= likelihood_->get_marginal_log_likelihood(table_k);

            // insert the smaller set to the larger one (less insert calls) and get joint table likelihood.
            if ( table_l.size() >= table_k.size() )
            {
                table_l.insert(table_k.begin(), table_k.end());
                p += likelihood_->get_marginal_log_likelihood(table_l);
            }
            else
            {
                table_k.insert(table_l.begin(), table_l.end());
                p += likelihood_->get_marginal_log_likelihood(table_k);
            }
        }
    }
    return p;
}

void ddCRP::print_tables(std::ostream& os) const
{
    c_.print_tables(os);
}

std::size_t ddCRP::get_table(std::size_t customer) const
{
    return c_.get_table(customer);
}

std::size_t ddCRP::num_tables() const
{
    return c_.num_tables();
}


#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <quadrotor_msgs/TRPYCommand.h>
#include <quadrotor_msgs/QuadData.h>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <algorithm>
#include <stdio.h>

#define USE_JOYSTICK 0

Eigen::Vector3f pos_des,vel_des,acc_des;
Eigen::Vector3f pos,vel;
Eigen::Quaternionf q;
Eigen::Vector3f acc,force_b,g;
float m = 0.084, factor = 2;
float kp_xy = 16, kd_xy = 8, ki_xy = 0.3;
float kp_z = 36, kd_z = 12, ki_z = 6, ki_yaw = 0.2;
ros::Publisher control_pub;
ros::Publisher euler_pub;
float yaw_des;
int mode = -1;
int lock1 = 0, lock2 = 0, lock3 = 0;
bool quit = false;
uint int_count = 0;
double cur_t, prev_t;
float delT;
FILE * fout;
FILE * fout2;
float x_int = 0, y_int = 0, z_int = 0, roll_int = 0, pitch_int = 0, th_int = 0, psi_int = 0, yaw_int = 0, psi_diff;
double time_st;
int iswrite = 0;

#define USE_INT 0

static const double     _PI= 3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348;
static const double _TWO_PI= 6.2831853071795864769252867665590057683943387987502116419498891846156328125724179972560696;

// Floating-point modulo
// The result (the remainder) has same sign as the divisor.
// Similar to matlab's mod(); Not similar to fmod() -   Mod(-3,4)= 1   fmod(-3,4)= -3
template<typename T>
T Mod(T x, T y)
{
//    static_assert(!std::numeric_limits<T>::is_exact , "Mod: floating-point type expected");

    if (0. == y)
        return x;

    double mm= x - y * floor(x/y);

    // handle boundary cases resulted from floating-point cut off:

    if (y > 0)              // modulo range: [0..y)
    {
        if (mm>=y)           // Mod(-1e-16             , 360.    ): m= 360.
            return 0;

        if (mm<0 )
        {
            if (y+mm == y)
                return 0  ; // just in case...
            else
                return y+mm; // Mod(106.81415022205296 , _TWO_PI ): m= -1.421e-14
        }
    }
    else                    // modulo range: (y..0]
    {
        if (mm<=y)           // Mod(1e-16              , -360.   ): m= -360.
            return 0;

        if (mm>0 )
        {
            if (y+mm == y)
                return 0  ; // just in case...
            else
                return y+mm; // Mod(-106.81415022205296, -_TWO_PI): m= 1.421e-14
        }
    }

    return mm;
}

void killprogram(const std_msgs::Int8::ConstPtr & msg)
{
  if(msg->data)
    quit = true;
}

void trajCallback(const quadrotor_msgs::QuadData::ConstPtr & msg)
{
    mode = msg->mode;
    if(msg->write == 1){
        iswrite = 1;
    }
    if(mode == 2){
        pos_des[0] = msg->x;
        pos_des[1] = msg->y;
        pos_des[2] = msg->z;
        vel_des[0] = msg->vx;
        vel_des[1] = msg->vy;
        vel_des[2] = msg->vz;
        acc_des[0] = msg->acc_x;
        acc_des[1] = msg->acc_y;
        acc_des[2] = msg->acc_z;
    }
    if(mode == 0){
        pos_des[2] = msg->z;
        vel_des[2] = msg->vz;
        acc_des[2] = msg->acc_z;
    }
}

void stateCallback(const nav_msgs::Odometry::ConstPtr& state)
{
    pos[0] = state->pose.pose.position.x;
    pos[1] = state->pose.pose.position.y;
    pos[2] = state->pose.pose.position.z;
    vel[0] = state->twist.twist.linear.x;
    vel[1] = state->twist.twist.linear.y;
    vel[2] = state->twist.twist.linear.z;

    q.x() = state->pose.pose.orientation.x;
    q.y() = state->pose.pose.orientation.y;
    q.z() = state->pose.pose.orientation.z;
    q.w() = state->pose.pose.orientation.w;

    Eigen::Matrix3f Rbw(q);
    Eigen::Vector3f euler = Rbw.eulerAngles(2,1,0);
    float roll_des = 0;
    float pitch_des = 0;

    if(mode == 0){
    lock2 = 0;
    if(lock1 == 0){
        lock1 = 1;
        pos_des[0] = pos[0];
        vel_des[0] = 0;
        acc_des[0] = 0;
        pos_des[1] = pos[1];
        vel_des[1] = 0;
        acc_des[1] = 0;
        yaw_des = euler(0);
    }
    }
    if(mode == 1){
        printf("I'm here lock2=%d\n", lock2);
        lock1 = 0;
        if(lock2 == 0){
            iswrite = 0;
            lock2 = 1;
            vel_des[0] = 0;
            acc_des[0] = 0;
            vel_des[1] = 0;
            acc_des[1] = 0;
            vel_des[2] = 0;
            acc_des[2] = 0;
        }
    }
    if(mode == 2){
        lock1 = 0;
        lock2 = 0;
        lock3 = 0;
    }
    if(mode == 3){
        if(lock3 == 0){
            iswrite = 0;
            lock3 = 1;
            pos_des[0] = pos[0];
            vel_des[0] = 0;
            acc_des[0] = 0;
            pos_des[1] = pos[1];
            vel_des[1] = 0;
            acc_des[1] = 0;
            pos_des[2] = pos[2];
            vel_des[2] = 0;
            acc_des[2] = 0;
        }
        pos_des[2] = std::max(pos_des[2] - 0.005,-0.15);
    }

    if(USE_INT && mode > 0){
        cur_t = ros::Time::now().toSec();
        if(int_count == 0){
            pos_des[0] = pos[0];
            pos_des[1] = pos[1];
            delT = 0;
        }
        else{
            delT = (float)(cur_t - prev_t);
        }
        x_int = delT*(cos(euler[0])*(pos_des[0]-pos[0]) + sin(euler[0])*(pos_des[1]-pos[1]));
        y_int = delT*(-sin(euler[0])*(pos_des[0]-pos[0]) + cos(euler[0])*(pos_des[1]-pos[1]));
        z_int = delT*(pos_des[2]-pos[2]);

        roll_int = roll_int - ki_xy*y_int;
        pitch_int = pitch_int + ki_xy*x_int;
        th_int = th_int + ki_z*z_int;
        psi_diff = yaw_des - euler(0);
        psi_diff = Mod(psi_diff + _PI, _TWO_PI) - _PI;
        //ROS_INFO("yaw angle: des %f  cur %f diff: %f",yaw_des, euler(0),psi_diff);
        psi_int = delT * psi_diff;
        yaw_int = yaw_int + ki_yaw * psi_int;
        yaw_int = Mod(yaw_int + _PI, _TWO_PI) - _PI;
        prev_t = cur_t;
        //ROS_INFO("int_terms: %f %f %f %f %f %f yaw_diff: %f %f",x_int, y_int, z_int, th_int, roll_int, pitch_int, psi_diff,yaw_int);
        int_count++;
    }
    else{
        th_int = 0;
        roll_int = 0;
        pitch_int = 0;
        yaw_int = 0;
    }

    printf("pos: %f %f %f\n", pos[0], pos[1], pos[2]);
    printf("pos_des: %f %f %f\n", pos_des[0], pos_des[1], pos_des[2]);
    printf("vel_des: %f %f %f\n", vel_des[0], vel_des[1], vel_des[2]);
    printf("acc_des: %f %f %f\n", acc_des[0], acc_des[1], acc_des[2]);

    acc[0] = kp_xy*(pos_des[0]-pos[0])+kd_xy*(vel_des[0]-vel[0])+acc_des[0];
    acc[1] = kp_xy*(pos_des[1]-pos[1])+kd_xy*(vel_des[1]-vel[1])+acc_des[1];
    acc[2] = kp_z*(pos_des[2]-pos[2])+kd_z*(vel_des[2]-vel[2])+acc_des[2] + th_int;

    roll_des = (acc[0]*sin(euler[0])-acc[1]*cos(euler[0]))/9.81 + roll_int;
    pitch_des = (acc[0]*cos(euler[0])+acc[1]*sin(euler[0]))/9.81 + pitch_int;

    quadrotor_msgs::TRPYCommand command;

    if(quit || (pos_des[2] < -0.148)){
        command.thrust = 0;
        command.roll = 0;
        command.pitch = 0;
        command.yaw = 0;
    }
    else if (mode == -1){
        command.thrust = 0.01;
        command.roll = 0;
        command.pitch = 0;
        command.yaw = 0;
    }
    else{
        if(iswrite == 1){
            fprintf(fout,"Time: %f State: %f %f %f %f %f %f %f %f %f\n",ros::Time::now().toSec() - time_st, pos[0],pos[1],pos[2],vel[0],vel[1],vel[2],euler(0),euler(1),euler(2));
        }
        command.header.stamp = ros::Time::now();
        command.thrust = std::min(std::max(((acc[2]+9.81)*m),0.01),1.3);
        command.roll = roll_des;
        command.pitch = pitch_des;
        command.yaw = yaw_des + yaw_int;
        command.aux.current_yaw = 0;
        command.aux.enable_motors = true;
        command.aux.use_external_yaw = true;
    }

    control_pub.publish(command);

    geometry_msgs::Vector3Stamped Euler;
    Euler.header.stamp = state->header.stamp;
    Euler.vector.x = euler[2];
    Euler.vector.y = euler[1];
    Euler.vector.z = euler[0];
    euler_pub.publish(Euler);
}

int main(int argc, char **argv)
{
  g<<0,
    0,
    9.81;
    std::string fname, fname2;
    ros::init(argc,argv,"pdcontrol");
    ros::NodeHandle nh("~");
    nh.param<std::string>("output/pos", fname, "out.txt");
    double c_time = ros::Time::now().toSec();
    int time_m = (int)(c_time/10);
    std::stringstream ss;
    ss << time_m;
    fname += ss.str();
    fout = fopen(fname.c_str(),"w");
    fout2 = fopen(fname2.c_str(),"w");
    ros::Subscriber trajectory_sub = nh.subscribe("trajectory",10,trajCallback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber state_sub = nh.subscribe("odom",10,stateCallback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber killsw = nh.subscribe("killsignal",10,killprogram, ros::TransportHints().tcpNoDelay());
    control_pub = nh.advertise<quadrotor_msgs::TRPYCommand>("trpy_cmd",10);
    euler_pub = nh.advertise<geometry_msgs::Vector3Stamped>("euler",10);
    time_st = ros::Time::now().toSec();
    ros::spin();
    fclose(fout);
    fclose(fout2);
   return 0;
}


/*
 * convnn.cpp
 *
 */
#include <string>

#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/functions.h"
#include "layers/contlayer.h"
#include "layers/convlayer.h"
#include "layers/fclayer.h"
#include "layers/polllayer.h"
#include "layers/smaxlayer.h"

#include "cnn.h"

using namespace std;
using namespace yann;

////////////////////////////////////////////////////////////////////////////////////////////////
//
// ConvolutionalNetwork implementation
//
yann::ConvolutionalNetwork::ConvPollParams::ConvPollParams() :
    _output_frames_num(0),
    _conv_filter_size(0),
    _polling_mode(PollingLayer::PollMode_Avg),
    _polling_filter_size(0)
{
}

void yann::ConvolutionalNetwork::append(unique_ptr<SequentialLayer> & container,
                   const MatrixSize & input_rows,
                   const MatrixSize & input_cols,
                   const MatrixSize & input_frames_num,
                   const ConvPollParams & params)
{
  YANN_CHECK(container);
  YANN_CHECK_GT(input_rows, 0);
  YANN_CHECK_GT(input_cols, 0);
  YANN_CHECK_GT(input_frames_num, 0);
  YANN_CHECK(input_frames_num == 1 || !params._mappings.empty());
  YANN_CHECK_GT(params._output_frames_num, 0 || !params._mappings.empty());
  YANN_CHECK_GT(params._conv_filter_size, 0);
  YANN_CHECK_GT(params._polling_filter_size, 0);

  // ConvolutionalLayer
  if(input_frames_num == 1) {
    auto conv_bcast_layer = ConvolutionalLayer::create_conv_bcast_layer(
        params._output_frames_num,   // N output frames
        input_rows,
        input_cols,
        params._conv_filter_size,
        params._conv_activation_funtion
    );
    YANN_CHECK(conv_bcast_layer);
    container->append_layer(std::move(conv_bcast_layer));
  } else {
    YANN_CHECK(!params._mappings.empty());
    YANN_CHECK(params._output_frames_num == 0 || params._output_frames_num == params._mappings.size());

    auto conv_mapping_layer = make_unique<MappingLayer>(input_frames_num);
    YANN_CHECK(conv_mapping_layer);
    for(auto & layer_mappings : params._mappings) {
      auto conv_layer = make_unique<ConvolutionalLayer>(
          input_rows,
          input_cols,
          params._conv_filter_size);
      YANN_CHECK(conv_layer);

      if(params._conv_activation_funtion) {
        conv_layer->set_activation_function(params._conv_activation_funtion);
      }
      conv_mapping_layer->append_layer(std::move(conv_layer), layer_mappings);
    }
    container->append_layer(std::move(conv_mapping_layer));
  }

  // PollingLayer
  MatrixSize poll_input_rows = ConvolutionalLayer::get_conv_output_rows(input_rows, params._conv_filter_size);
  MatrixSize poll_input_cols = ConvolutionalLayer::get_conv_output_cols(input_cols, params._conv_filter_size);
  auto poll_layer = PollingLayer::create_poll_parallel_layer(
     params._output_frames_num,
     poll_input_rows,
     poll_input_cols,
     params._polling_filter_size,
     params._polling_mode,
     params._polling_activation_funtion);
  YANN_CHECK(poll_layer);
  container->append_layer(std::move(poll_layer));
}

unique_ptr<SequentialLayer> yann::ConvolutionalNetwork::create(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const ConvPollParams & params,
    const std::unique_ptr<ActivationFunction> & fc_activation_funtion,
    const MatrixSize & output_size)
{
  YANN_CHECK_GT(output_size, 0);

  auto container = make_unique<SequentialLayer>();
  YANN_CHECK(container);

  // append one layer: we have 1 input frame
  append(container, input_rows, input_cols, 1, params);

  // FullyConnectedLayer
  auto fc_layer = make_unique<FullyConnectedLayer>(
      container->get_output_size(),
      output_size);
  YANN_CHECK(fc_layer);
  if(fc_activation_funtion) {
    fc_layer->set_activation_function(fc_activation_funtion);
  }
  container->append_layer(std::move(fc_layer));

  // done
  return container;
}

std::unique_ptr<SequentialLayer> yann::ConvolutionalNetwork::create(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    const ConvPollParams & params1,
    const ConvPollParams & params2,
    const std::unique_ptr<ActivationFunction> & fc_activation_funtion,
    const MatrixSize & output_size)
{
  YANN_CHECK_GT(output_size, 0);

  auto container = make_unique<SequentialLayer>();
  YANN_CHECK(container);

  // append one layer: we have 1 input frame
  append(container, input_rows, input_cols, 1, params1);

  // append second layer: the output frames from first layer are our inputs
  MatrixSize poll1_input_rows = ConvolutionalLayer::get_conv_output_rows(input_rows, params1._conv_filter_size);
  MatrixSize poll1_input_cols = ConvolutionalLayer::get_conv_output_cols(input_cols, params1._conv_filter_size);
  MatrixSize conv2_input_rows = PollingLayer::get_output_rows(poll1_input_rows, params1._polling_filter_size);
  MatrixSize conv2_input_cols = PollingLayer::get_output_cols(poll1_input_cols, params1._polling_filter_size);
  append(container, conv2_input_rows, conv2_input_cols, params1._output_frames_num, params2);

  // FullyConnectedLayer
  auto fc_layer = make_unique<FullyConnectedLayer>(
      container->get_output_size(),
      output_size);
  YANN_CHECK(fc_layer);
  if(fc_activation_funtion) {
    fc_layer->set_activation_function(fc_activation_funtion);
  }
  container->append_layer(std::move(fc_layer));

  // done
  return container;
}

std::unique_ptr<SequentialLayer> yann::ConvolutionalNetwork::create_lenet1(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    PollingLayer::Mode polling_mode,
    const MatrixSize & fc_size,
    const MatrixSize & output_size,
    const std::unique_ptr<ActivationFunction> & conv_activation_funtion,
    const std::unique_ptr<ActivationFunction> & poll_activation_funtion,
    const std::unique_ptr<ActivationFunction> & fc_activation_funtion)
{
  // see http://yann.lecun.com/exdb/publis/pdf/lecun-90c.pdf
  ConvPollParams params1;
  params1._output_frames_num = 4;
  params1._conv_filter_size  = 5;
  if(conv_activation_funtion) {
    params1._conv_activation_funtion = conv_activation_funtion->copy();
  }
  params1._polling_mode = polling_mode;
  params1._polling_filter_size = 2;
  if(poll_activation_funtion) {
    params1._polling_activation_funtion = poll_activation_funtion->copy();
  }

  ConvPollParams params2;
  params2._output_frames_num = 12;
  params2._conv_filter_size  = 5;
  if(conv_activation_funtion) {
    params2._conv_activation_funtion = conv_activation_funtion->copy();
  }
  params2._polling_mode = polling_mode;
  params2._polling_filter_size = 2;
  if(poll_activation_funtion) {
    params2._polling_activation_funtion = poll_activation_funtion->copy();
  }
  params2._mappings.push_back({ 0 });
  params2._mappings.push_back({ 0, 1 });
  params2._mappings.push_back({ 0, 1 });
  params2._mappings.push_back({ 1 });
  params2._mappings.push_back({ 0, 1 });
  params2._mappings.push_back({ 0, 1 });
  params2._mappings.push_back({ 2 });
  params2._mappings.push_back({ 2, 3 });
  params2._mappings.push_back({ 2, 3 });
  params2._mappings.push_back({ 3 });
  params2._mappings.push_back({ 2, 3 });
  params2._mappings.push_back({ 2, 3 });

  auto container = create(
      input_rows, input_cols, params1, params2,
      fc_activation_funtion, fc_size > 0 ? fc_size : output_size);
  YANN_CHECK(container);

  // add one more FC layer if needed
  if(fc_size > 0) {
    auto fc_layer = make_unique<FullyConnectedLayer>(fc_size, output_size);
    YANN_CHECK(fc_layer);
    if(fc_activation_funtion) {
      fc_layer->set_activation_function(fc_activation_funtion);
    }
    container->append_layer(std::move(fc_layer));
  }

  // done
  return container;
}

std::unique_ptr<SequentialLayer> yann::ConvolutionalNetwork::create_boosted_lenet1(
    const MatrixSize & paths_num,
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    PollingLayer::Mode polling_mode,
    const MatrixSize & fc_size,
    const MatrixSize & output_size,
    const std::unique_ptr<ActivationFunction> & conv_activation_funtion,
    const std::unique_ptr<ActivationFunction> & poll_activation_funtion,
    const std::unique_ptr<ActivationFunction> & fc_activation_funtion)
{
  // create N paths
  auto bcast_layer = make_unique<BroadcastLayer>();
  YANN_CHECK(bcast_layer);
  for(auto ii = paths_num; ii > 0; --ii) {
    auto path = create_lenet1(
        input_rows, input_cols, polling_mode,
        0, fc_size, // don't create fc layer, we will add one for the merge
        conv_activation_funtion, poll_activation_funtion,
        fc_activation_funtion);
    YANN_CHECK(path);
    bcast_layer->append_layer(std::move(path));
  }

  // merge them together
  auto fc_layer = make_unique<FullyConnectedLayer>(paths_num * fc_size, output_size);
  YANN_CHECK(fc_layer);
  fc_layer->set_activation_function(fc_activation_funtion);

  // and finally create container
  auto container = make_unique<SequentialLayer>();
  YANN_CHECK(container);
  container->append_layer(std::move(bcast_layer));
  container->append_layer(std::move(fc_layer));

  // done
  return container;
}

std::unique_ptr<SequentialLayer> yann::ConvolutionalNetwork::create_lenet5(
    const MatrixSize & input_rows,
    const MatrixSize & input_cols,
    PollingLayer::Mode polling_mode,
    const MatrixSize & fc1_size,
    const MatrixSize & fc2_size,
    const MatrixSize & output_size,
    const std::unique_ptr<ActivationFunction> & conv_activation_funtion,
    const std::unique_ptr<ActivationFunction> & poll_activation_funtion,
    const std::unique_ptr<ActivationFunction> & fc_activation_funtion)
{
  // see http://vision.stanford.edu/cs598_spring07/papers/Lecun98.pdf
  ConvPollParams params1;
  params1._output_frames_num = 6;
  params1._conv_filter_size  = 5;
  if(conv_activation_funtion) {
    params1._conv_activation_funtion = conv_activation_funtion->copy();
  }
  params1._polling_mode = polling_mode;
  params1._polling_filter_size = 2;
  if(poll_activation_funtion) {
    params1._polling_activation_funtion = poll_activation_funtion->copy();
  }

  ConvPollParams params2;
  params2._output_frames_num = 16;
  params2._conv_filter_size  = 5;
  if(conv_activation_funtion) {
    params2._conv_activation_funtion = conv_activation_funtion->copy();
  }
  params2._polling_mode = polling_mode;
  params2._polling_filter_size = 2;
  if(poll_activation_funtion) {
    params2._polling_activation_funtion = poll_activation_funtion->copy();
  }
  params2._mappings.push_back({ 0, 1, 2 }); // 0
  params2._mappings.push_back({ 1, 2, 3 });
  params2._mappings.push_back({ 2, 3, 4 });
  params2._mappings.push_back({ 3, 4, 5 });
  params2._mappings.push_back({ 0, 4, 5 }); // 4
  params2._mappings.push_back({ 0, 1, 5 });
  params2._mappings.push_back({ 0, 1, 2, 3 });
  params2._mappings.push_back({ 1, 2, 3, 4 });
  params2._mappings.push_back({ 2, 3, 4, 5 }); // 8
  params2._mappings.push_back({ 0, 3, 4, 5 });
  params2._mappings.push_back({ 0, 1, 4, 5 });
  params2._mappings.push_back({ 0, 1, 2, 5 });
  params2._mappings.push_back({ 0, 1, 3, 4 }); // 12
  params2._mappings.push_back({ 1, 2, 4, 5 });
  params2._mappings.push_back({ 0, 2, 3, 5 });
  params2._mappings.push_back({ 0, 1, 2, 3, 4, 5 });

  auto container = create(input_rows, input_cols, params1, params2, fc_activation_funtion, fc1_size);
  YANN_CHECK(container);

  // add one more FC layer
  auto fc_layer1 = make_unique<FullyConnectedLayer>(fc1_size, fc2_size);
  YANN_CHECK(fc_layer1);
  if(fc_activation_funtion) {
    fc_layer1->set_activation_function(fc_activation_funtion);
  }
  container->append_layer(std::move(fc_layer1));

  // and one more FC layer
  auto fc_layer2 = make_unique<FullyConnectedLayer>(fc2_size, output_size);
  YANN_CHECK(fc_layer2);
  if(fc_activation_funtion) {
    fc_layer2->set_activation_function(fc_activation_funtion);
  }
  container->append_layer(std::move(fc_layer2));

  // done
  return container;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/replace.hpp>
#include <boost/compute/algorithm/replace_copy.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(replace_int)
{
    bc::vector<int> vector(5, context);
    bc::iota(vector.begin(), vector.end(), 0, queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (0, 1, 2, 3, 4));

    bc::replace(vector.begin(), vector.end(), 2, 6, queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (0, 1, 6, 3, 4));
}

BOOST_AUTO_TEST_CASE(replace_copy_int)
{
    bc::vector<int> a(5, context);
    bc::iota(a.begin(), a.end(), 0, queue);
    CHECK_RANGE_EQUAL(int, 5, a, (0, 1, 2, 3, 4));

    bc::vector<int> b(5, context);
    bc::vector<int>::iterator iter =
        bc::replace_copy(a.begin(), a.end(), b.begin(), 3, 9, queue);
    BOOST_CHECK(iter == b.end());
    CHECK_RANGE_EQUAL(int, 5, b, (0, 1, 2, 9, 4));

    // ensure 'a' was not modified
    CHECK_RANGE_EQUAL(int, 5, a, (0, 1, 2, 3, 4));
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * @Description: ceres parameter block for LIO extended pose
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */

#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_PARAM_PRVAG_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_PARAM_PRVAG_HPP_

#include <ceres/ceres.h>

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

namespace sliding_window {

class ParamPRVAG : public ceres::LocalParameterization {
public:
    static const int INDEX_P = 0;
	static const int INDEX_R = 3;
	static const int INDEX_V = 6;
	static const int INDEX_A = 9;
	static const int INDEX_G = 12;

    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const {
        Eigen::Map<const Eigen::Vector3d> pos(x + INDEX_P);
        Eigen::Map<const Eigen::Vector3d> ori(x + INDEX_R);
        Eigen::Map<const Eigen::Vector3d> vel(x + INDEX_V);
        Eigen::Map<const Eigen::Vector3d> b_a(x + INDEX_A);
        Eigen::Map<const Eigen::Vector3d> b_g(x + INDEX_G);

        Eigen::Map<const Eigen::Vector3d> d_pos(delta + INDEX_P);
        Eigen::Map<const Eigen::Vector3d> d_ori(delta + INDEX_R);
        Eigen::Map<const Eigen::Vector3d> d_vel(delta + INDEX_V);
        Eigen::Map<const Eigen::Vector3d> d_b_a(delta + INDEX_A);
        Eigen::Map<const Eigen::Vector3d> d_b_g(delta + INDEX_G);

        Eigen::Map<Eigen::Vector3d> pos_plus(x_plus_delta + INDEX_P);
        Eigen::Map<Eigen::Vector3d> ori_plus(x_plus_delta + INDEX_R);
        Eigen::Map<Eigen::Vector3d> vel_plus(x_plus_delta + INDEX_V);
        Eigen::Map<Eigen::Vector3d> b_a_plus(x_plus_delta + INDEX_A);
        Eigen::Map<Eigen::Vector3d> b_g_plus(x_plus_delta + INDEX_G);

        pos_plus = pos + d_pos;
        //
        // TODO: evaluate performance penalty of applying exp-exp-log transform for each update
        //
        ori_plus = (Sophus::SO3d::exp(ori) * Sophus::SO3d::exp(d_ori)).log();
        vel_plus = vel + d_vel;
        b_a_plus = b_a + d_b_a;
        b_g_plus = b_g + b_g_plus;

        return true;
    }

    virtual bool ComputeJacobian(const double *x, double *jacobian) const {
        Eigen::Map<Eigen::Matrix<double, 15, 15, Eigen::RowMajor>> J(jacobian);
        J.setIdentity();

        return true;
    }

    virtual int GlobalSize() const { return 15; };

    virtual int LocalSize() const { return 15; };
};

} // namespace sliding_window

#endif //LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_PARAM_PRVAG_HPP_


#include "stdafx.h"
#include "NRSolver.h"
#include "PrimitiveNetwork.h"
#include "Exceptions.h"
#include <Eigen/Sparse>
#include <Eigen/SparseLU>

using Eigen::Triplet;
using Eigen::SparseLU;
using Eigen::SparseQR;
using Eigen::SparseMatrix;

using namespace std;
using namespace PowerSolutions::ObjectModel;

namespace PowerSolutions
{
	namespace PowerFlow
	{

		NRSolver::NRSolver()
		{ }

		NRSolver::~NRSolver()
		{ }

		void NRSolver::BeforeIterations()
		{
			assert(Block1EquationCount() == PQNodeCount + PVNodeCount);
			assert(EquationCount() == PQNodeCount * 2 + PVNodeCount);
			//PQ节点：[dP dQ] = J * [V theta]
			//PV节点：[dP] = J * [theta]
			//       PQ/PV    |   PQ
			//Δy = [dP ... dP | dQ ... dQ] = ConstraintPower - CurrentPower
			//Δx = [theta ... theta | V ... V]
			// 分界线/起始索引：
			// H[0,0], N[NodeCount - 1, 0]
			//初始化矩阵维数
			//注意向量需要手动清零
			ConstraintPowerInjection.resize(EquationCount());
			CurrentAnswer.resize(EquationCount());
			CorrectionAnswer.resize(EquationCount());
			PowerInjectionDeviation.resize(EquationCount());
			//估计雅可比矩阵非零元的数量
			//按照最不理想的情况来考虑雅可比矩阵的空间
			JocobianReservedValuesCount = 0;
			for (int n = 0; n < Block1EquationCount(); n++)
			{
				auto& node = PNetwork->Nodes(n);
				JocobianReservedValuesCount += min(node.Degree() * 2, EquationCount());
				if (node.Type() == NodeType::PQNode)
				{
					JocobianReservedValuesCount += min(node.Degree() * 2, EquationCount());
				}
			}
			Jocobian.resize(EquationCount(), EquationCount());

			//生成目标注入功率向量 y，以及迭代初值向量。
			for (auto& node : PNetwork->Nodes())
			{
				if (node->Type() != NodeType::SlackNode)
				{
					int subIndex = Block1EquationCount() + node->SubIndex();
					ConstraintPowerInjection(node->Index()) = node->ActivePowerInjection();
					CurrentAnswer(node->Index()) = arg(node->Bus()->InitialVoltage());
					if (node->Type() == NodeType::PQNode)
					{
						ConstraintPowerInjection(subIndex) = node->ReactivePowerInjection();
						CurrentAnswer(subIndex) = abs(node->Bus()->InitialVoltage());
					}
				}
			}
			_PS_TRACE("目标函数值 Y ==========");
			_PS_TRACE(ConstraintPowerInjection);
		}

		pair<const PrimitiveNetwork::NodeInfo*, double> NRSolver::EvalDeviation()
		{
			EvalPowerInjection();
			//注意，最小系数不一定是绝对值最小的系数
			int row;
			double dev;
			dev = PowerInjectionDeviation.cwiseAbs().maxCoeff(&row);
			if (row < Block1EquationCount())
				return make_pair(&PNetwork->Nodes(row), dev);
			else
				return make_pair(&PNetwork->PQNodes(row - Block1EquationCount()), dev);
		}

		bool NRSolver::OnIteration()
		{
			GenerateJacobian();
			if (!GenerateNextAnswer()) return false;
			return true;
		}

		void NRSolver::AfterIterations()
		{
			for (int i = 0; i < NodeCount; i++)
				UpdateNodeStatus(i);
		}

		inline double NRSolver::NodeVoltage(int NodeIndex)
		{
			auto& node = PNetwork->Nodes(NodeIndex);
			if (node.Type() == NodeType::PQNode)
				return CurrentAnswer(Block1EquationCount() + node.SubIndex());
			else
				return node.Voltage();
		}

		inline double NRSolver::NodeAngle(int NodeIndex)
		{
			assert(NodeIndex < NodeCount);
			//注意此处区分平衡节点。
			return NodeIndex < NodeCount - 1 ? CurrentAnswer(NodeIndex) : 0;
		}

		void NRSolver::EvalPowerInjection()
		{
			//TODO 计入导纳矩阵可能的不对称性
			//计算各节点的实际注入功率，以及功率偏差 PowerInjectionDeviation
			PowerInjectionDeviation = ConstraintPowerInjection;
			for (auto& node : PSolution->NodeStatus()) node.ClearPowerInjection();
			//遍历所有节点，包括平衡节点
			//此处使用 for 而非 for-each 是为了与数学表达保持一致
			auto &Admittance = PNetwork->Admittance;
			for (int m = 0; m < NodeCount; m++)
			{
				auto &statusM = PSolution->NodeStatus(m);
				auto Um = NodeVoltage(m);
				auto thetaM = NodeAngle(m);
				int subM = Block1EquationCount() + statusM.SubIndex();
				//计算导纳矩阵中非对角元素对应的功率。
				for (int n = m + 1; n < NodeCount; n++)
				{
					// 导纳矩阵是上三角矩阵，row < col
					// TODO 取消对导纳矩阵对称的假定（移相变压器）。
					complexd Y = Admittance.coeff(m, n);
					complexd Y1 = Admittance.coeff(n, m);
					//if (abs(Y) < 1e-10) continue;
					auto UmUn = Um * NodeVoltage(n);
					auto thetaMn = thetaM - NodeAngle(n);
					auto sinMn = sin(thetaMn);
					auto cosMn = cos(thetaMn);
					auto &statusN = PSolution->NodeStatus(n);
					//累计上一次迭代结果对应的注入功率
					statusM.AddPowerInjections(UmUn * (Y.real() * cosMn + Y.imag() * sinMn),
						UmUn * (Y.real() * sinMn - Y.imag() * cosMn));
					statusN.AddPowerInjections(UmUn * (Y1.real() * cosMn - Y1.imag() * sinMn),
						UmUn * (-Y1.real() * sinMn - Y1.imag() * cosMn));
				}
				//计算导纳矩阵中对角元素对应的功率。
				auto UmSqr = Um * Um;
				auto Y = Admittance.coeff(m, m);
				statusM.AddPowerInjections(UmSqr * Y.real(), -UmSqr * Y.imag());
				//生成功率偏差向量 Δy'
				if (statusM.Type() != NodeType::SlackNode)
				{
					PowerInjectionDeviation(m) -= statusM.ActivePowerInjection();
					if (statusM.Type() == NodeType::PQNode)
						PowerInjectionDeviation(subM) -= statusM.ReactivePowerInjection();
				}
			}
			_PS_TRACE("平衡节点 ======");
			_PS_TRACE("有功注入：" << PSolution->NodeStatus(NodeCount - 1).ActivePowerInjection());
			_PS_TRACE("无功注入：" << PSolution->NodeStatus(NodeCount - 1).ReactivePowerInjection());
			_PS_TRACE("偏差 deltaY ==========");
			_PS_TRACE(PowerInjectionDeviation);
		}

		void NRSolver::GenerateJacobian()
		{
			//PQ节点：[dP dQ] = J * [V theta]
			//PV节点：[dP] = J * [theta]
			//       PQ/PV        PQ
			//Δy = [dP ... dP dQ ... dQ]
			//Δx = [theta ... theta V ... V]
			//    / H | N \
			//J = | --+-- |
			//    \ M | L /
			// 起始索引：（注意排除平衡节点）
			// H[0,0], N[NodeCount - 1, 0]
			// M[NodeCount - 1, 0], L[..., ...]
			// 对非对角元有
			// H =  L
			// N = -M
			auto& Admittance = PNetwork->Admittance;
			// 雅可比矩阵非零元。
			vector<Triplet<double>> values;
			values.reserve(JocobianReservedValuesCount);
			// 建议使用 Triplet 构造矩阵，可以取得不错的性能，而且生成的矩阵是压缩过的。
			for (int outer = 0; outer < Admittance.outerSize(); outer++)
				for (SparseMatrix<complexd>::InnerIterator it(Admittance, outer); it; ++it)
				{
					auto m = it.row();
					auto n = it.col();
					// 注意排除平衡节点
					if (m == NodeCount - 1 || n == NodeCount - 1)
						continue;
					auto& nodeM = PSolution->NodeStatus(m);
					auto Um = NodeVoltage(m);
					auto thetaM = NodeAngle(m);
					auto subM = Block1EquationCount() + nodeM.SubIndex();
					//计算非对角元素
					if (n > m)
					{
						complexd Ymn = it.value();
						complexd Ynm = it.value();
						double UmUn = Um * NodeVoltage(n);
						double thetaMn = thetaM - NodeAngle(n);
						double sinMn = sin(thetaMn);
						double cosMn = cos(thetaMn);
						//H(m,n)
						double H = -UmUn * (Ymn.real() * sinMn - Ymn.imag() * cosMn);
						values.emplace_back(m, n, H);
						//H(n,m)
						double Hp = UmUn * (Ynm.real() * sinMn + Ynm.imag() * cosMn);
						values.emplace_back(n, m, Hp);
						//i.e. H(n,m) = -UmUn * (-Y.real() * sinMn - Y.imag() * cosMn)
						//N
						double N = -UmUn * (Ymn.real() * cosMn + Ymn.imag() * sinMn);
						double Np = -UmUn * (Ynm.real() * cosMn - Ynm.imag() * sinMn);
						//cout << "::" << m << "," << n << " = " << Jocobian.insert(1, 0) << endl;
						auto& nodeN = PSolution->NodeStatus(n);
						int subN = Block1EquationCount() + nodeN.SubIndex();
						if (nodeM.Type() == NodeType::PQNode)
						{
							if (nodeN.Type() == NodeType::PQNode)
							{
								//PQ-PQ，子阵具有对称性
								// N
								values.emplace_back(m, subN, N);
								values.emplace_back(n, subM, Np);
								// M = -N
								values.emplace_back(subM, n, -N);
								values.emplace_back(subN, m, -Np);
								// L = H
								values.emplace_back(subM, subN, H);
								values.emplace_back(subN, subM, Hp);
							}
							else {
								//PQ-PV（m,n）
								// M = -N
								values.emplace_back(subM, n, -N);
								//PV-PQ（n,m）
								// N
								values.emplace_back(n, subM, Np);
							}
						}
						else {
							if (nodeN.Type() == NodeType::PQNode)
							{
								//PV-PQ（m,n）
								// N
								values.emplace_back(m, subN, N);
								//PQ-PV（n,m）
								// M
								values.emplace_back(subN, m, -Np);
							}
						}
					}
					else if (n == m)
					{
						complexd Y = it.value();
						double UmSqr = Um * Um;
						//计算对角元素
						//H
						values.emplace_back(m, m, UmSqr * Y.imag() + nodeM.ReactivePowerInjection());
						if (nodeM.Type() == NodeType::PQNode)
						{
							//N
							values.emplace_back(m, subM, -UmSqr * Y.real() - nodeM.ActivePowerInjection());
							//M
							values.emplace_back(subM, m, UmSqr * Y.real() - nodeM.ActivePowerInjection());
							//L
							values.emplace_back(subM, subM, UmSqr * Y.imag() - nodeM.ReactivePowerInjection());
						}
					}
				}
			Jocobian.setFromTriplets(values.begin(), values.end());
			_PS_TRACE("雅可比矩阵 ==========");
			_PS_TRACE(Jocobian);
		}

		bool NRSolver::GenerateNextAnswer()
		{
			SparseLU<SparseMatrix<double>> solver;
			//求解矩阵方程
			Jocobian.makeCompressed();
			solver.compute(Jocobian);
			if (solver.info() != Eigen::Success) return false;
			CorrectionAnswer = solver.solve(PowerInjectionDeviation);
			if (solver.info() != Eigen::Success) return false;
			//计算新的结果
			//注意到 Δy = -J Δx
			//而此处实际解的方程组为 Δy' = J Δx
			//也就是说，Δy = -Δy'
			//Δx = [theta ... theta V ... V]
			CurrentAnswer.head(Block1EquationCount()) -= CorrectionAnswer.head(Block1EquationCount());
			CurrentAnswer.tail(Block2EquationCount()) -= CurrentAnswer.tail(Block2EquationCount()).cwiseProduct(CorrectionAnswer.tail(Block2EquationCount()));
			_PS_TRACE("当前解向量 ==========");
			_PS_TRACE(CurrentAnswer);
			return true;
		}
	}
}


#include<iostream>
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include <boost/program_options.hpp>
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
  using boost::program_options::value;
   int Llead1{};
   int Llead2{};
  int Lchain{};
  double tint{};
  double t0{};
  double tl{};
    double V{};
  double dt{};
  double tot{};
  bool PB{};
  std::string sLlead1{};
  std::string sLlead2{};
  std::string sLchain{};
  std::string stint{};
 std::string st0{};
  std::string stl{};
  std::string sV{};
  std::string sdt{};
  std::string stot{};
  std::string filename;
  std::string sPB{};
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("Ll1", value(&Llead1)->default_value(4), "Ll1")
      ("Ll2", value(&Llead2)->default_value(4), "Ll2")
      ("Lc", value(&Lchain)->default_value(2), "Lc")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("tl", value(&tl)->default_value(1.), "tl")
      ("V", value(&V)->default_value(1.), "V")
      ("tint", value(&tint)->default_value(1.), "tint")
      ("dt", value(&dt)->default_value(0.1), "dt")
      ("tot", value(&tot)->default_value(1.), "tot")
      ("pb", value(&PB)->default_value(false), "PB");

    boost::program_options::variables_map vm;
    boost::program_options::store(parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);

  if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("Ll1"))
      {      std::cout << "Llead1: " << vm["Ll1"].as<int>() << '\n';
      	sLlead1="Ll1"+std::to_string(vm["Ll1"].as<int>());
  	filename+=sLlead1;
      }
        if (vm.count("Ll2"))
      {      std::cout << "Llead2: " << vm["Ll2"].as<int>() << '\n';
      	sLlead2="Ll2"+std::to_string(vm["Ll2"].as<int>());
  	filename+=sLlead2;
      }
         if (vm.count("Lc"))
      {      std::cout << "Lchain: " << vm["Lc"].as<int>() << '\n';
      	sLchain="Lc"+std::to_string(vm["Lc"].as<int>());
  	filename+=sLchain;
      }
      	 if (vm.count("t0"))
      {      std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      	st0="t0"+std::to_string(vm["t0"].as<double>()).substr(0, 3);
      	filename+=st0;
      }
      	 	 if (vm.count("tl"))
      {      std::cout << "tl: " << vm["tl"].as<double>() << '\n';
      	stl="tl"+std::to_string(vm["tl"].as<double>()).substr(0, 3);
      		filename+=stl;
      }
      		 if (vm.count("tint"))
      {      std::cout << "tint: " << vm["tint"].as<double>() << '\n';
      	stint="tint"+std::to_string(vm["tint"].as<double>()).substr(0, 3);
      		filename+=stint;
      }
      		 if (vm.count("V"))
      {      std::cout << "V: " << vm["V"].as<double>() << '\n';
      	sV="V"+std::to_string(vm["V"].as<double>()).substr(0, 6);
      		filename+=sV;
      }
      		 if (vm.count("tot"))
      {      std::cout << "tot: " << vm["tot"].as<double>() << '\n';
      	stot="tot"+std::to_string(vm["tot"].as<double>()).substr(0, 3);
      		filename+=stot;
      }
		 if (vm.count("dt"))
      {      std::cout << "dt: " << vm["dt"].as<double>() << '\n';
      	sdt="dt"+std::to_string(vm["dt"].as<double>()).substr(0, 6);
      		filename+=sdt;
      }
       if (vm.count("pb"))
      {
  	std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
  		sPB="PB"+std::to_string(vm["pb"].as<bool>());
  	filename+=sPB;
      }
     }
  }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }

  int Ltot=Llead1+Llead2+Lchain;

  OneElectronBasis e( Ltot);

		filename+=".bin";

		std::cout<< "dim "<< e.dim << std::endl;

			std::cout<< "dim "<< e << std::endl;

		auto O=Operators::totCurrOperator(e, tint,  Llead1, Llead2, Lchain);
		Mat H1=Operators::totalHetOperator(e,  tint, t0, tl, 0,  Llead1,  Llead2, Lchain );
	 Mat H2=Operators::totalHetOperator(e,  tint, t0, tl, V, Llead1, Llead2,   Lchain );
 //       //;
 //      //

       Eigen::VectorXd eigenVals1(e.dim);
       Eigen::VectorXd eigenVals2(e.dim);

       Eigen::MatrixXd HH1=Eigen::MatrixXd(H1);
       Eigen::MatrixXd HH2=Eigen::MatrixXd(H2);
    std::cout<< Eigen::MatrixXd(H2)<<std::endl;
	std::cout<< "  end "<<std::endl;
    std::cout<< Eigen::MatrixXd(O)<<std::endl;
	//std::cout<< Eigen::MatrixXd(H2)<<std::endl;
   //      Eigen::MatrixXd N=Eigen::MatrixXd(Eph);
      Many_Body::diagMat(HH1, eigenVals1);
      Many_Body::diagMat(HH2, eigenVals2);
      std::cout << "done diag"<<std::endl;
        std::cout<< "EV 1 "<<eigenVals1(0)<<std::endl;

    std::cout<< eigenVals2(eigenVals2.rows()-1)<<std::endl;
    std::cout<< std::exp(eigenVals2(0))<<std::endl;
    std::cout<< std::exp(eigenVals2(eigenVals2.rows()-1))<<std::endl;
      Eigen::MatrixXcd evExp=TimeEv::EigenvalExponent(eigenVals2, dt);
    Eigen::MatrixXcd cEVec=HH2.cast<std::complex<double>>();
    Eigen::VectorXcd newIn=HH1.col(0);
    std::vector<Eigen::VectorXcd> statesVec(int(Ltot/2), Eigen::VectorXcd::Zero(e.dim));
    double energy_0=0;
    for(int i=0; i<int(Ltot/2); i++)
      {
 	statesVec[i]=HH1.col(i);
	energy_0+=eigenVals1(i);
	std::cout<< "i "<< i<< std::endl;
      }
    std::cout<< "GS "<< energy_0<<std::endl;

    std::vector<double> obstebd;
  std::vector<double> time;
  Eigen::MatrixXd O1=Eigen::MatrixXd(O);
  //std::cout<< O << std::endl;
 //  for(int b=0; b<e.dim; b++)
//     {
//       for(int v=0; v<e.dim; v++)
// 	{
// 	  if(std::abs(O1(b, v)>0.000001)){
// 	      std::cout << O1(b, v)<< std::endl;

// }
//     }
//     }
  int i=0;
        while(i*dt<tot)
       {
 	 double sum1{0};
 	 double sum2{0};
	  	 double sum3{0};

 	 for(auto& state : statesVec)
 	   {

	 std::complex<double> c=-im*(state.adjoint()*(O*state))(0);

		   //
 		 std::complex<double> c2=(state.adjoint()*(H1*state))(0);
		  		 std::complex<double> c3=(state.adjoint()*(H2*state))(0);
 		 sum1+=real(c);
 		 sum2+=real(c2);
		  		 sum3+=real(c3);
		 TimeEv::timeev_exact(state, cEVec, evExp);
 	   }
  		  time.push_back(i*dt);
  		  obstebd.push_back(sum1);

       			// 	outputVals(i)=real(c);
       			// 	outputVals2(i)=real(c2);
        		// outputTime(i)=i*dt;
		  	 i++;
			 std::cout<< std::setprecision(8)<<sum1<<" dt "<< i*dt<< "  E  "<< sum2<< " E eyt "<< sum3<<std::endl;
       	 //	  	 BOOST_CHECK(std::abs(real(c2)-real(c))<Many_Body::err);
       	     	             }

       Many_Body::bin_write("timeSPexFermi"+filename, time);
    Many_Body::bin_write("JSPexFermi"+filename, obstebd);

  return 0;
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_HYPERCUBECONVLAYER_HH
#define NETKET_HYPERCUBECONVLAYER_HH

#include <time.h>
#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <fstream>
#include <memory>
#include <random>
#include <vector>

#include "Graph/abstract_graph.hpp"
#include "Utils/all_utils.hpp"
#include "abstract_layer.hpp"

namespace netket {
/** Convolutional layer with spin 1/2 hidden units.
 Important: In order for this to work correctly, VectorType and MatrixType must
 be column major.
 */
class ConvolutionalHypercube : public AbstractLayer {
  static_assert(!MatrixType::IsRowMajor, "MatrixType must be column-major");

  bool usebias_;  // boolean to turn or off bias

  int nv_;            // number of units in the input image
  int nout_;          // number of units in the output image
  int lin_;           // length of input hypercube
  int lout_;          // length of output hypercube
  int dim_;           // dimension of hypercube
  int in_channels_;   // number of input channels
  int in_size_;       // input size = in_channels_ * nv_;
  int out_channels_;  // number of output channels
  int out_size_;      // output size = out_channels_ * nout_;
  int npar_;          // number of parameters in layer

  int stride_;         // convolution stride
  int kernel_length_;  // length of kernel
  int kernel_size_;    // Size of convolutional kernel
  std::vector<std::vector<int>>
      neighbours_;  // list of neighbours for each site
  std::vector<std::vector<int>>
      flipped_nodes_;   // list of reverse neighbours for each site
  MatrixType kernels_;  // Weight parameters, W(in_size x out_size)
  VectorType bias_;     // Bias parameters, b(out_size x 1)

  std::string name_;

  MatrixType lowered_image_;
  MatrixType lowered_image2_;
  MatrixType lowered_der_;
  MatrixType flipped_kernels_;

 public:
  /// Constructor
  ConvolutionalHypercube(const int length, const int dim,
                         const int input_channels, const int output_channels,
                         const int stride = 1, const int kernel_length = 2,
                         const bool use_bias = true)
      : usebias_(use_bias),
        lin_(length),
        dim_(dim),
        in_channels_(input_channels),
        out_channels_(output_channels),
        stride_(stride),
        kernel_length_(kernel_length) {
    // Compatibility checks
    // Check that stride_ is compatible with input image length lin_
    if (lin_ % stride_ != 0) {
      throw InvalidInputError(
          "Stride size is incompatiple with input image size: they should be "
          "commensurate");
    }
    // Check that kernel_length_ is smaller than or equal input image length_
    if (kernel_length_ > lin_) {
      throw InvalidInputError(
          "kernel_length must be at most as large as input image length, "
          "length");
    }

    // Compute the image sizes
    lout_ = lin_ / stride_;
    nv_ = 1;
    nout_ = 1;
    kernel_size_ = 1;
    for (int i = 0; i < dim_; ++i) {
      nv_ *= lin_;
      nout_ *= lout_;
      kernel_size_ *= kernel_length_;
    }
    in_size_ = in_channels_ * nv_;
    out_size_ = out_channels_ * nout_;

    Init();
  }

  void Init() {
    // Construct neighbourhood of all nodes to be acted on by a kernel, i.e.
    // kernel(k) will act on the node neighbours_[i][k] of the
    // input image to give a value at node i in the output image.
    std::vector<Eigen::VectorXi> trans;
    for (int i = 0; i < kernel_size_; ++i) {
      trans.push_back(Site2Coord(i, kernel_length_));
    }
    for (int i = 0; i < nout_; ++i) {
      std::vector<int> neigh;
      Eigen::VectorXi coord = Site2Coord(i, lout_) * stride_;
      for (auto t : trans) {
        Eigen::VectorXi newcoord(dim_);
        for (int d = 0; d < dim_; ++d) {
          newcoord(d) = (coord(d) + t(d)) % lin_;
        }
        neigh.push_back(Coord2Site(newcoord, lin_));
      }
      neighbours_.push_back(neigh);
    }

    // Construct flipped_nodes_[i][k] = l such that
    // input site i contributes to output site l via kernel(k)
    for (int i = 0; i < nv_; ++i) {
      std::vector<int> flippednodes;
      Eigen::VectorXi coord = Site2Coord(i, lin_);
      for (auto t : trans) {
        Eigen::VectorXi newcoord(dim_);
        for (int d = 0; d < dim_; ++d) {
          newcoord(d) = ((coord(d) - t(d)) % lin_ + lin_) % lin_;
        }
        Eigen::VectorXi newcoordprime = newcoord / stride_;
        if (newcoordprime * stride_ == newcoord) {
          flippednodes.push_back(Coord2Site(newcoordprime, lout_));
        } else {
          flippednodes.push_back(-1);
        }
      }
      flipped_nodes_.push_back(flippednodes);
    }

    kernels_.resize(in_channels_ * kernel_size_, out_channels_);
    bias_.resize(out_channels_);

    lowered_image_.resize(in_channels_ * kernel_size_, nout_);
    lowered_image2_.resize(nout_, in_channels_ * kernel_size_);
    lowered_der_.resize(kernel_size_ * out_channels_, nv_);
    flipped_kernels_.resize(kernel_size_ * out_channels_, in_channels_);

    npar_ = in_channels_ * kernel_size_ * out_channels_;

    if (usebias_) {
      npar_ += out_channels_;
    } else {
      bias_.setZero();
    }

    name_ = "Convolutional Hypercube Layer";
  }

  std::string Name() const override { return name_; }

  void InitRandomPars(int seed, double sigma) override {
    VectorType par(npar_);

    netket::RandomGaussian(par, seed, sigma);

    SetParameters(par);
  }

  int Npar() const override { return npar_; }

  int Ninput() const override { return in_size_; }

  int Noutput() const override { return out_size_; }

  void GetParameters(VectorRefType pars) const override {
    int k = 0;
    if (usebias_) {
      for (int i = 0; i < out_channels_; ++i) {
        pars(k) = bias_(i);
        ++k;
      }
    }

    for (int j = 0; j < out_channels_; ++j) {
      for (int i = 0; i < in_channels_ * kernel_size_; ++i) {
        pars(k) = kernels_(i, j);
        ++k;
      }
    }
  }

  void SetParameters(VectorConstRefType pars) override {
    int k = 0;

    if (usebias_) {
      for (int i = 0; i < out_channels_; ++i) {
        bias_(i) = pars(k);
        ++k;
      }
    }

    for (int j = 0; j < out_channels_; ++j) {
      for (int i = 0; i < in_channels_ * kernel_size_; ++i) {
        kernels_(i, j) = pars(k);
        ++k;
      }
    }
  }

  // Feedforward
  void Forward(const VectorType &input, VectorType &output) override {
    Convolve(input, output);

    if (usebias_) {
      int k = 0;
      for (int out = 0; out < out_channels_; ++out) {
        for (int i = 0; i < nout_; ++i) {
          output(k) += bias_(out);
          ++k;
        }
      }
    };
  }

  // performs the convolution of the kernel onto the image and writes into z
  inline void Convolve(const VectorType &image, VectorType &z) {
    // im2col method
    for (int i = 0; i < nout_; ++i) {
      int j = 0;
      for (auto n : neighbours_[i]) {
        for (int in = 0; in < in_channels_; ++in) {
          lowered_image_(in * kernel_size_ + j, i) = image(in * nv_ + n);
        }
        j++;
      }
    }
    Eigen::Map<MatrixType> output_image(z.data(), nout_, out_channels_);
    output_image.noalias() = lowered_image_.transpose() * kernels_;
  }

  void Backprop(const VectorType &prev_layer_output,
                const VectorType & /*this_layer_output*/,
                const VectorType &dout, VectorType &din,
                VectorRefType der) override {
    // VectorType dLz = dout;
    int kd = 0;

    // Derivative for bias, d(L) / d(b) = d(L) / d(z)
    if (usebias_) {
      int k = 0;
      for (int out = 0; out < out_channels_; ++out) {
        der(kd) = 0;
        for (int i = 0; i < nout_; ++i) {
          der(kd) += dout(k);
          ++k;
        }
        ++kd;
      }
    }

    // Derivative for weights, d(L) / d(W) = [d(L) / d(z)] * in'
    // Reshape dLdZ
    Eigen::Map<const MatrixType> dLz_reshaped(dout.data(), nout_,
                                              out_channels_);

    // Reshape image
    for (int in = 0; in < in_channels_; ++in) {
      for (int k = 0; k < kernel_size_; ++k) {
        for (int i = 0; i < nout_; ++i) {
          lowered_image2_(i, k + in * kernel_size_) =
              prev_layer_output(in * nv_ + neighbours_[i][k]);
        }
      }
    }
    Eigen::Map<MatrixType> der_w(der.data() + kd, in_channels_ * kernel_size_,
                                 out_channels_);
    der_w.noalias() = lowered_image2_.transpose() * dLz_reshaped;

    // Compute d(L) / d_in = W * [d(L) / d(z)]
    // int kout = 0;
    for (int out = 0; out < out_channels_; ++out) {
      for (int in = 0; in < in_channels_; ++in) {
        flipped_kernels_.block(out * kernel_size_, in, kernel_size_, 1) =
            kernels_.block(in * kernel_size_, out, kernel_size_, 1);
      }
    }

    for (int i = 0; i < nv_; i++) {
      int j = 0;
      for (auto n : flipped_nodes_[i]) {
        for (int out = 0; out < out_channels_; ++out) {
          lowered_der_(out * kernel_size_ + j, i) =
              n >= 0 ? dout(out * nout_ + n) : 0;
        }
        j++;
      }
    }

    din.resize(in_size_);
    Eigen::Map<MatrixType> der_in(din.data(), nv_, in_channels_);
    der_in.noalias() = lowered_der_.transpose() * flipped_kernels_;
  }

  void to_json(json &pars) const override {
    json layerpar;
    layerpar["Name"] = "Convolutional";
    layerpar["UseBias"] = usebias_;
    layerpar["Inputs"] = in_size_;
    layerpar["Outputs"] = out_size_;
    layerpar["InputChannels"] = in_channels_;
    layerpar["OutputChannels"] = out_channels_;
    layerpar["Bias"] = bias_;
    layerpar["Kernels"] = kernels_;

    pars["Layers"].push_back(layerpar);
  }

  void from_json(const json &pars) override {
    if (FieldExists(pars, "Kernels")) {
      kernels_ = pars["Kernels"];
    } else {
      kernels_.setZero();
    }
    if (FieldExists(pars, "Bias")) {
      bias_ = pars["Bias"];
    } else {
      bias_.setZero();
    }
  }

  int Coord2Site(Eigen::VectorXi const &coord, int L) {
    auto site = 0;
    auto scale = 1;
    for (int i = 0; i < dim_; ++i) {
      site += scale * coord(i);
      scale *= L;
    }
    return site;
  }

  Eigen::VectorXi Site2Coord(int site, int L) {
    Eigen::VectorXi coord(dim_);
    for (int i = 0; i < dim_; ++i) {
      coord(i) = site % L;
      site /= L;
    }
    return coord;
  }
};
}  // namespace netket

#endif


//==============================================================================
//         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 BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE4_1_SPLIT_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE4_1_SPLIT_MULTIPLIES_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE4_1_SUPPORT

#include <boost/simd/swar/functions/split_multiplies.hpp>
#include <boost/simd/include/functions/simd/interleave_first.hpp>
#include <boost/simd/include/functions/simd/interleave_second.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( split_multiplies_
                                   , boost::simd::tag::sse4_1_
                                   , (A0)(A1)
                                   , ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<int64_<A1>,boost::simd::tag::sse_>))
                                     ((simd_<int64_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1, A1& a2, A1& a3) const
    {
      A1 lo = _mm_mul_epi32(a0, a1);
      A1 hi = _mm_mul_epi32(_mm_srli_si128(a0, 4), _mm_srli_si128(a1, 4));

      a2 = interleave_first(lo, hi);
      a3 = interleave_second(lo, hi);
    }
  };
} } }

#endif
#endif


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

  @copyright 2017 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_ALGORITHM_MIN_VAL_HPP_INCLUDED
#define BOOST_SIMD_ALGORITHM_MIN_VAL_HPP_INCLUDED

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minimum.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/is_aligned.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns the value of the element with the smallest value in the range [first,last),
    The largest possible value for the order if the range is empty.

    @param first  Beginning of the range of elements to min_val
    @param last   End of the range of elements to min_val
    @param comp   comparison function object that will be applied.

    @par Requirement

      - @c first and @c last must be pointer to Vectorizable type.

      - @c comp must be a polymorphic unary function object, i.e callable on generic types.

    @par Example

    The following code uses simd::min_val to find the greatest and smallest element
    @c std::vector.
    @snippet min_val.cpp min_val
    @snippet min_val.txt min_val

    @return the minimum value of the range elements.
  **/
  template<typename T, typename Comp> T min_val(T const* first, T const* last, Comp comp)
  {
    if (first == last) return comp(T(0), T(1)) ? Inf<T>() : Minf<T>();

    auto pr = segmented_aligned_range(first,last);
    T m =  *first;

    for( auto const & e : pr.head ) { if (comp(e, m))  m = e; }

    pack<T> mm(m);
    for( pack<T> e : pr.body ) mm = if_else(comp(e, mm), e, mm);

    m = mm[0];
    for(T v : mm) if(comp(v, m)) m = v;

    for( T e : pr.tail ) { if(comp(e, m))  m = e; }

    return m;
  }

  /*!
    @ingroup group-algo

    Returns the value of the element with the smallest value in the range [first,last),
    The largest possible value for the order if the range is empty.

    @param first  Beginning of the range of elements to min_val
    @param last   End of the range of elements to min_val

    @par Requirement

      - @c first and @c last must be pointer to Vectorizable type.

    @par Example

    The following code uses simd::min_val to find the greatest and smallest element
    @c std::vector.
    @snippet min_val.cpp min_val
    @snippet min_val.txt min_val

    @return the minimum value of the range elements.
  **/
  template<typename T> T min_val(T const* first, T const* last)
  {
    if (first == last) return Inf<T>();

    auto pr = segmented_aligned_range(first,last);
    T m = *first;

    for( auto const & e : pr.head ) m = min(e, m);

    pack<T> mm(m);
    for( pack<T> e : pr.body )  mm = min(e, mm);

    m = minimum(mm);
    for( T e : pr.tail ) m = min(e, m);

    return m;
  }
} }

#endif