/*!
  * \file
  * \brief Include this into the main file to have access to the MHD library and code.
  */
#pragma once

// std includes
#include <iostream>
#include <fstream>
#include <cmath>
#include <memory>
// external libraries
#include <armadillo>
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/lu.hpp"
#include "viennacl/linalg/sum.hpp"
#include "viennacl/linalg/maxmin.hpp"
#include "viennacl/tools/random.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/tools/timer.hpp"
#include "viennacl/forwards.h"

typedef viennacl::vector<double> vcl_vec; /*!< GPU arma::vector with ViennaCL */
typedef viennacl::compressed_matrix<double> vcl_sp_mat; /*!< GPU sparse matrix with ViennaCL */
typedef viennacl::matrix<double> vcl_mat; /*!< GPU dense matrix with ViennaCL */

/*!
 * Everything in CoMFi will be in this namespace for safety. This is the top level namespace.
 */
namespace comfi
{

/*!
* All the classes, enums and types can be found here.
*/
namespace types {

/*!
 * \struct Settings
 * \brief The settings for the simulation (used in main loop) are here.
 */
struct Settings {
  /// Maximum number of time steps before simulation end.
  int max_time_steps = -1;
  /// Maximum time before simulation end in normalized units. \f$(t_0)\f$
  double max_time = -1.0;
  /// gmres tolerance
  double tolerance = 1.e-6;
  /// Save solution every X time steps
  int save_dn = -1;
  /// Save solution every X time steps
  double save_dt = -1;
  /// If this run restarts from a previous run
  bool restart = false;
  /// Other flags for runtime.
  uint flags = 0;
};

/*!
 * \brief The Boundary Condition enum.
 */
enum BoundaryCondition {PERIODIC, /*!< Periodic boundary condition (i+1 at right boundary is i=0) */
                        DIRICHLET, /*!< Dirichlet boundary conditions. Inserting data up to user. */
                        NEUMANN, /*!< Neumann boundary conditions. (i+1 at right boundary = i) */
                        MIRROR, /*!< Mirror boundary conditions. (i+1 at right boundary = i or negative that value in case of x component) */
                        DIMENSIONLESS /*!< This boundary is not in a free dimension. */
                       };

/*!
 * \brief The simulation context. Everything needed about the simulation is in here.
 */
class Context {
  /// Current \f$\Delta t\f$ in normalized units of \ref t_0 .
  double m_dt = 0.0;
  /// Current \f$t\f$ in normalized units of \ref t_0 .
  double m_time_elapsed = 0.0;
  /// Current step \f$n\f$ .
  uint m_time_step = 0;

public:
  // Simulation domain
  /// Number of grid points in x-direction (horizontal).
  const arma::uword nx;
  /// Number of grid points in z-direction (vertical).
  const arma::uword nz;
  /// Upper boundary condition in the z-direction.
  const BoundaryCondition bc_up;
  /// Lower boundary condition in the z-direction.
  const BoundaryCondition bc_down;
  /// Right boundary condition in the x-direction.
  const BoundaryCondition bc_right;
  /// Left boundary condition in the x-direction.
  const BoundaryCondition bc_left;

  /// Settings passed on from main.
  const Settings settings;

  // Constants
  /// ratio of specific heats
  //const double gammamono = 5.0/3.0;
  const double gammamono = 2.0;
  /// \f$alpha_p\f$ divergence error propogation parameter
  const double alpha_p = 0.18;
  /// π
  const double pi = arma::datum::pi;
  /// mass of proton in kg
  const double m_i = arma::datum::m_p;
  /// permeability of free space
  const double mu_0 = arma::datum::mu_0;
  /// boltzmann constant in SI
  const double k_b = arma::datum::k;
  /// elementary charge in C
  const double e_ = arma::datum::ec;
  // Normalization constants
  /// Length normalization constant in meters.
  const double l_0 = 1.0;
  /// \f$m^{-3}\f$ density normalization constant.
  const double n_0 = 25.0/(36.0*arma::datum::pi);
  /// Magnetic field normalization constant in Teslas.
  const double B_0 = 1.0/std::sqrt(4.0*arma::datum::pi);
  /// Width (x-direction) in normalized units.
  const double width = l_0;
  /// Height (z-direction) in normalized units.
  const double height = l_0;
  // Derived constants
  /// \f$\Delta x\f$ in normalized units.
  const double dx = (width/nx)/l_0;
  /// \f$\Delta z\f$ in normalized units.
  const double dz = (height/nz)/l_0;
  /// \f$\Delta s\f$ (smallest grid length) in normalized units.
  const double ds = (dx>dz)*dz + (dz>=dx)*dx;
  /// Normalized mass of electron in kg.
  const double m_e = arma::datum::m_e/m_i;
  /// Derived normalization constant of Alfven velocity in m/s. \f$V_0 = V_A = B_0 / \sqrt{\mu_0 n_0 m_i}\f$
  const double V_0 = B_0/std::sqrt(mu_0*n_0*m_i);
  /// Derived normalization constant of time in seconds.
  const double t_0 = l_0/V_0;
  /// Derived normalization constant of pressure in Pascals. \f$p_0 = B_0^2 / \mu_0\f$
  const double p_0 = B_0*B_0/mu_0;
  /// Derived normalization constant of temperature in Kelvin. \f$T_0 = p_0 / (n_0 k_b)\f$
  const double T_0 = p_0/(n_0*k_b);
  /// In normalized units, default is sun value of 0.27395 km/s/s.
  const double g = 0.27395e3*t_0/V_0;
  /// Derived normalization constant for heat transfer.
  const double q_0               = p_0*V_0;
  /// Derived normalization constant for charge in Coulombs.
  const double e_0               = B_0/(mu_0*n_0*V_0*l_0);
  /// Normalized elementary charge constant.
  const double q                 = e_/e_0;
  /// Derived normalization constant for heat transfer coefficient.
  const double kappa_0           = q_0*l_0/T_0;

  // Solution index
  /// Result vector top level index
  const arma::uword Bx    = 0;
  /// Result vector top level index
  const arma::uword Bz    = 1;
  /// Result vector top level index
  const arma::uword Bp    = 2;
  /// Result vector top level index
  const arma::uword n_n   = 3;
  /// Result vector top level index
  const arma::uword Ux    = 4;
  /// Result vector top level index
  const arma::uword Uz    = 5;
  /// Result vector top level index
  const arma::uword Up    = 6;
  /// Result vector top level index
  const arma::uword n_p   = 7;
  /// Result vector top level index
  const arma::uword Vx    = 8;
  /// Result vector top level index
  const arma::uword Vz    = 9;
   /// Result vector top level index
  const arma::uword Vp    = 10;
  /// Result vector top level index
  const arma::uword E_n   = 11;
   /// Result vector top level index
  const arma::uword E_p   = 12;
  /// Result vector top level index
  const arma::uword GLM   = 13;

  // Ranges
  const viennacl::range r_grid = viennacl::range(0, nx*nz);
  const viennacl::range r_Np = viennacl::range(n_p, n_p+1);
  const viennacl::range r_Nn = viennacl::range(n_n, n_n+1);
  const viennacl::range r_NVx = viennacl::range(Vx, Vx+1);
  const viennacl::range r_NVz = viennacl::range(Vz, Vz+1);
  const viennacl::range r_NVp = viennacl::range(Vp, Vp+1);
  const viennacl::range r_NUx = viennacl::range(Ux, Ux+1);
  const viennacl::range r_NUz = viennacl::range(Uz, Uz+1);
  const viennacl::range r_NUp = viennacl::range(Up, Up+1);
  const viennacl::range r_Ep = viennacl::range(E_p, E_p+1);
  const viennacl::range r_En = viennacl::range(E_n, E_n+1);
  const viennacl::range r_Bx = viennacl::range(Bx, Bx+1);
  const viennacl::range r_Bz = viennacl::range(Bz, Bz+1);
  const viennacl::range r_Bp = viennacl::range(Bp, Bp+1);
  const viennacl::range r_GLM = viennacl::range(GLM, GLM+1);
  const arma::uword num_of_eq = 14;

  Context(arma::uword _nx,
          arma::uword _nz,
          BoundaryCondition _bc_up=NEUMANN,
          BoundaryCondition _bc_down=NEUMANN,
          BoundaryCondition _bc_right=NEUMANN,
          BoundaryCondition _bc_left=NEUMANN,
          const Settings _settings = Settings()
          ) : nx(_nx), nz(_nz),
              bc_up(_bc_up),
              bc_down(_bc_down),
              bc_right(_bc_right),
              bc_left(_bc_left),
              settings(_settings) {
  }

  /*!
   * \brief returns number of grid points per unknown.
   * \return Number of grid points per unknown.
   */
  arma::uword num_of_grid() const { return nz*nx; }

  /*!
   * \brief Set \f$\Delta t\f$ in normalized units of \ref t_0 .
   * \param dt Time step in normalized units of \ref t_0 .
   * Since \ref m_dt is a private member. This variable must be changed through this function.
   */
  void set_dt(const double &dt) {
    m_dt = dt;
  }

  /*!
   * \brief Advance the state of the simulation.
   * This will advance the state of the simulation so that the \ref time_elapsed()
   * and \ref time_step() are updated.
   */
  void advance() {
    m_time_elapsed += m_dt;
    m_time_step++;
  }

  /// Getter of \f$\c_h\f$
  double c_h() const { return ds/m_dt; }
  /// Getter of \f$\Delta t\f$
  double dt() const { return m_dt; }
  /// Getter of \f$t\f$
  double time_elapsed() const { return m_time_elapsed; }
  /// Getter of \f$n\f$
  uint time_step() const { return m_time_step; }

  // Matrix range returns
  inline viennacl::range range(const uint &var) const { return viennacl::range(var, var+1); }
  inline viennacl::matrix_range<vcl_mat> range(const uint &var, const vcl_mat &xn) {
    return project(xn, r_grid, range(var));
  }
  inline viennacl::matrix_range<vcl_mat> v_Np(const vcl_mat &xn) {
    return project(xn, r_grid, r_Np);
  }
  inline viennacl::matrix_range<vcl_mat> v_Nn(const vcl_mat &xn) {
    return project(xn, r_grid, r_Nn);
  }
  inline viennacl::matrix_range<vcl_mat> v_NVx(const vcl_mat &xn) {
    return project(xn, r_grid, r_NVx);
  }
  inline viennacl::matrix_range<vcl_mat> v_NVz(const vcl_mat &xn) {
    return project(xn, r_grid, r_NVz);
  }
  inline viennacl::matrix_range<vcl_mat> v_NVp(const vcl_mat &xn) {
    return project(xn, r_grid, r_NVp);
  }
  inline viennacl::matrix_range<vcl_mat> v_NUx(const vcl_mat &xn) {
    return project(xn, r_grid, r_NUx);
  }
  inline viennacl::matrix_range<vcl_mat> v_NUz(const vcl_mat &xn) {
    return project(xn, r_grid, r_NUz);
  }
  inline viennacl::matrix_range<vcl_mat> v_NUp(const vcl_mat &xn) {
    return project(xn, r_grid, r_NUp);
  }
  inline viennacl::matrix_range<vcl_mat> v_Bx(const vcl_mat &xn) {
    return project(xn, r_grid, r_Bx);
  }
  inline viennacl::matrix_range<vcl_mat> v_Bz(const vcl_mat &xn) {
    return project(xn, r_grid, r_Bz);
  }
  inline viennacl::matrix_range<vcl_mat> v_Bp(const vcl_mat &xn) {
    return project(xn, r_grid, r_Bp);
  }
  inline viennacl::matrix_range<vcl_mat> v_GLM(const vcl_mat &xn) {
    return project(xn, r_grid, r_GLM);
  }
  inline viennacl::matrix_range<vcl_mat> v_Ep(const vcl_mat &xn) {
    return project(xn, r_grid, r_Ep);
  }
  inline viennacl::matrix_range<vcl_mat> v_En(const vcl_mat &xn) {
    return project(xn, r_grid, r_En);
  }
};

} // namespace types

/*!
 * \brief Functions to build or apply the 'operators'.
 * Permutations of the matrix.
 */
namespace operators {

/*!
* \brief Permutate a solution down one cell.
* \param xn Solution matrix context.
* \param ctx Simulation context.
* \return Matrix of \f$A_{j-1}\f$
* Every index is replaced by the cell below it while maintaining boundary conditions.
*/
vcl_mat jm1(const vcl_mat &xn, comfi::types::Context ctx);

/*!
* \brief Permutate a solution up one cell.
* \param xn Solution matrix context.
* \param ctx Simulation context.
* \return Matrix of \f$A_{j+1}\f$
* Every index is replaced by the cell above it while maintaining boundary conditions.
*/
vcl_mat jp1(const vcl_mat &xn, comfi::types::Context ctx);

/*!
* \brief Permutate a solution left one cell.
* \param xn Solution matrix context.
* \param ctx Simulation context.
* \return Matrix of \f$A_{i-1}\f$
* Every index is replaced by the cell to the left of it while maintaining boundary conditions.
*/
vcl_mat im1(const vcl_mat &xn, comfi::types::Context ctx);

/*!
* \brief Permutate a solution right one cell.
* \param xn Solution matrix context.
* \param ctx Simulation context.
* \return Matrix of \f$A_{i+1}\f$
* Every index is replaced by the cell to the right of it while maintaining boundary conditions.
*/
vcl_mat ip1(const vcl_mat &xn, comfi::types::Context ctx);

/*!
 * \brief Permutate a result arma::vector right one cell.
 * \param ctx Simulation context.
 * \return Sparse matrix to multiply a result arma::vector by.
 * Every index is replaced by the cell to the right of it while maintaining boundary conditions.
 */
const arma::sp_mat buildPip1(comfi::types::Context &ctx);

/*!
 * \brief Permutate a result arma::vector left one cell.
 * \param ctx Simulation context.
 * \return Sparse matrix to multiply a result arma::vector by.
 * Every index is replaced by the cell to the left of it while maintaining boundary conditions.
 */
const arma::sp_mat buildPim1(comfi::types::Context &ctx);

} // namespace operators

/*!
 * Utility functions. This namespace contains non-mathematical functions to assist with IO, monitoring, logging and error checking. Also included are RHS and LHS functions.
 */
namespace util
{
/*!
 * \brief gettimestr Get current time.
 * \return const std::string in YYYY-MM-DD-HH-MM format.
 */
std::string gettimestr();

/*!
 * \brief save_solution Saves the solution to the output folder
 * \param x0 Solution matrix.
 * \param ctx Simulation context.
 * \param data_name data set name to save it under in hdf5 tree. Defaults to "unknowns"
 * \return Success or fail.
 */
bool save_solution(const vcl_mat &x0,
                   comfi::types::Context &ctx,
                   const std::string &data_name = "unknowns");

/*!
 * \brief sendtolog Send message to log file.
 * \param message Message std::string to show in log file.
 * \param filename The log filename make sure this is always the same.
 */
void sendtolog(const std::string message, const std::string filename);

/*!
 * \brief Calculate the initial condition based on Orszang-Tang Vortex.
 * \param ctx Simulation context
 * \return initial condition matrix
 */
vcl_mat ot_vortex_ic(comfi::types::Context &ctx);

/*!
 * \brief Calculate the initial condition based on Sod's Shock Tube.
 * \param ctx Simulation context
 * \return initial condition matrix
 */
vcl_mat shock_tube_ic(comfi::types::Context &ctx);

// Misc

/*!
 * \brief getmaxV get fast mode speed + local speed OR resistivity speed whichever is faster
 * \param x0 Solution matrix
 * \param ctx Simulation context
 * \return Max characteristic speed in normalized units
 */
double getmaxV(const vcl_mat &x0, comfi::types::Context &ctx);

/*!
 * \brief vec_to_mat Change a column vector to a viennacl::matrix type with one column
 * \param vec viennacl::vector
 * \return viennacl::matrix of one column
 */
vcl_mat vec_to_mat(const vcl_vec &vec);

/*!
 * \brief Write a binary file of a field arma::vector. Seperate binary files for each direction.
 * \param x0 result arma::vector
 * \param name Name of field, will be used as filename prefix.
 * \param timestep Time step to save as
 * \return True if no error.
 */
bool saveField(const arma::vec &x0, const std::string name, const int timestep);

/*!
 * \brief Read the arguments passed to the executable and change the Settings struct.
 */
void interpret_arguments(comfi::types::Settings &settings, int argc, char** argv);

/*!
 * \brief saveScalar Write a binary file of a scalar arma::vector.
 * \param x0 result arma::vector
 * \param name Name of scalar, will be used as filename prefix.
 * \param timestep Time step to save as
 * \return True if no error.
 */
bool saveScalar(const arma::vec &x0, const std::string name, const int timestep);

/*!
 * \brief saveField Write a binary file of a field arma::vector. Seperate binary files for each direction.
 * \param x0 result arma::vector
 * \param name Name of field, will be used as filename prefix.
 * \param timestep Time step to save as
 * \return True if no error.
 */
bool saveField(const vcl_vec &x0, const std::string name, const int timestep);

/*!
 * \brief saveScalar Write a binary file of a scalar arma::vector.
 * \param x0 result arma::vector
 * \param name Name of scalar, will be used as filename prefix.
 * \param timestep Time step to save as
 * \return True if no error.
 */
bool saveScalar(const vcl_vec &x0, const std::string name, const int timestep);

} // namespace util

/*!
 * Linear algebra routines and deriving variables and such are all found here.
 */
namespace routines
{

/*!
 * \brief Build an eigenvalue matrix (wave speeds) for the Lax-Friedrichs scheme in the \f$x\f$-direction.
 * \param p_eig Ion wave speeds.
 * \param n_eig Neutral wave speeds.
 */
vcl_mat build_eig_matrix_x(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief Build an eigenvalue matrix (wave speeds) for the Lax-Friedrichs scheme in the \f$z\f$-direction.
 * \param p_eig Ion wave speeds.
 * \param n_eig Neutral wave speeds.
 */
vcl_mat build_eig_matrix_z(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief pressure_n Returns the plasma pressure based on the type of energy being used
 * \param xn Solution matrix
 * \param ctx Simulation context
 * \return Simulation pressure matrix in (grid, 1) dimensions
 */
vcl_mat pressure_p(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief pressure_n Returns the neutral pressure based on the type of energy being used
 * \param xn Solution matrix
 * \param ctx Simulation context
 * \return Simulation pressure matrix in (grid, 1) dimensions
 */
vcl_mat pressure_n(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief Fx Get flux values in the x direction.
 * \param xn Solution matrix in the cell edge.
 * \param xn_ij Solution matrix in the cell center.
 * \param ctx Simulation context.
 * \return Flux function matrix results.
 */
vcl_mat Fx(const vcl_mat &xn, const vcl_mat &xn_ij, comfi::types::Context &ctx);

/*!
 * \brief Fz Get flux values in the z direction.
 * \param xn Solution matrix in the cell edge.
 * \param xn_ij Solution matrix in the cell center.
 * \param ctx Simulation context.
 * \return Flux function matrix results.
 */
vcl_mat Fz(const vcl_mat &xn, const vcl_mat &xn_ij, comfi::types::Context &ctx);

/*!
 * \brief Re_MUSCL Do flux reconstruction with the MUSCL scheme and any source terms.
 * \param xn Solution matrix
 * \param ctx Simulation context
 * \return Solution.
 */
vcl_mat Re_MUSCL(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief Flux limiter
 * \param r ratio of gradients
 * \return Limiter function values
 */
vcl_mat fluxl(const vcl_mat &r);

/*!
 * \brief Get plasma sound speed
 * \param xn matrix of results
 * \param ctx Simulation context
 * \return vector (vcl_mat) of plasma sound speed
 */
vcl_mat sound_speed_p(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief Get neutral sound speed
 * \param xn matrix of results
 * \param ctx Simulation context
 * \return vector (vcl_mat) of neutral sound speed
 */
vcl_mat sound_speed_n(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief fast_speed_z Get vertical fast mode speed
 * \param xn result matrix
 * \param ctx Simulation context.
 * \return Column vector vcl_mat of vertical fast mode speed of ions.
 */
vcl_mat fast_speed_z(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief fast_speed_x Get horizontal fast mode speed
 * \param xn Result matrix.
 * \param ctx	Simulation context.
 * \return Column vector (vcl_mat) of horizontal fast mode speeds.
 */
vcl_mat fast_speed_x(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief polyval compute's polynomial using Herner's scheme
 * \param p polynomial coefficient vector of size (polynomial degree + 1)
 * \param x vector to be computed
 * \return result of the polynomial
 */
vcl_vec polyval(const arma::vec &p, const vcl_vec &x);

/*!
 * \brief computeRHS_Euler Compute right hand side using Eulerian time stepping.
 * \param xn Current time step result matrix
 * \param ctx Simulation context
 * \return Right hand side result solution matrix
 */
vcl_mat computeRHS_Euler(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief computeRHS_RK4 Compute right hand side using Runge-Kutta 4 time stepping.
 * \param xn Current time step result arma::vector
 * \param dt Change in time
 * \param t Time elapsed
 * \param op operators
 * \param bg Background data
 * \return  Right hand side result arma::vector
 */
vcl_mat computeRHS_RK4(const vcl_mat &xn, comfi::types::Context &ctx);

/*!
 * \brief Shock Tube dirichlet boundary conditions
 * \param Lxn Left state
 * \param Rxn Right state
 * \param ctx Context
 */
void topbc_shock_tube(vcl_mat &Lxn, vcl_mat &Rxn, comfi::types::Context &ctx);

/*!
 * \brief Shock Tube dirichlet boundary conditions
 * \param Lxn Left state
 * \param Rxn Right state
 * \param ctx Context
 */
void bottombc_shock_tube(vcl_mat &Lxn, vcl_mat &Rxn, comfi::types::Context &ctx);

// inlines

/* inline const vcl_vec div_tvd(const vcl_vec &s_iph, const vcl_vec &s_imh, const vcl_vec &s_jph, const vcl_vec &s_jmh) */
/* { */
/*   const vcl_vec xpart = (s_iph-s_imh)/dx; */
/*   const vcl_vec zpart = (s_jph-s_jmh)/dz; */
/*   return xpart+zpart; */
/* } */

} // namespace routines

/*!
 * Solar chromosphere math functions.
 */
namespace sol
{

inline double nu_nn(const double &nn, const double &T, const comfi::types::Context &ctx)
{
  const double sigma_nn = 7.73e-19; //m-2
  const double m_nn = 1.007825*arma::datum::m_u;
  const double nn_coeff = sigma_nn * std::sqrt(16.0*arma::datum::k/(arma::datum::pi*m_nn));
  return (nn_coeff * nn * ctx.n_0 * std::sqrt(std::abs(T*ctx.T_0)) * ctx.t_0); // ion-neutral collision rate
}

inline arma::vec nu_nn(const arma::vec &nn, const arma::vec &T, const comfi::types::Context &ctx)
{
  const double sigma_nn = 7.73e-19; //m-2
  const double m_nn = 1.007825*arma::datum::m_u;
  const double nn_coeff = sigma_nn * std::sqrt(16.0*arma::datum::k/(arma::datum::pi*m_nn));
  return (nn_coeff * ctx.n_0 * ctx.t_0 * nn % arma::sqrt(arma::abs(T*ctx.T_0)) ); // ion-neutral collision rate
}

inline vcl_vec nu_nn(const vcl_vec &nn, const vcl_vec &T, const comfi::types::Context &ctx)
{
  const double sigma_nn = 7.73e-19; //m-2
  const double m_nn = 1.007825*arma::datum::m_u;
  const double nn_coeff = sigma_nn * std::sqrt(16.0*arma::datum::k/(arma::datum::pi*m_nn));
  return nn_coeff * ctx.n_0 * ctx.t_0  * viennacl::linalg::element_prod(nn, viennacl::linalg::element_sqrt(T*ctx.T_0)); // ion-neutral collision rate
}

inline double nu_in(const double &nn, const double &T, const comfi::types::Context &ctx)
{
  const double sigma_in = 1.16e-18; //m-2
  const double m_in = 1.007276466879*1.007825/(1.007276466879+1.007825);
  const double in_coeff = sigma_in * std::sqrt(8.0*arma::datum::k/(arma::datum::pi*m_in));
  return (in_coeff * nn * ctx.n_0 * std::sqrt(std::abs(T*ctx.T_0)) * ctx.t_0); // ion-neutral collision rate
}

inline arma::vec nu_in(const arma::vec &nn, const arma::vec &T, const comfi::types::Context &ctx)
{
  const double sigma_in = 1.16e-18; //m-2
  const double m_in = 1.007276466879*1.007825/(1.007276466879+1.007825);
  const double in_coeff = sigma_in * std::sqrt(8.0*arma::datum::k/(arma::datum::pi*m_in));
  return (in_coeff * ctx.n_0 * ctx.t_0 * nn % arma::sqrt(arma::abs(T*ctx.T_0)) ); // ion-neutral collision rate
}

inline vcl_vec nu_in(const vcl_vec &nn, const vcl_vec &T, const comfi::types::Context &ctx)
{
  const double sigma_in = 1.16e-18; //m-2
  const double m_in = 1.007276466879*1.007825/(1.007276466879+1.007825);
  const double in_coeff = sigma_in * std::sqrt(8.0*arma::datum::k/(arma::datum::pi*m_in));
  return in_coeff * ctx.n_0 * ctx.t_0  * viennacl::linalg::element_prod(nn, viennacl::linalg::element_sqrt(T*ctx.T_0)); // ion-neutral collision rate
}

inline double nu_en(const double &nn, const double &T, const comfi::types::Context &ctx)
{
  const double sigma_en = 1.0e-19; //m-2
  const double m_en = arma::datum::m_e*1.007825*arma::datum::m_u/(arma::datum::m_e+1.007825*arma::datum::m_u);
  const double en_coeff = sigma_en * std::sqrt(8.0*arma::datum::k/(arma::datum::pi*m_en));
  return (en_coeff * nn * ctx.n_0 * sqrt(std::abs(T*ctx.T_0)) * ctx.t_0); // electron-neutral collision rate
}

inline vcl_vec nu_en(const vcl_vec &nn, const vcl_vec &T, const comfi::types::Context &ctx)
{
  const double sigma_en = 1.0e-19; //m-2
  const double m_en = arma::datum::m_e*1.007825*arma::datum::m_u/(arma::datum::m_e+1.007825*arma::datum::m_u);
  const double en_coeff = sigma_en * std::sqrt(8.0*arma::datum::k/(arma::datum::pi*m_en));
  return en_coeff * ctx.n_0 * ctx.t_0 * viennacl::linalg::element_prod(nn, viennacl::linalg::element_sqrt(T*ctx.T_0)); // electron-neutral collision rate
}

inline double nu_ei(const double &ne, const double &T, const comfi::types::Context &ctx)
{
  const double coloumb_logarithm = 10.0;
  const double r = arma::datum::ec*arma::datum::ec/(4.0*arma::datum::pi*arma::datum::eps_0*arma::datum::k);
  const double sigma_ei = coloumb_logarithm*arma::datum::pi*r*r;
  const double coeff_ei = (4.0/3.0) * sigma_ei * std::sqrt(2.0*arma::datum::k/(arma::datum::pi*arma::datum::m_e));
  return (coeff_ei * ne * ctx.n_0 * std::pow(std::abs(T*ctx.T_0),-1.5) * ctx.t_0); // electron-ion collision rate
}

inline vcl_vec nu_ei(const vcl_vec &ne, const vcl_vec &Te, const comfi::types::Context &ctx)
{
  const double coloumb_logarithm = 10.0;
  const double r = arma::datum::ec*arma::datum::ec/(4.0*arma::datum::pi*arma::datum::eps_0*arma::datum::k);
  const double sigma_ei = coloumb_logarithm*arma::datum::pi*r*r;
  const double coeff_ei = (4.0/3.0) * sigma_ei * std::sqrt(2.0*arma::datum::k/(arma::datum::pi*arma::datum::m_e));
  return coeff_ei * ctx.n_0 * ctx.t_0* viennacl::linalg::element_prod(ne,viennacl::linalg::element_exp(-1.5*viennacl::linalg::element_log(ctx.T_0*Te))); // electron-ion collision rate
}

inline vcl_vec nu_ii(const vcl_vec &Np, const vcl_vec &Tp, const comfi::types::Context &ctx)
{
  const double coloumb_logarithm = 10.0;
  const double r = arma::datum::ec*arma::datum::ec/(4.0*arma::datum::pi*arma::datum::eps_0*arma::datum::k); //without T
  const double sigma_ii = coloumb_logarithm*arma::datum::pi*r*r;
  const double coeff_ii = (4.0/3.0) * sigma_ii * std::sqrt(2.0*arma::datum::k/(arma::datum::pi*arma::datum::m_p));
  return ctx.t_0 * ctx.n_0 * coeff_ii * viennacl::linalg::element_prod(Np, viennacl::linalg::element_exp(-1.5*viennacl::linalg::element_log(ctx.T_0*Tp))); // electron-ion collision rate
}

inline double nu_ii(const double &Np, const double &Tp, const comfi::types::Context &ctx)
{
  const double coloumb_logarithm = 10.0;
  const double r = arma::datum::ec*arma::datum::ec/(4.0*arma::datum::pi*arma::datum::eps_0*arma::datum::k); //without T
  const double sigma_ii = coloumb_logarithm*arma::datum::pi*r*r;
  const double coeff_ii = (4.0/3.0) * sigma_ii * std::sqrt(2.0*arma::datum::k/(arma::datum::pi*arma::datum::m_p));
  return coeff_ii * ctx.t_0 * Np * ctx.n_0 * std::exp(-1.5*std::log(ctx.T_0*Tp)); // electron-ion collision rate
}

inline vcl_vec resistivity(const vcl_vec &Np, const vcl_vec &Nn, const vcl_vec &Tp, const vcl_vec &Tn, const comfi::types::Context &ctx)
{
  return (ctx.m_e/(ctx.q*ctx.q))*viennacl::linalg::element_div((nu_ei(Np, Tp, ctx)+nu_en(Nn, 0.5*(Tp+Tn), ctx)),Np);
}
inline double resistivity(const double &Np, const double &Nn, const double &Tp, const double &Tn, const comfi::types::Context &ctx)
{
  return (ctx.m_e/(ctx.q*ctx.q)) * (nu_ei(Np, Tp, ctx)+nu_en(Nn, 0.5*(Tp+Tn), ctx))/Np;
}

inline double thermalvel(const double &T, const comfi::types::Context &ctx)
{
  return std::sqrt(2.0*arma::datum::k/arma::datum::m_p) * std::sqrt(T*ctx.T_0) / ctx.V_0;
}

inline vcl_vec thermalvel(const vcl_vec &T, const comfi::types::Context &ctx)
{
  using namespace viennacl::linalg;
  return std::sqrt(2.0*arma::datum::k/arma::datum::m_p) * element_sqrt(T*ctx.T_0) / ctx.V_0;
}

inline double kappa_n(const double &Tp, const double &Tn, const double &Np, const double &Nn, const comfi::types::Context &ctx)
{
  return Nn * thermalvel(Tn, ctx) * thermalvel(Tn, ctx) / nu_nn(Nn, Tn, ctx);
}

inline vcl_vec kappa_n(const vcl_vec &Tp, const vcl_vec &Tn, const vcl_vec &Np, const vcl_vec &Nn, const comfi::types::Context &ctx)
{
  using namespace viennacl::linalg;
  const vcl_vec v2 = element_prod(thermalvel(Tn, ctx), thermalvel(Tn, ctx));
  const vcl_vec nv2 = element_prod(Nn, v2);
  const vcl_vec nunn = nu_nn(Nn, Tn, ctx);
  return element_div(nv2, nunn);
}

inline double kappa_p(const double &Tp, const double &Tn, const double &Np, const double &Nn, const comfi::types::Context &ctx)
{
  return Np * thermalvel(Tp, ctx) * thermalvel(Tp, ctx) / (nu_in(Nn, 0.5*(Tp+Tn), ctx) + nu_ii(Np, Tp, ctx));
}

inline vcl_vec kappa_p(const vcl_vec &Tp, const vcl_vec &Tn, const vcl_vec &Np, const vcl_vec &Nn, const comfi::types::Context &ctx)
{
  using namespace viennacl::linalg;
  const vcl_vec v2 = element_prod(thermalvel(Tp, ctx), thermalvel(Tp, ctx));
  const vcl_vec nv2 = element_prod(Np, v2);
  const vcl_vec nuin = nu_in(Nn, 0.5*(Tp+Tn), ctx);
  const vcl_vec nuii = nu_ii(Np, Tp, ctx);
  return element_div(nv2, nuin+nuii);
}

/*!
 * \brief Calculate recombination coefficient for ions to recombine to neutrals.
 * \param Tp Ion temperature
 * \return Vector of normalized volumetric recombination rate.
 */
inline vcl_vec recomb_coeff(const vcl_vec &T, const comfi::types::Context &ctx)
{
  using namespace viennacl::linalg;
  static const vcl_vec one = viennacl::scalar_vector<double>(T.size(), 1.0);
  const vcl_vec beta = 0.6 * 13.6 * 1.6021766208e-19 * element_div(one, arma::datum::k*ctx.T_0*T);
  vcl_vec coeff = 0.4288*one + 0.5*element_log(beta) + 0.4698*element_pow(beta, -(1.0/3.0)*one);
  coeff = element_prod(5.20e-20*element_sqrt(beta), coeff);
  return ctx.t_0 * ctx.n_0 * coeff;
}

inline double recomb_coeff(const double &T, const comfi::types::Context &ctx)
{
  const double beta = 0.6 * 13.6 * 1.6021766208e-19 / (arma::datum::k*ctx.T_0*T);
  return ctx.t_0 * ctx.n_0 * 5.20e-20 * std::sqrt(beta) * (0.4288+std::log(beta)+0.4698*std::pow(beta, -(1.0/3.0)));
}

/*!
 * \brief Calculate normalized ionization coefficient for neutrals to ionize due to collisions (Draine p 134)
 * \param Tn Neutral temperature
 * \return Vector of normalized volumetric ionization rate
 */
inline vcl_vec ionization_coeff(const vcl_vec &T, const comfi::types::Context &ctx)
{
  using namespace viennacl::linalg;
  static const vcl_vec one = viennacl::scalar_vector<double>(T.size(), 1.0);
  const vcl_vec beta = 0.6 * 13.6 * 1.6021766208e-19 * element_div(one, arma::datum::k*ctx.T_0*T);
  const vcl_vec coeff = element_div(2.34e-14*element_exp(-beta), element_sqrt(beta));
  return ctx.t_0 * ctx.n_0 * coeff;

}

inline double ionization_coeff(const double &T, const comfi::types::Context &ctx)
{
  const double beta = 0.6 * 13.6 * 1.6021766208e-19 / (arma::datum::k*ctx.T_0*T);
  return ctx.t_0 * ctx.n_0 * 2.34e-14 * std::exp(-beta) / std::sqrt(beta);
}
} // namespace sol

} // namespace mhdsim

/*!
 * \brief Find scalar vector index.
 * \param i Horizontal index.
 * \param j Vertical index
 * \return Scalar vector index.
 */
inline arma::uword inds(const arma::uword &i, const arma::uword &j, comfi::types::Context &ctx)
{
  return i+j*ctx.nx;
}

/*
vim: tabstop=2
vim: shiftwidth=2
vim: smarttab
vim: expandtab
*/


#define BOOST_TEST_MODULE "test_worm_like_chain_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/local/WormLikeChainPotential.hpp>

BOOST_AUTO_TEST_CASE(WormLikeChain_double)
{
    mjolnir::LoggerManager::set_default_logger("test_worm_like_chain_potential.log");

    using real_type = double;
    constexpr std::size_t N = 500;
    constexpr real_type   h = 1e-6;
    constexpr real_type tol = 1e-6;
    const real_type       p = 3.9;
    const real_type      lc = 19.0;

    mjolnir::WormLikeChainPotential<real_type> pot(p, lc);

    const real_type x_min = lc * 0.1;
    const real_type x_max = lc * 0.9;

    mjolnir::test::check_potential(pot, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(WormLikeChain_float)
{
    using real_type = float;
    constexpr std::size_t N = 500;
    constexpr real_type   h = 1e-3;
    constexpr real_type tol = 1e-2;
    const real_type       p = 3.9;
    const real_type      lc = 19.0;

    mjolnir::WormLikeChainPotential<real_type> pot(p, lc);

    const real_type x_min = lc * 0.1;
    const real_type x_max = lc * 0.9;

    mjolnir::test::check_potential(pot, x_min, x_max, tol, h, N);
}


/*
 * rt_generic_rk4_lorenz.cpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * 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)
 */

#include <boost/array.hpp>

#include "rt_explicit_rk.hpp"

#include "rk_performance_test_case.hpp"

#include "lorenz.hpp"

typedef boost::array< double , 3 > state_type;

typedef rt_explicit_rk< state_type > rk_stepper_type;

const size_t stage_count = 4;

class rt_generic_wrapper
{
public:

    rt_generic_wrapper( const double * const * a ,
                        const rk_stepper_type::coeff_b_type &b ,
                        const rk_stepper_type::coeff_c_type &c )
        : m_stepper( stage_count ,
                     (rk_stepper_type::coeff_a_type) a , b , c )
    { }

    void reset_init_cond()
    {
        m_x[0] = 10.0 * rand() / RAND_MAX;
        m_x[1] = 10.0 * rand() / RAND_MAX;
        m_x[2] = 10.0 * rand() / RAND_MAX;
        m_t = 0.0;
    }

    inline void do_step( const double dt )
    {
        m_stepper.do_step( lorenz() , m_x , m_t , dt );
        //m_t += dt;
    }

    double state( const size_t i ) const
    { return m_x[i]; }

private:
    state_type m_x;
    double m_t;
    rk_stepper_type m_stepper;
};

int main()
{

    const double a_tmp[3*4/2] = { 0.5 ,
                                  0.0 , 1.0 ,
                                  0.0 , 0.0 , 1.0 };
    const double* const a[3] = { a_tmp , a_tmp+1 , a_tmp+3 };

    rk_stepper_type::coeff_b_type b( stage_count );
    b[0] = 1.0/6; b[1] = 1.0/3; b[2] = 1.0/3; b[3] = 1.0/6;

    rk_stepper_type::coeff_c_type c( stage_count );
    c[0] = 0.0; c[1] = 0.5; c[2] = 0.5; c[3] = 1.0;

    rt_generic_wrapper stepper( a , b , c );

    run( stepper );
}


// -----------------------------------------------------------------------------
/**
 *  @brief 2D homography motion model (function) in direct methods tracking.
 *  @author Jose M. Buenaposada
 *  @date 2012/10/14
 *  @version $revision$
 *
 *  $id$
 *
 *  Grupo de investigación en Percepción Computacional y Robótica)
 *  (Perception for Computers & Robots research Group)
 *  Facultad de Informática (Computer Science School)
 *  Universidad Politécnica de Madrid (UPM) (Madrid Technical University)
 *  http://www.dia.fi.upm.es/~pcr
 *
 */
// -----------------------------------------------------------------------------

#include "homography_2d.hpp"
#include "trace.hpp"
#include <limits>
#include <boost/concept_check.hpp>

namespace upm { namespace pcr
{

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
Homography2D::Homography2D
  ()
{
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
Homography2D::~Homography2D
  ()
{
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -------------------------------------------------------------------------
// cv::Mat
// Homography2D::computeMotionJacobian
//   (
//   cv::Mat params
//   )
// {
// };

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::scaleInputImageResolution
  (
  cv::Mat params,
  double scale
  )
{
  cv::Mat newH;
  cv::Mat new_params = cv::Mat::eye(params.rows, 1, cv::DataType<MAT_TYPE>::type);
  cv::Mat H          = params.reshape(1,3).t();

  cv::Mat S    = cv::Mat::eye(3,3,cv::DataType<MAT_TYPE>::type);
  S.at<MAT_TYPE>(0,0) = scale;
  S.at<MAT_TYPE>(1,1) = scale;

  newH = S*H;
  newH = newH.t();
  newH.reshape(1,9).copyTo(new_params);

  return new_params;
}

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
//    The motion params are always from template to current image and with this
//    method we have to use the inverse motion model.
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::transformCoordsToTemplate
  (
  cv::Mat coords,
  cv::Mat params
  )
{
  cv::Mat H = params.reshape(1,3).t();
  cv::Mat invH      = H.inv().t();

  cv::Mat inv_params = invH.reshape(1,9);

  return transformCoordsToImage(coords, inv_params);
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::transformCoordsToImage
  (
  cv::Mat coords,
  cv::Mat params
  )
{
  assert(coords.cols == 2); // We need two dimensional coordinates

  cv::Mat H                      = params.reshape(1, 3).t();
  cv::Mat homogeneous_coords     = cv::Mat::ones(coords.rows, 3, cv::DataType<MAT_TYPE>::type);
  cv::Mat homogeneous_coords_ref = homogeneous_coords(cv::Range::all(), cv::Range(0, 2));
  coords.copyTo(homogeneous_coords_ref);

  cv::Mat homogeneous_new_coords = (homogeneous_coords * H.t());

  // Divide by the third homogeneous coordinates to get the cartersian coordinates.
  for (int j=0; j<3; j++)
  {
    cv::Mat col     = homogeneous_new_coords.col(j).mul(1.0 / homogeneous_new_coords.col(2));
    cv::Mat col_new = homogeneous_new_coords.col(j);
    col.copyTo(col_new);
  }

  cv::Mat homogeneous_new_coords_ref = homogeneous_new_coords(cv::Range::all(), cv::Range(0, 2));
  cv::Mat new_coords;
  homogeneous_new_coords_ref.copyTo(new_coords);

#ifdef DEBUG
  // write Mat objects to the file
  cv::FileStorage fs("transformCoordsToImage.xml", cv::FileStorage::WRITE);
  fs << "H" << H;
  fs << "coords" << coords;
  fs << "new_coords" << new_coords;
  fs.release();
#endif

  return new_coords;
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
cv::Mat
Homography2D::warpImage
  (
  cv::Mat image,
  cv::Mat params,
  cv::Mat template_coords,
  std::vector<int>& template_ctrl_points_indices
  )
{
  MAT_TYPE min_x, max_x, min_y, max_y;
  cv::Mat warped_image;

  cv::Mat M;
  cv::Mat H = params.reshape(1,3).t();

  // Find minimum x and minimum y in template coords
  cv::MatConstIterator_<MAT_TYPE> it;
  max_x = std::numeric_limits<MAT_TYPE>::min();
  max_y = std::numeric_limits<MAT_TYPE>::min();
  min_x = std::numeric_limits<MAT_TYPE>::max();
  min_y = std::numeric_limits<MAT_TYPE>::max();

  for (int i = 0; i < template_coords.rows; i++)
  {
    MAT_TYPE x = template_coords.at<MAT_TYPE>(i,0);
    MAT_TYPE y = template_coords.at<MAT_TYPE>(i,1);

    if (x > max_x) max_x = x;
    if (x < min_x) min_x = x;
    if (y > max_y) max_y = y;
    if (y < min_y) min_y = y;
  }

  cv::Mat TR  = (cv::Mat_<MAT_TYPE>(3,3) << 1.,    0,    min_x,
		                            0,    1.,    min_y,
		                            0,     0,    1.);
  warped_image = cv::Mat::zeros(max_y-min_y+1, max_x-min_x+1, cv::DataType<uint8_t>::type);

//   if (scale < 0.000000001)
//   {
//     return warped_image;
//   }

  // TR is necessary because the Warpers do warping taking
  // the pixel (0,0) as the left and top most pixel of the template.
  // So, we have move the (0,0) to the center of the Template.
  M = H*TR;

#ifdef DEBUG
  // write Mat objects to the file
  cv::FileStorage fs("template_coords_warpImage.xml", cv::FileStorage::WRITE);
  fs << "template_coords" << template_coords;
  fs << "M" << M;
  fs.release();
#endif

  cv::warpPerspective(image, warped_image, M,
		      cv::Size(warped_image.cols,  warped_image.rows),
		      cv::INTER_AREA | cv::WARP_INVERSE_MAP);
  return warped_image;
};

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
bool
Homography2D::consistentPoints
  (
  cv::Mat points,
  cv::Mat transformed_points,
  int t1,
  int t2,
  int t3
  )
{
  cv::Mat A = (cv::Mat_<MAT_TYPE>(3, 3) << points.at<MAT_TYPE>(t1, 0), points.at<MAT_TYPE>(t1, 1), 1,
                                           points.at<MAT_TYPE>(t2, 0), points.at<MAT_TYPE>(t2, 1), 1,
                                           points.at<MAT_TYPE>(t3, 0), points.at<MAT_TYPE>(t3, 1), 1);
  cv::Mat B = (cv::Mat_<MAT_TYPE>(3, 3) << transformed_points.at<MAT_TYPE>(t1, 0), transformed_points.at<MAT_TYPE>(t1, 1), 1,
                                           transformed_points.at<MAT_TYPE>(t2, 0), transformed_points.at<MAT_TYPE>(t2, 1), 1,
                                           transformed_points.at<MAT_TYPE>(t3, 0), transformed_points.at<MAT_TYPE>(t3, 1), 1);

  double detA = cv::determinant(A);
  double detB = cv::determinant(B);

  return ((detA*detB) >= 0.0);
}

// -----------------------------------------------------------------------------
//
// Purpose and Method:
// Inputs:
// Outputs:
// Dependencies:
// Restrictions and Caveats:
//
// -----------------------------------------------------------------------------
bool
Homography2D::invalidParams
  (
  cv::Mat params
  )
{
  cv::Mat M;
  cv::Mat H = params.reshape(1,3).t();

  cv::SVD svd(H, cv::SVD::NO_UV);

  int rank = 0;
  for (int i=0; i<H.rows; i++)
  {
    if (svd.w.at<MAT_TYPE>(i,0)>1.E-6)
    {
      rank++;
    }
  }

  SHOW_VALUE(H);

  // Stablish the
  cv::Mat points = (cv::Mat_<MAT_TYPE>(4, 2) <<   0, 0,
                                                100, 0,
		                                100, 100,
		                                  0, 100);
  cv::Mat transformed_points;
  points.copyTo(transformed_points);
//   cv::perspectiveTransform(points, transformed_points, H);
  transformed_points = transformCoordsToImage(points, params);

  double detH = cv::determinant(H);

//   return (rank<3) || (!consistentPoints(points, transformed_points, 0, 1, 2))
//                   || (!consistentPoints(points, transformed_points, 1, 2, 3))
//                   || (!consistentPoints(points, transformed_points, 0, 2, 3))
//                   || (!consistentPoints(points, transformed_points, 0, 1, 3));
  return (rank<3) ||(detH < (1./10.)) || (detH > 10.)
                  || (!consistentPoints(points, transformed_points, 0, 1, 2))
                  || (!consistentPoints(points, transformed_points, 1, 2, 3))
                  || (!consistentPoints(points, transformed_points, 0, 2, 3))
                  || (!consistentPoints(points, transformed_points, 0, 1, 3));
};

}; }; // namespace


#include <boost/iterator/counting_iterator.hpp>
#include <algorithm>

int factorial(int n)
{
  // last is one-past-end
  return std::accumulate(boost::counting_iterator<int>(1), boost::counting_iterator<int>(n+1), 1, std::multiplies<int>());
}


/*=============================================================================
Copyright 2019 Sarthak Singhal <singhalsarthak2007@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#define BOOST_TEST_MODULE spherical_equatorial_differential_test

#include <boost/test/unit_test.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/astronomy/coordinate/arithmetic.hpp>
#include <boost/astronomy/coordinate/diff/differential.hpp>

using namespace std;
using namespace boost::astronomy::coordinate;
using namespace boost::units::si;
using namespace boost::geometry;
using namespace boost::units;
namespace bud = boost::units::degree;

BOOST_AUTO_TEST_SUITE(spherical_equatorial_differential_constructors)

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_default_constructor)
{
    //using set functions
    spherical_equatorial_differential<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::velocity>> motion1;
    motion1.set_dlat_dlon_ddist(45.0 * bud::degrees, 18.0 * bud::degrees, 3.5 * meters/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), 45.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), 18.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), 3.5, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_quantities_constructor)
{
    //checking construction from value
    auto motion1 = make_spherical_equatorial_differential
    (15.0 * bud::degrees, 39.0 * bud::degrees, 3.0*si::centi*meter/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), 39.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), 3.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_ddist()),
        quantity<bu::divide_typeof_helper<decltype(si::centi*meters), si::time>::type>>::value));

    spherical_equatorial_differential<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::velocity>> motion2(1.5 * bud::degrees, 9.0 * bud::degrees, 3.0 * meter/seconds);
    BOOST_CHECK_CLOSE(motion2.get_dlat().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dlon().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_ddist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_copy_constructor)
{
    //checking construction from value
    auto motion1 = make_spherical_equatorial_differential
    (15.0 * bud::degrees, 30.0 * bud::degrees, 3.0*si::centi*meter/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), 30.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_ddist()),
        quantity<bu::divide_typeof_helper<decltype(si::centi*meters), si::time>::type>>::value));

    //copy constructor
    auto motion2 = make_spherical_equatorial_differential(motion1);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), motion2.get_dlat().value(), 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), motion2.get_dlon().value(), 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), motion2.get_ddist().value(), 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_ddist()),
        quantity<bu::divide_typeof_helper<decltype(si::centi*meters), si::time>::type>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_copy_constructor_with_different_units)
{
    //checking construction from value
    auto motion1 = make_spherical_equatorial_differential
    (15.0 * bud::degrees, 10.0 * bud::degrees, 3.0*si::centi*meter/seconds);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), 10.0, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_ddist()),
        quantity<bu::divide_typeof_helper<decltype(si::centi*meters), si::time>::type>>::value));

    //Conversion from one unit type to other
    auto motion2 = make_spherical_equatorial_differential
    <double, quantity<bud::plane_angle>, quantity<bud::plane_angle>, quantity<si::velocity>>(motion1);
    BOOST_CHECK_CLOSE(motion2.get_dlat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dlon().value(), 10.0, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_ddist().value(), 0.03, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_geometry_point_constructor)
{
    //constructing from boost::geometry::model::motion
    model::point<double, 3, cs::cartesian> model_point(30, 60, 10);
    auto motion1 = make_spherical_equatorial_differential
    <double,quantity<bud::plane_angle>,quantity<bud::plane_angle>,quantity<si::velocity>>(model_point);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), 63.434948822922, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), 8.47871315, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), 67.823299831253, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_ddist()), quantity<si::velocity>>::value));

    spherical_equatorial_differential<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::velocity>> motion2(model_point);
    BOOST_CHECK_CLOSE(motion2.get_dlat().value(), 63.434948822922, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dlon().value(), 8.47871315, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_ddist().value(), 67.823299831253, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_conversion_from_cartesian_differential)
{
    //constructing from spherical differential
    auto cartesian_motion = make_cartesian_differential(20.0 * meters/seconds,
        60.0 * meters/seconds, 1.0 * meter/seconds);
    auto motion1 = make_spherical_equatorial_differential(cartesian_motion);
    BOOST_CHECK_CLOSE(motion1.get_dlat().value(), 1.2490457723983, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_dlon().value(), 0.0158100741, 0.001);
    BOOST_CHECK_CLOSE(motion1.get_ddist().value(), 63.253458403474, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion1.get_dlat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_dlon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion1.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_conversion_from_spherical_differential)
{
    //constructing from spherical_equitorial differential
    auto spherical_motion = make_spherical_differential
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meter/seconds);
    auto motion2 = make_spherical_equatorial_differential(spherical_motion);
    BOOST_CHECK_CLOSE(motion2.get_dlat().value(), 0.523599, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_dlon().value(), 0.523598776, 0.001);
    BOOST_CHECK_CLOSE(motion2.get_ddist().value(), 1.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion2.get_dlat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_dlon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion2.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_conversion_from_spherical_coslat_differential)
{
    //constructing from spherical_coslat differential
    auto spherical_coslat_motion = make_spherical_coslat_differential
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meters/seconds);
    auto motion3 = make_spherical_equatorial_differential(spherical_coslat_motion);
    BOOST_CHECK_CLOSE(motion3.get_dlat().value(), 0.523599, 0.001);
    BOOST_CHECK_CLOSE(motion3.get_dlon().value(), 0.36159733, 0.001);
    BOOST_CHECK_CLOSE(motion3.get_ddist().value(), 1, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(motion3.get_dlat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion3.get_dlon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(motion3.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(spherical_equatorial_differential_operators)

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_addition_operator)
{
    auto motion1 = make_spherical_equatorial_differential(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters/seconds);
    auto motion2 = make_spherical_equatorial_differential(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meters/seconds);

    auto sum = make_spherical_equatorial_differential(motion1 + motion2);

    BOOST_CHECK_CLOSE(sum.get_dlat().value(), 24.3128259, 0.001);
    BOOST_CHECK_CLOSE(sum.get_dlon().value(), 40.2412189, 0.001);
    BOOST_CHECK_CLOSE(sum.get_ddist().value(), 29.6314681, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(sum.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_ddist()), quantity<si::velocity>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_differential_multiplication_operator)
{
    auto motion1 = make_spherical_equatorial_differential(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters/seconds);

    spherical_equatorial_differential<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> product = make_spherical_equatorial_differential(motion1 * quantity<si::time>(5.0*seconds));

    BOOST_CHECK_CLOSE(product.get_dlat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(product.get_dlon().value(), 30.0, 0.001);
    BOOST_CHECK_CLOSE(product.get_ddist().value(), 50.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(product.get_dlat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(product.get_dlon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(product.get_ddist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2017, Waterloo Autonomous Vehicles Laboratory (WAVELab),
 * Waterloo Intelligent Systems Engineering Lab (WISELab),
 * University of Waterloo.
 *
 * Refer to the accompanying LICENSE file for license information.
 *
 * ############################################################################
 ******************************************************************************
 |                                                                            |
 |                         /\/\__/\_/\      /\_/\__/\/\                       |
 |                         \          \____/          /                       |
 |                          '----________________----'                        |
 |                              /                \                            |
 |                            O/_____/_______/____\O                          |
 |                            /____________________\                          |
 |                           /    (#UNIVERSITY#)    \                         |
 |                           |[**](#OFWATERLOO#)[**]|                         |
 |                           \______________________/                         |
 |                            |_""__|_,----,_|__""_|                          |
 |                            ! !                ! !                          |
 |                            '-'                '-'                          |
 |       __    _   _  _____  ___  __  _  ___  _    _  ___  ___   ____  ____   |
 |      /  \  | | | ||_   _|/ _ \|  \| |/ _ \| \  / |/ _ \/ _ \ /     |       |
 |     / /\ \ | |_| |  | |  ||_||| |\  |||_|||  \/  |||_||||_|| \===\ |====   |
 |    /_/  \_\|_____|  |_|  \___/|_| \_|\___/|_|\/|_|\___/\___/ ____/ |____   |
 |                                                                            |
 ******************************************************************************
 * ############################################################################
 *
 * File: world_frame_conversions.cpp
 * Desc: Implementation file for world frame conversion functions
 * Auth: Michael Smart <michael.smart@uwaterloo.ca>
 *
 * ############################################################################
*/

#include <Eigen/Core>
#include "wave/geography/world_frame_conversions.hpp"

namespace wave {

void ecefPointFromLLH(const double llh[3], double ecef[3]) {
    double latitude = llh[0], longitude = llh[1], height = llh[2];

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();

    double X, Y, Z;
    earth.Forward(latitude, longitude, height, X, Y, Z);

    ecef[0] = X;
    ecef[1] = Y;
    ecef[2] = Z;
}

void llhPointFromECEF(const double ecef[3], double llh[3]) {
    double X = ecef[0], Y = ecef[1], Z = ecef[2];

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();

    double latitude, longitude, height;
    earth.Reverse(X, Y, Z, latitude, longitude, height);

    llh[0] = latitude;
    llh[1] = longitude;
    llh[2] = height;
}

void ecefFromENUTransformMatrix(const double datum[3],
                                double T_ecef_enu[4][4],
                                bool datum_is_llh) {
    // Both Forward() and Reverse() return the same rotation matrix from ENU
    // to ECEF
    std::vector<double> R_ecef_enu(9, 0.0);
    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
    double datum_X, datum_Y, datum_Z;

    if (datum_is_llh) {
        double latitude = datum[0], longitude = datum[1], height = datum[2];

        earth.Forward(
          latitude, longitude, height, datum_X, datum_Y, datum_Z, R_ecef_enu);
    } else {
        // Datum is already given in ECEF
        datum_X = datum[0];
        datum_Y = datum[1];
        datum_Z = datum[2];

        double latitude, longitude, height;
        earth.Reverse(
          datum_X, datum_Y, datum_Z, latitude, longitude, height, R_ecef_enu);
    }

    T_ecef_enu[0][0] = R_ecef_enu[0];
    T_ecef_enu[0][1] = R_ecef_enu[1];
    T_ecef_enu[0][2] = R_ecef_enu[2];
    T_ecef_enu[0][3] = datum_X;
    T_ecef_enu[1][0] = R_ecef_enu[3];
    T_ecef_enu[1][1] = R_ecef_enu[4];
    T_ecef_enu[1][2] = R_ecef_enu[5];
    T_ecef_enu[1][3] = datum_Y;
    T_ecef_enu[2][0] = R_ecef_enu[6];
    T_ecef_enu[2][1] = R_ecef_enu[7];
    T_ecef_enu[2][2] = R_ecef_enu[8];
    T_ecef_enu[2][3] = datum_Z;
    T_ecef_enu[3][0] = 0.0;
    T_ecef_enu[3][1] = 0.0;
    T_ecef_enu[3][2] = 0.0;
    T_ecef_enu[3][3] = 1.0;
}

void enuFromECEFTransformMatrix(const double datum[3],
                                double T_enu_ecef[4][4],
                                bool datum_is_llh) {
    // Get T_ecef_enu and then invert it
    double T_ecef_enu[4][4];
    ecefFromENUTransformMatrix(datum, T_ecef_enu, datum_is_llh);

    // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t]
    // TODO(msmart/benskikos) - Move these functions to somewhere where we have
    // matrix classes and ownership since it is ours. Manual transposition is
    // undesirable. The below should be 2 lines:
    // R_new = R.transpose(); t_new = - R.transpose()*t;
    T_enu_ecef[0][0] = T_ecef_enu[0][0];
    T_enu_ecef[0][1] = T_ecef_enu[1][0];
    T_enu_ecef[0][2] = T_ecef_enu[2][0];
    T_enu_ecef[1][0] = T_ecef_enu[0][1];
    T_enu_ecef[1][1] = T_ecef_enu[1][1];
    T_enu_ecef[1][2] = T_ecef_enu[2][1];
    T_enu_ecef[2][0] = T_ecef_enu[0][2];
    T_enu_ecef[2][1] = T_ecef_enu[1][2];
    T_enu_ecef[2][2] = T_ecef_enu[2][2];

    // Affine inverse translation component: -R_inverse * b
    //    with b as the 4th column of T_ecef_enu
    T_enu_ecef[0][3] = -T_enu_ecef[0][0] * T_ecef_enu[0][3]    //
                       - T_enu_ecef[0][1] * T_ecef_enu[1][3]   //
                       - T_enu_ecef[0][2] * T_ecef_enu[2][3];  //

    T_enu_ecef[1][3] = -T_enu_ecef[1][0] * T_ecef_enu[0][3]    //
                       - T_enu_ecef[1][1] * T_ecef_enu[1][3]   //
                       - T_enu_ecef[1][2] * T_ecef_enu[2][3];  //

    T_enu_ecef[2][3] = -T_enu_ecef[2][0] * T_ecef_enu[0][3]    //
                       - T_enu_ecef[2][1] * T_ecef_enu[1][3]   //
                       - T_enu_ecef[2][2] * T_ecef_enu[2][3];  //

    // Last row is the same
    T_enu_ecef[3][0] = 0.0;
    T_enu_ecef[3][1] = 0.0;
    T_enu_ecef[3][2] = 0.0;
    T_enu_ecef[3][3] = 1.0;
}

void enuPointFromLLH(const double point_llh[3],
                     const double enu_datum[3],
                     double point_enu[3],
                     bool datum_is_llh) {
    double enu_datum_llh[3];
    if (datum_is_llh) {
        enu_datum_llh[0] = enu_datum[0];
        enu_datum_llh[1] = enu_datum[1];
        enu_datum_llh[2] = enu_datum[2];
    } else {
        // Datum is ECEF
        llhPointFromECEF(enu_datum, enu_datum_llh);
    }

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
    GeographicLib::LocalCartesian localENU(
      enu_datum_llh[0], enu_datum_llh[1], enu_datum_llh[2], earth);

    localENU.Forward(point_llh[0],
                     point_llh[1],
                     point_llh[2],
                     point_enu[0],
                     point_enu[1],
                     point_enu[2]);
}

void llhPointFromENU(const double point_enu[3],
                     const double enu_datum[3],
                     double point_llh[3],
                     bool datum_is_llh) {
    double enu_datum_llh[3];
    if (datum_is_llh) {
        enu_datum_llh[0] = enu_datum[0];
        enu_datum_llh[1] = enu_datum[1];
        enu_datum_llh[2] = enu_datum[2];
    } else {
        // Datum is ECEF
        llhPointFromECEF(enu_datum, enu_datum_llh);
    }

    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
    GeographicLib::LocalCartesian localENU(
      enu_datum_llh[0], enu_datum_llh[1], enu_datum_llh[2], earth);

    localENU.Reverse(point_enu[0],
                     point_enu[1],
                     point_enu[2],
                     point_llh[0],
                     point_llh[1],
                     point_llh[2]);
}

}  // namespace wave


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_CORRECT_FMA_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_CORRECT_FMA_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/simd/function/scalar/exponent.hpp>
#include <boost/simd/function/scalar/ldexp.hpp>
#include <boost/simd/function/scalar/max.hpp>
#include <boost/simd/function/scalar/multiplies.hpp>
#include <boost/simd/function/scalar/sign.hpp>
#include <boost/simd/function/scalar/two_add.hpp>
#include <boost/simd/function/scalar/two_prod.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( correct_fma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          , bd::scalar_< bd::single_<A0> >
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1, A0 a2) const BOOST_NOEXCEPT
    {
      return static_cast<A0>( static_cast<double>(a0)*static_cast<double>(a1)
                              + static_cast<double>(a2)
                            );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( correct_fma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1, A0 a2) const BOOST_NOEXCEPT
    {
      A0 p, rp, s, rs;
    #ifndef BOOST_SIMD_DONT_CARE_FMA_OVERFLOW
      using iA0 = bd::as_integer_t<A0>;
      iA0 e0 = exponent(a0);
      iA0 e1 = exponent(a1);
      iA0 e = -bs::max(e0, e1)/2;
      A0 ae2  = ldexp(a2, e);
      auto choose = (e0 > e1);
      A0 amax = choose ? ldexp(a0, e) : ldexp(a1, e);
      A0 amin = choose ? a1 : a0;
      two_prod(amax, amin, p, rp);
      two_add(p, ae2, s, rs);
      return ldexp(s+(rp+rs), -e);
    #else
      two_prod(a0, a1, p, rp);
      two_add(p, a2, s, rs);
      return s+(rp+rs);
    #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( correct_fma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1, A0 a2) const BOOST_NOEXCEPT
    {
      // correct fma has to ensure "no intermediate overflow".
      // This is done in the case of signed integers by transtyping to unsigned type
      // to perform the computations in a guaranted 2-complement environment
      // since signed integer overflows in C++ produce "undefined results"
      using u_t = bd::as_integer_t<A0, unsigned>;
      return A0(correct_fma(u_t(a0), u_t(a1), u_t(a2)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( correct_fma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::uint_<A0> >
                          , bd::scalar_< bd::uint_<A0> >
                          , bd::scalar_< bd::uint_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1, A0 a2) const BOOST_NOEXCEPT
    {
      return multiplies(a0, a1)+a2;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( correct_fma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          , boost::simd::std_tag
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1, A0 a2, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::fma(a0, a1, a2);
    }
  };
} } }

#endif


// Copyright (c) 2010 Satoshi Nakamoto
// Copyright (c) 2009-2012 The Bitcoin developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "main.h"
#include "bitcoinrpc.h"
#include "init.h"
#include <boost/filesystem.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/stream.hpp>
#include <ostream>

double CRPCTable::GetDifficulty(const CBlockIndex *blockindex/* = NULL */)
{
    //
    // Floating point number that is a multiple of the minimum difficulty,
    // minimum difficulty = 1.0.
    //
    if (blockindex == NULL) {
        if (block_info::pindexBest == NULL) {
            return 1.0;
        } else {
            blockindex = diff::spacing::GetLastBlockIndex(block_info::pindexBest, false);
        }
    }

    int nShift = (blockindex->nBits >> 24) & 0xff;

    double dDiff = (double)0x0000ffff / (double)(blockindex->nBits & 0x00ffffff);

    while (nShift < 29)
    {
        dDiff *= 256.0;
        nShift++;
    }
    while (nShift > 29)
    {
        dDiff /= 256.0;
        nShift--;
    }

    return dDiff;
}

double CRPCTable::GetPoWMHashPS()
{
    int nPoWInterval = 72;
    int64_t nTargetSpacingWorkMin = 30, nTargetSpacingWork = 30;

    CBlockIndex *pindex = block_info::pindexGenesisBlock;
    CBlockIndex *pindexPrevWork = block_info::pindexGenesisBlock;

    while (pindex)
    {
        if (pindex->IsProofOfWork()) {
            int64_t nActualSpacingWork = pindex->GetBlockTime() - pindexPrevWork->GetBlockTime();
            nTargetSpacingWork = ((nPoWInterval - 1) * nTargetSpacingWork + nActualSpacingWork + nActualSpacingWork) / (nPoWInterval + 1);
            nTargetSpacingWork = std::max(nTargetSpacingWork, nTargetSpacingWorkMin);
            pindexPrevWork = pindex;
        }

        pindex = pindex->pnext;
    }

    return GetDifficulty() * 4294.967296 / nTargetSpacingWork;
}

double CRPCTable::GetPoSKernelPS()
{
    int nPoSInterval = 72;
    double dStakeKernelsTriedAvg = 0;
    int nStakesHandled = 0, nStakesTime = 0;

    CBlockIndex *pindex = block_info::pindexBest;
    CBlockIndex *pindexPrevStake = NULL;

    while (pindex && nStakesHandled < nPoSInterval)
    {
        if (pindex->IsProofOfStake()) {
            dStakeKernelsTriedAvg += GetDifficulty(pindex) * 4294967296.0;
            nStakesTime += pindexPrevStake ? (pindexPrevStake->nTime - pindex->nTime) : 0;
            pindexPrevStake = pindex;
            nStakesHandled++;
        }

        pindex = pindex->pprev;
    }

    if (! nStakesHandled) {
        return 0;
    }

    return dStakeKernelsTriedAvg / nStakesTime;
}

json_spirit::Object CRPCTable::blockToJSON(const CBlock &block, const CBlockIndex *blockindex, bool fPrintTransactionDetail)
{
    json_spirit::Object result;

    result.push_back(json_spirit::Pair("hash", block.GetHash().GetHex()));

    CMerkleTx txGen(block.vtx[0]);
    txGen.SetMerkleBranch(&block);

    result.push_back(json_spirit::Pair("confirmations", (int)txGen.GetDepthInMainChain()));
    result.push_back(json_spirit::Pair("size", (int)::GetSerializeSize(block, SER_NETWORK, version::PROTOCOL_VERSION)));
    result.push_back(json_spirit::Pair("height", blockindex->nHeight));
    result.push_back(json_spirit::Pair("version", block.nVersion));
    result.push_back(json_spirit::Pair("merkleroot", block.hashMerkleRoot.GetHex()));
    result.push_back(json_spirit::Pair("mint", ValueFromAmount(blockindex->nMint)));
    result.push_back(json_spirit::Pair("time", (int64_t)block.GetBlockTime()));
    result.push_back(json_spirit::Pair("nonce", (uint64_t)block.nNonce));
    result.push_back(json_spirit::Pair("bits", HexBits(block.nBits)));
    result.push_back(json_spirit::Pair("difficulty", GetDifficulty(blockindex)));
    result.push_back(json_spirit::Pair("blocktrust", util::leftTrim(blockindex->GetBlockTrust().GetHex(), '0')));
    result.push_back(json_spirit::Pair("chaintrust", util::leftTrim(blockindex->nChainTrust.GetHex(), '0')));
    if (blockindex->pprev) {
        result.push_back(json_spirit::Pair("previousblockhash", blockindex->pprev->GetBlockHash().GetHex()));
    }
    if (blockindex->pnext) {
        result.push_back(json_spirit::Pair("nextblockhash", blockindex->pnext->GetBlockHash().GetHex()));
    }

    result.push_back(json_spirit::Pair("flags", strprintf("%s%s", blockindex->IsProofOfStake()? "proof-of-stake" : "proof-of-work", blockindex->GeneratedStakeModifier()? " stake-modifier": "")));
    result.push_back(json_spirit::Pair("proofhash", blockindex->IsProofOfStake()? blockindex->hashProofOfStake.GetHex() : blockindex->GetBlockHash().GetHex()));
    result.push_back(json_spirit::Pair("entropybit", (int)blockindex->GetStakeEntropyBit()));
    result.push_back(json_spirit::Pair("modifier", strprintf("%016" PRIx64, blockindex->nStakeModifier)));
    result.push_back(json_spirit::Pair("modifierchecksum", strprintf("%08x", blockindex->nStakeModifierChecksum)));

    json_spirit::Array txinfo;
    BOOST_FOREACH (const CTransaction &tx, block.vtx)
    {
        if (fPrintTransactionDetail) {
            CDataStream ssTx(SER_NETWORK, version::PROTOCOL_VERSION);
            ssTx << tx;
            std::string strHex = util::HexStr(ssTx.begin(), ssTx.end());

            txinfo.push_back(strHex);
        } else {
            txinfo.push_back(tx.GetHash().GetHex());
        }
    }

    result.push_back(json_spirit::Pair("tx", txinfo));

    if ( block.IsProofOfStake() ) {
        result.push_back(json_spirit::Pair("signature", util::HexStr(block.vchBlockSig.begin(), block.vchBlockSig.end())));
    }

    return result;
}

json_spirit::Value CRPCTable::getbestblockhash(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() != 0) {
        throw std::runtime_error(
            "getbestblockhash\n"
            "Returns the hash of the best block in the longest block chain.");
    }

    return block_info::hashBestChain.GetHex();
}

json_spirit::Value CRPCTable::getblockcount(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() != 0) {
        throw std::runtime_error(
            "getblockcount\n"
            "Returns the number of blocks in the longest block chain.");
    }

    return block_info::nBestHeight;
}

json_spirit::Value CRPCTable::getdifficulty(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() != 0) {
        throw std::runtime_error(
            "getdifficulty\n"
            "Returns the difficulty as a multiple of the minimum difficulty.");
    }

    json_spirit::Object obj;
    obj.push_back(json_spirit::Pair("proof-of-work", GetDifficulty()));
    obj.push_back(json_spirit::Pair("proof-of-stake", GetDifficulty(diff::spacing::GetLastBlockIndex(block_info::pindexBest, true))));
    obj.push_back(json_spirit::Pair("search-interval", (int)block_info::nLastCoinStakeSearchInterval));
    return obj;
}

json_spirit::Value CRPCTable::settxfee(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 1 || AmountFromValue(params[0]) < block_param::MIN_TX_FEE) {
        throw std::runtime_error(
            "settxfee <amount>\n"
            "<amount> is a real and is rounded to the nearest " + bitstr::FormatMoney(block_param::MIN_TX_FEE));
    }

    block_info::nTransactionFee = AmountFromValue(params[0]);
    block_info::nTransactionFee = (block_info::nTransactionFee / block_param::MIN_TX_FEE) * block_param::MIN_TX_FEE;  // round to minimum fee
    return true;
}

json_spirit::Value CRPCTable::getrawmempool(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() != 0) {
        throw std::runtime_error(
            "getrawmempool\n"
            "Returns all transaction ids in memory pool.");
    }

    std::vector<uint256> vtxid;
    CTxMemPool::mempool.queryHashes(vtxid);

    json_spirit::Array a;
    BOOST_FOREACH(const uint256 &hash, vtxid)
    {
        a.push_back(hash.ToString());
    }

    return a;
}

json_spirit::Value CRPCTable::getblockhash(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() != 1) {
        throw std::runtime_error(
            "getblockhash <index>\n"
            "Returns hash of block in best-block-chain at <index>.");
    }

    int nHeight = params[0].get_int();
    if (nHeight < 0 || nHeight > block_info::nBestHeight) {
        throw std::runtime_error("Block number out of range.");
    }

    CBlockIndex *pblockindex = block_transaction::manage::FindBlockByHeight(nHeight);
    return pblockindex->phashBlock->GetHex();
}

json_spirit::Value CRPCTable::getblock(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2) {
        throw std::runtime_error(
            "getblock <hash> [txinfo]\n"
            "txinfo optional to print more detailed tx info\n"
            "Returns details of a block with given block-hash.");
    }

    std::string strHash = params[0].get_str();
    uint256 hash(strHash);

    if (block_info::mapBlockIndex.count(hash) == 0) {
        throw bitjson::JSONRPCError(RPC_INVALID_ADDRESS_OR_KEY, "Block not found");
    }

    CBlock block;
    CBlockIndex *pblockindex = block_info::mapBlockIndex[hash];
    block.ReadFromDisk(pblockindex, true);

    return blockToJSON(block, pblockindex, params.size() > 1 ? params[1].get_bool() : false);
}

json_spirit::Value CRPCTable::getblockbynumber(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2) {
        throw std::runtime_error(
            "getblockbynumber <number> [txinfo]\n"
            "txinfo optional to print more detailed tx info\n"
            "Returns details of a block with given block-number.");
    }

    int nHeight = params[0].get_int();
    if (nHeight < 0 || nHeight > block_info::nBestHeight) {
        throw std::runtime_error("Block number out of range.");
    }

    CBlock block;
    CBlockIndex *pblockindex = block_info::mapBlockIndex[block_info::hashBestChain];
    while (pblockindex->nHeight > nHeight)
    {
        pblockindex = pblockindex->pprev;
    }

    pblockindex = block_info::mapBlockIndex[*pblockindex->phashBlock];
    block.ReadFromDisk(pblockindex, true);

    return blockToJSON(block, pblockindex, params.size() > 1 ? params[1].get_bool() : false);
}

bool CRPCTable::ExportBlock(const std::string &strBlockHash, const CDataStream &ssBlock)
{
    boost::filesystem::path pathDest = iofs::GetDataDir() / strBlockHash;
    if (boost::filesystem::is_directory(pathDest)) {
        pathDest /= strBlockHash;
    }

    try {
        boost::iostreams::stream_buffer<boost::iostreams::file_sink> buf(pathDest.string());
        std::ostream exportStream(&buf);
        exportStream << util::HexStr(ssBlock.begin(), ssBlock.end());
        exportStream.flush();

        printf("Successfully exported block to %s\n", pathDest.string().c_str());
        return true;
    } catch(const boost::filesystem::filesystem_error &e) {
        printf("error exporting the block data %s (%s)\n", pathDest.string().c_str(), e.what());
        return false;
    }
}

json_spirit::Value CRPCTable::dumpblock(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2) {
        throw std::runtime_error(
            "dumpblock <hash> [destination]\n"
            "Returns serialized contents of a block with given block-hash.");
    }

    std::string strHash = params[0].get_str();
    uint256 hash(strHash);

    if (block_info::mapBlockIndex.count(hash) == 0) {
        throw bitjson::JSONRPCError(RPC_INVALID_ADDRESS_OR_KEY, "Block not found");
    }

    CBlock block;
    CBlockIndex *pblockindex = block_info::mapBlockIndex[hash];
    block.ReadFromDisk(pblockindex, true);

    CDataStream ssBlock(SER_NETWORK, version::PROTOCOL_VERSION);
    ssBlock << block;

    if (params.size() > 1) {
        return ExportBlock(params[1].get_str(), ssBlock);
    }

    return util::HexStr(ssBlock.begin(), ssBlock.end());
}

json_spirit::Value CRPCTable::dumpblockbynumber(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() < 1 || params.size() > 2) {
        throw std::runtime_error(
            "dumpblockbynumber <number>  [destination]\n"
            "Returns serialized contents of a block with given block-number.");
    }

    int nHeight = params[0].get_int();
    if (nHeight < 0 || nHeight > block_info::nBestHeight) {
        throw std::runtime_error("Block number out of range.");
    }

    CBlock block;
    CBlockIndex *pblockindex = block_info::mapBlockIndex[block_info::hashBestChain];
    while (pblockindex->nHeight > nHeight)
    {
        pblockindex = pblockindex->pprev;
    }

    pblockindex = block_info::mapBlockIndex[*pblockindex->phashBlock];
    block.ReadFromDisk(pblockindex, true);

    CDataStream ssBlock(SER_NETWORK, version::PROTOCOL_VERSION);
    ssBlock << block;

    if (params.size() > 1) {
        return ExportBlock(params[1].get_str(), ssBlock);
    }

    return util::HexStr(ssBlock.begin(), ssBlock.end());
}

//
// get information of sync-checkpoint
//
json_spirit::Value CRPCTable::getcheckpoint(const json_spirit::Array &params, bool fHelp)
{
    if (fHelp || params.size() != 0) {
        throw std::runtime_error(
            "getcheckpoint\n"
            "Show info of synchronized checkpoint.\n");
    }

    json_spirit::Object result;
    result.push_back(json_spirit::Pair("synccheckpoint", Checkpoints::manage::getHashSyncCheckpoint().ToString().c_str()));

    CBlockIndex *pindexCheckpoint = block_info::mapBlockIndex[Checkpoints::manage::getHashSyncCheckpoint()];
    result.push_back(json_spirit::Pair("height", pindexCheckpoint->nHeight));
    result.push_back(json_spirit::Pair("timestamp", util::DateTimeStrFormat(pindexCheckpoint->GetBlockTime()).c_str()));

    if (Checkpoints::checkpointMessage.vchSig.size() != 0) {
        json_spirit::Object msgdata;
        CUnsignedSyncCheckpoint checkpoint;

        CDataStream sMsg(Checkpoints::checkpointMessage.vchMsg, SER_NETWORK, version::PROTOCOL_VERSION);
        sMsg >> checkpoint;

        json_spirit::Object parsed; // message version and data (block hash)
        parsed.push_back(json_spirit::Pair("version", checkpoint.nVersion));
        parsed.push_back(json_spirit::Pair("hash", checkpoint.hashCheckpoint.GetHex().c_str()));
        msgdata.push_back(json_spirit::Pair("parsed", parsed));

        json_spirit::Object raw; // raw checkpoint message data
        raw.push_back(json_spirit::Pair("data", util::HexStr(Checkpoints::checkpointMessage.vchMsg).c_str()));
        raw.push_back(json_spirit::Pair("signature", util::HexStr(Checkpoints::checkpointMessage.vchSig).c_str()));
        msgdata.push_back(json_spirit::Pair("raw", raw));

        result.push_back(json_spirit::Pair("data", msgdata));
    }

    // Check that the block satisfies synchronized checkpoint
    if (entry::CheckpointsMode == Checkpoints::STRICT) {
        result.push_back(json_spirit::Pair("policy", "strict"));
    }
    if (entry::CheckpointsMode == Checkpoints::ADVISORY) {
        result.push_back(json_spirit::Pair("policy", "advisory"));
    }
    if (entry::CheckpointsMode == Checkpoints::PERMISSIVE) {
        result.push_back(json_spirit::Pair("policy", "permissive"));
    }
    if (map_arg::GetMapArgsCount("-checkpointkey")) {
        result.push_back(json_spirit::Pair("checkpointmaster", true));
    }

    return result;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE expression_arithmetic_test

#include <vector>
#include <cstdint>

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

#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>

#include <nil/crypto3/math/polynomial/polynom.hpp>
#include <nil/crypto3/math/expressions/expression.hpp>

using namespace nil::crypto3::algebra;
using namespace nil::crypto3::math;

typedef fields::bls12<381> FieldType;

BOOST_AUTO_TEST_SUITE(expression_test_suite)

// BOOST_AUTO_TEST_CASE(expression_field_evaluation) {

//     expressions::lazy_expression< typename FieldType::value_type > v0, v1;

//     expressions::lazy_expression< typename FieldType::value_type >  expr;
//     expr = (v1 - v0) + v1 * 100;

//     v0.assign(100);
//     v1.assign(505);

//     typename FieldType::value_type d = ((v1 - v0) + v1 * 100).evaluate();

//     BOOST_CHECK_EQUAL((505 - 100) + 505*100, d.data);
// }

BOOST_AUTO_TEST_CASE(expression_polynom_evaluation) {

    // expressions::lazy_expression< polynomial::polynom<typename FieldType::value_type> > v0, v1;

    // using expr_type = typename boost::proto::terminal<polynomial::polynom<typename FieldType::value_type>>::type;

    // expressions::lazy_expression< polynomial::polynom<typename FieldType::value_type> >  expr =
    //     expressions::lazy_expression< polynomial::polynom<typename FieldType::value_type> > (v1 + v0);

    // boost::proto::display_expr(expr);

    // v0.assign({1, 3, 4, 25, 6, 7, 7, 2});
    // v1.assign({9, 3, 11, 14, 7, 1, 5, 8});

    // polynomial::polynom<typename FieldType::value_type> c_ans = {10, 6, 15, 39, 13, 8, 12, 10};

    // polynomial::polynom<typename FieldType::value_type> d = expr.evaluate();

    // for (std::size_t i = 0; i < c_ans.size(); ++i) {
    //     BOOST_CHECK_EQUAL(c_ans[i].data, d[i].data);
    // }
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <Eigen/Dense>
#include "maths.h"
#include "graphics.h"
#include "rasterizer.h"

const int width = 800;
const int height = 800;

int main()
{
	const char* filename = "african_head.obj";

	fff::Rasterizer r(width, height);

	//r.LoadAndDrawModel(filename, fff::RenderMode::FRAME);
	r.LoadAndDrawModel(filename, fff::RenderMode::FILLED);

	r.Serialize("output.png", true);

    return 0;
}


#include "TensorTopologyEvaluator.hh"

#include <Eigen/Geometry>

#include <boost/algorithm/cxx11/any_of.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/min_element.hpp>

#include <tuple>
#include <utility>
#include <vector>

using namespace cpp_utils;

namespace tl
{
template <typename T, std::size_t... Degrees>
using TPBT = TensorProductBezierTriangle<T, double, Degrees...>;

std::array<TPBT<double, 3, 0>, 7> tensorTopologyCoeffs(const TensorInterp& t)
{
    using Coords = TPBT<double, 3, 0>::Coords;
    // Constraint functions according to Zheng et al. 2004

    auto fx = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 0) * ((tv(1, 1) * tv(1, 1) - tv(2, 2) * tv(2, 2))
                           + (tv(0, 1) * tv(0, 1) - tv(0, 2) * tv(0, 2)))
               + tv(1, 1) * ((tv(2, 2) * tv(2, 2) - tv(0, 0) * tv(0, 0))
                             + (tv(1, 2) * tv(1, 2) - tv(0, 1) * tv(0, 1)))
               + tv(2, 2) * ((tv(0, 0) * tv(0, 0) - tv(1, 1) * tv(1, 1))
                             + (tv(0, 2) * tv(0, 2) - tv(1, 2) * tv(1, 2)));
    };

    auto fy1 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(1, 2) * (2 * (tv(1, 2) * tv(1, 2) - tv(0, 0) * tv(0, 0))
                           - (tv(0, 2) * tv(0, 2) + tv(0, 1) * tv(0, 1))
                           + 2 * (tv(1, 1) * tv(0, 0) + tv(2, 2) * tv(0, 0)
                                  - tv(1, 1) * tv(2, 2)))
               + tv(0, 1) * tv(0, 2) * (2 * tv(0, 0) - tv(2, 2) - tv(1, 1));
    };

    auto fy2 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 2) * (2 * (tv(0, 2) * tv(0, 2) - tv(1, 1) * tv(1, 1))
                           - (tv(0, 1) * tv(0, 1) + tv(1, 2) * tv(1, 2))
                           + 2 * (tv(2, 2) * tv(1, 1) + tv(0, 0) * tv(1, 1)
                                  - tv(2, 2) * tv(0, 0)))
               + tv(1, 2) * tv(0, 1) * (2 * tv(1, 1) - tv(0, 0) - tv(2, 2));
    };

    auto fy3 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 1) * (2 * (tv(0, 1) * tv(0, 1) - tv(2, 2) * tv(2, 2))
                           - (tv(1, 2) * tv(1, 2) + tv(0, 2) * tv(0, 2))
                           + 2 * (tv(0, 0) * tv(2, 2) + tv(1, 1) * tv(2, 2)
                                  - tv(0, 0) * tv(1, 1)))
               + tv(0, 2) * tv(1, 2) * (2 * tv(2, 2) - tv(1, 1) - tv(0, 0));
    };

    auto fz1 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(1, 2) * (tv(0, 2) * tv(0, 2) - tv(0, 1) * tv(0, 1))
               + tv(0, 1) * tv(0, 2) * (tv(1, 1) - tv(2, 2));
    };

    auto fz2 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 2) * (tv(0, 1) * tv(0, 1) - tv(1, 2) * tv(1, 2))
               + tv(1, 2) * tv(0, 1) * (tv(2, 2) - tv(0, 0));
    };

    auto fz3 = [&](const Coords& coords) -> double {
        auto tv = t(coords.head<3>());
        return tv(0, 1) * (tv(1, 2) * tv(1, 2) - tv(0, 2) * tv(0, 2))
               + tv(0, 2) * tv(1, 2) * (tv(0, 0) - tv(1, 1));
    };

    return {TPBT<double, 3, 0>{fx},
            TPBT<double, 3, 0>{fy1},
            TPBT<double, 3, 0>{fy2},
            TPBT<double, 3, 0>{fy3},
            TPBT<double, 3, 0>{fz1},
            TPBT<double, 3, 0>{fz2},
            TPBT<double, 3, 0>{fz3}};
}

using TSHE = TensorTopologyEvaluator;

TSHE::TensorTopologyEvaluator(const DoubleTri& tri,
                              const TensorInterp& t,
                              const Options& opts)
        : _tri(tri), _target_funcs(tensorTopologyCoeffs(t)), _opts(opts)
{
}

std::array<TSHE, 4> TSHE::split() const
{
    auto part = [&](std::size_t i) {
        return TensorTopologyEvaluator(_tri.split<0>(i),
                                       {_target_funcs[0].split<0>(i),
                                        _target_funcs[1].split<0>(i),
                                        _target_funcs[2].split<0>(i),
                                        _target_funcs[3].split<0>(i),
                                        _target_funcs[4].split<0>(i),
                                        _target_funcs[5].split<0>(i),
                                        _target_funcs[6].split<0>(i)},
                                       _split_level + 1,
                                       _opts);
    };
    return {part(0), part(1), part(2), part(3)};
}

Result TSHE::eval()
{
    // Check if any of the error components can not become zero in the
    // current subdivision triangles
    auto has_nonzero =
            boost::algorithm::any_of(_target_funcs, [](const auto& c) {
                return sameSign(c.coefficients()) != 0;
            });

    // Discard triangles if no roots can occur inside
    if(has_nonzero)
    {
        return Result::Discard;
    }

    // Compute upper bound for target functions
    auto max_error = abs_max_upper_bound(_target_funcs);

    if(max_error < _opts.tolerance)
    {
        return Result::Accept;
    }

    return Result::Split;
}

double TSHE::error() const
{
    return upper_bound_norm(_target_funcs);
}

double distance(const TSHE& t1, const TSHE& t2)
{
    return distance(t1.tris(), t2.tris());
}

bool operator==(const TSHE& t1, const TSHE& t2)
{
    return t1._tri == t2._tri && t1._target_funcs == t2._target_funcs
           && t1._split_level == t2._split_level && t1._opts == t2._opts;
}

bool operator!=(const TSHE& t1, const TSHE& t2)
{
    return !(t1 == t2);
}
}


/*
 * Author: Patrick Schmidt
 */
#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Geometry/EigenVectorT.hh>

#include <HomologyInference/Utils/ExternalProperty.hh>
#include <HomologyInference/Utils/Out.hh>

namespace HomologyInference
{

// Scalar constants
constexpr double NAN_DOUBLE = std::numeric_limits<double>::quiet_NaN();
constexpr double INF_DOUBLE = std::numeric_limits<double>::infinity();
constexpr int DEFAULT_MAX_ITERS = -1;

// Complex numbers
using Complex = std::complex<double>;

// Fixed-size vectors
template <int d, typename T> using Vec = Eigen::Matrix<T, d, 1>;
template <typename T> using Vec2 = Vec<2, T>;
template <typename T> using Vec3 = Vec<3, T>;
template <typename T> using Vec4 = Vec<4, T>;
template <typename T> using Vec6 = Vec<6, T>;
template <typename T> using Vec9 = Vec<9, T>;
using Vec2d = Vec2<double>;
using Vec2i = Vec2<int>;
using Vec3d = Vec3<double>;
using Vec3i = Vec3<int>;
using Vec4f = Vec4<float>;
using Vec4d = Vec4<double>;
using Vec6d = Vec6<double>;
using Vec9d = Vec9<double>;

// Fixed-size matrices
template <int rows, int cols, typename T> using Mat = Eigen::Matrix<T, rows, cols>;
template <typename T> using Mat2 = Mat<2, 2, T>;
template <typename T> using Mat3 = Mat<3, 3, T>;
template <typename T> using Mat6 = Mat<6, 6, T>;
template <typename T> using Mat9 = Mat<9, 9, T>;
using Mat2d = Mat2<double>;
using Mat3d = Mat3<double>;
using Mat6d = Mat6<double>;
using Mat9d = Mat9<double>;

// Dynamic-size vectors
template <typename T> using VecX = Eigen::Matrix<T, Eigen::Dynamic, 1>;
using VecXd = Eigen::VectorXd;
using VecXi = Eigen::VectorXi;

// Dynamic-size matrices
template <typename T> using MatX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
using MatXd = MatX<double>;
using MatXi = MatX<int>;

// Sparse matrices
using Triplet = Eigen::Triplet<double>;
using SparseMatrix = Eigen::SparseMatrix<double>;

// Diagonal matrices
using DiagonalMatrix = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;

// Custom vectors
using Color = Vec4f;

// OpenMesh traits
struct ISMOpenMeshTraits : public OpenMesh::DefaultTraits
{
  typedef Vec3d Point;
  typedef Vec3d Normal;
  typedef double TexCoord1D;
  typedef Vec2d TexCoord2D;
  typedef Vec3d TexCoord3D;
  typedef Color Color;
};

// Mesh types
using TriMesh = OpenMesh::TriMesh_ArrayKernelT<ISMOpenMeshTraits>;
using PolyMesh = OpenMesh::PolyMesh_ArrayKernelT<ISMOpenMeshTraits>;

// Handle types
using VH = OpenMesh::VertexHandle;
using EH = OpenMesh::EdgeHandle;
using FH = OpenMesh::FaceHandle;
using HEH = OpenMesh::HalfedgeHandle;
using SVH = OpenMesh::SmartVertexHandle;
using SHEH = OpenMesh::SmartHalfedgeHandle;
using SEH = OpenMesh::SmartEdgeHandle;
using SFH = OpenMesh::SmartFaceHandle;

// Custom properties
using Parametrization = ExternalProperty<VH, Vec2d>;
using Metric = ExternalProperty<EH, double>;
using TexCoords = ExternalProperty<HEH, Vec2d>;

struct BarycentricPoint;
using VertexToPointMap = ExternalProperty<VH, BarycentricPoint>;

}