//
//  common.cpp
//  kcalg
//
//  Created by knightc on 2019/7/12.
//  Copyright © 2019 knightc. All rights reserved.
//

#include <stdio.h>

#include "opentsb/math.h"
#include <NTL/GF2X.h>
#include <NTL/ZZX.h>

using namespace NTL;
using namespace std;

 void lcm(ZZ& k, const ZZ& x, const ZZ& y) {
    ZZ gcd;
    mul(k, x, y);
    GCD(gcd, x, y);
    k /= gcd;
}

//一次性运算求 向量的 lcm/gcd
vec_ZZ lcm_gcd_vec(const vec_ZZ& v,int typemode){

    vec_ZZ vec_out;
    long m,n,j,i;
    ZZ temp;
    i=0;
    j=0;

     n= v.length();
    m= ceil(n/2.0);//向上取整，必须是非整数，即 2---》2.0
      vec_out.SetLength(m);
    switch (typemode) {
        case 1:
            for (i=0; i < n-1; i+=2) {

                lcm(temp,v[i],v[i+1]);
                // cout << temp<< endl;
                vec_out[j] = temp;
                //cout << vec_out << endl;
                j++;
                // cout << j << endl;
            }
            // cout << i << endl;
            if (n % 2 == 1 ) {
                lcm( vec_out[j],v[i],ZZ(1));
            }
            break;
         case 2:
            for (i=0; i < n-1; i+=2) {
                GCD(temp,v[i],v[i+1]);
                // cout << temp<< endl;
                vec_out[j] = temp;
                //cout << vec_out << endl;
                j++;
                // cout << j << endl;
            }
            // cout << i << endl;
            if (n % 2 == 1 ) {
                GCD( vec_out[j],v[i],v[i-1]);
            }
            break;
    }
    return vec_out;
}

//求 n 个数的最大公约数/最小公倍数
vec_ZZ lcm_gcd_vec(const vec_ZZ& v,int typemode,long endleng){

    vec_ZZ vec_out;

    vec_out= lcm_gcd_vec(v, typemode);
    endleng=vec_out.length();

    while (endleng != 1) {
       vec_out= lcm_gcd_vec(vec_out, typemode);
         endleng=vec_out.length();
    }

    return vec_out;
}

//array can use "for" to assignment，but vector can't。if must，can use vector.push_back
void array_to_vec(const ZZ arr[],vec_ZZ &v,long len) {
   // long n= v.length();

    v.SetLength(len);
    for (int i=0; i<=len-1; i++) {
        v[i]=arr[i];
    }
    //cout<<v<<endl;
}

void array_to_vec(const int arr[],vec_ZZ &v,long len) {
   // long n= v.length();

    for (int i=0; i<=len-1; i++) {
        v[i]=to_ZZ(arr[i]);

    }
    // cout<<v<<endl;
}

void array2_to_mat(const vector< vector<ZZ> > arr,mat_ZZ &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=arr[i][j];
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_ZZ &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_ZZ(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_GF2 &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_GF2(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<ZZ> > arr,mat_GF2 &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_GF2(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<GF2> > arr,mat_GF2 &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=arr[i][j];
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_GF2E &v){

    long m= v.NumRows();
    long n= v.NumCols();

//    const GF2X P(INIT_MONO,8);
//    GF2E::init(P);
    GF2X P;
    SetCoeff(P, 8, 1);
    SetCoeff(P, 4, 1);
    SetCoeff(P, 3, 1);
    SetCoeff(P, 1, 1);
    SetCoeff(P, 0, 1);
    GF2E::init(P);

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_GF2E(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_ZZ_p &v){

    v.SetDims(arr.size(),arr[0].size());

    long m= v.NumRows();
    long n= v.NumCols();

    ZZ_p::init(ZZ(256));

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_ZZ_p(long(arr[i][j]));
        }
    }

}

//void inv_array2_to_mat(const mat_GF2E &v, vector< vector<int> > &arr) //gf2e --> int  ?????

//to_ZZX(vec);可以把一个一维数组/向量 转为多项式（系数更换）
void kc_lagrange(vec_vec_ZZ_p &pointMat,vec_zz_p &coeffVec) {

}


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "stiffness_matrix.hpp"

//----------------AssembleMatrixBegin----------------
//! Assemble the stiffness matrix
//! for the linear system.
//!
//!
//! @param[in] vertices a list of triangle vertices
//! @param[in] triangles a list of triangles
SparseMatrix assembleStiffnessMatrix(
    const Eigen::MatrixXd &vertices,
    const Eigen::MatrixXi &triangles) {
	SparseMatrix         A(vertices.rows(), vertices.rows());
	std::vector<Triplet> triplets;
	const int            numberOfElements = triangles.rows();

	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = triangles.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::Matrix3d stiffnessMatrix;
		computeStiffnessMatrix(stiffnessMatrix, a, b, c);

		for (int n = 0; n < 3; ++n) {
			for (int m = 0; m < 3; ++m) {
				auto triplet = Triplet(indexSet(n), indexSet(m), stiffnessMatrix(n, m));
				triplets.push_back(triplet);
			}
		}
	}

	A.setFromTriplets(triplets.begin(), triplets.end());

	return A;
}
//----------------AssembleMatrixEnd----------------


/**
 * @file   gweyl.hpp
 * @author ALIKAWA Hidehisa <alleyhide@gmail.com>
 * @date   2018/04/30
 *
 * @brief  public header file of gweyl
 *
 * Released under the MIT license
 */
#pragma once

#include <boost/rational.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

//
// our namespace "gweyl"
//
namespace gweyl{

//
// rational numbers
//
using rational=boost::rational<int>;

//
// matrix of rational numbers
//
using matrix=boost::numeric::ublas::matrix<rational>;

//
// number vector
// this is vector of mathematics (primitive linear algera)
//
using NumberVector=boost::numeric::ublas::vector<rational>;

//
// we define equality of matirx and number vector
// because boost does not define the equality
//
bool equal(const matrix &X, const matrix &Y);
bool equal(const NumberVector &v, const NumberVector &w);

//
// gweyl treats simple Lie algebra types
// in Cartan's classification
//
enum class Type{
    invalid,A,B,C,D,E,F,G,
};

//
// Coordinate system
// There are at least two coordinates on root spaces.
// One is simple roots, and the other is fundamental weights.
//
enum class Coordinate {
    simple, fundamental,
};

//
// trace the message
// @param[in] msg message
//
//
void trace(std::string& msg);

///
/// The definition of Cartan Matrix is equivalent to
/// the definition of the root space, according to structure theory of Lie algebra,
/// that is, Cartan matrix is the source of "gweyl"
///
matrix CartanMatrix(Type X, unsigned n);
matrix InverseCartanMatrix(Type X, unsigned n);

class VectorRootSpace;

//
// class Cartan
//  other names are RootSpace, DynkinDiagram, ..
// fundamental class of gweyl
//
class Cartan
{
public:
    explicit Cartan(Type X, unsigned n);
    Cartan();
    Cartan(const Cartan &rhs);
    virtual ~Cartan();
    Cartan& operator=(const Cartan& rhs);

    //
    // @return Cartan matrix
    //
    matrix CartanMatrix();

    //
    // @return invers of Cartan matrix
    //
    matrix InverseCartanMatrix();

    //
    // @return the 'i'-th simple root
    //
    VectorRootSpace SimpleRoot(unsigned i);

    //
    // @return the 'i'-th fundamental weight
    //
    VectorRootSpace FundamentalWeight(unsigned i);

    //
    // @return highest root <not implemented>
    //
    VectorRootSpace HighestRoot();

    //
    // @return zero vector
    //
    VectorRootSpace Zero();

    //
    // @return half sum of positive roots
    //
    VectorRootSpace Rho();

    //
    // @return positive roots
    //
    std::vector<VectorRootSpace> PositiveRoots();

    //
    // @return roots <not implemented>
    //
    std::vector<VectorRootSpace> Roots();

    // getters
    Type type();
    Type type() const;
    unsigned rank();
    unsigned rank() const;

    // operators
    bool operator==(const Cartan& rhs);
    bool operator!=(const Cartan& rhs);

private:
    struct Impl;
    std::unique_ptr<Impl> pImpl;
};

//
// In gweyl, the class RootSpace, DynkinDiagram and the class Cartan are same
//
using RootSpace = Cartan;
using DynkinDiagram = Cartan;

//
// Vector in root space
//
class VectorRootSpace
{
public:
    // constructors
    explicit VectorRootSpace(Type X, NumberVector& v, Coordinate c);
    VectorRootSpace();
    virtual ~VectorRootSpace();
    VectorRootSpace(const VectorRootSpace& rhs);
    VectorRootSpace& operator=(const VectorRootSpace& rhs);

    // for debug
    void printf();

    // getters
    NumberVector simpleCoefficients();
    NumberVector simpleCoefficients() const;
    NumberVector fundamentalCoefficients();
    NumberVector fundamentalCoefficients() const;
    Type type();
    Type type() const;
    unsigned rank();
    unsigned rank() const;

    // eqaulity
    bool isInSameSpace(const VectorRootSpace& rhs);
    bool operator==(const VectorRootSpace& rhs);
    bool operator!=(const VectorRootSpace& rhs);

    // operators
    VectorRootSpace& operator+=(const VectorRootSpace& rhs);
    VectorRootSpace& operator-=(const VectorRootSpace& rhs);
    VectorRootSpace& operator*=(rational r);

    // dominant integral
    bool dominant();
    bool integral();
    bool isDominantIntegral();
private:
    struct Impl;
    std::unique_ptr<Impl> pImpl;
};

//
// operators of the class VectorRootSpace
//
rational InnerProduct(VectorRootSpace& v, VectorRootSpace& w);
VectorRootSpace operator+(const VectorRootSpace& v1, const VectorRootSpace& v2);
VectorRootSpace operator-(const VectorRootSpace& v1, const VectorRootSpace& v2);
VectorRootSpace operator*(const VectorRootSpace& v1, const rational r);
VectorRootSpace operator*(const rational r, const VectorRootSpace& v1);

//
// class IrreducibleRepresentation
//
class IrreducibleRepresentation{
public:
    //constructors
    IrreducibleRepresentation();
    explicit IrreducibleRepresentation(VectorRootSpace& highestweight);
    virtual ~IrreducibleRepresentation();
    IrreducibleRepresentation(const IrreducibleRepresentation& rhs);
    IrreducibleRepresentation& operator=(const IrreducibleRepresentation& rhs);

    //getters
    Type type();
    Type type() const;
    unsigned rank();
    unsigned rank() const;
    VectorRootSpace highestweight();
    VectorRootSpace highestweight() const;

    //operators
    bool operator==(const IrreducibleRepresentation& rhs);
    bool operator!=(const IrreducibleRepresentation& rhs);

    int dimension();

private:
    struct Impl;
    std::unique_ptr<Impl> pImpl;
};

}


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::gamma::log_unnormalized_pdf         //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_GAMMA_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <cmath>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{
namespace math{

template <class T, class P>
inline T log_unnormalized_pdf(
    const boost::math::gamma_distribution<T, P>& dist,
    const T& x
)
{
   BOOST_MATH_STD_USING  // for ADL of std functions

   static const char* function
    = "log_unnormalized_pdf(const gamma_distribution<%1%>&, %1%)";

   T shape = dist.shape();
   T scale = dist.scale();

   T result;
   if(false == boost::math::detail::check_gamma(
        function, scale, shape, &result, P()))
      return result;
   if(false == boost::math::detail::check_gamma_x(function, x, &result, P()))
      return result;

   if(x == 0)
   {
      // TODO just a guess based on definition of math::pdf
      // cast is needed by MSVC
      return log(static_cast<T>(0));
   }
   static T one_ = static_cast<T>(1);
   result = (shape - one_) * log(x) - x / scale;
   return result;
} // log_unnormalized_pdf

}// math
}// boost

#endif


// g++ -O3 -DNDEBUG benchmarkX.cpp -o benchmarkX && time ./benchmarkX

#include <Eigen/Core>

using namespace std;
using namespace Eigen;

#ifndef VECTYPE
#define VECTYPE VectorXLd
#endif

#ifndef VECSIZE
#define VECSIZE 1000000
#endif

#ifndef REPEAT
#define REPEAT 1000
#endif

int main(int argc, char *argv[])
{
	VECTYPE I = VECTYPE::Ones(VECSIZE);
	VECTYPE m(VECSIZE,1);
	for(int i = 0; i < VECSIZE; i++)
	{
		m[i] = 0.1 * i/VECSIZE;
	}
	for(int a = 0; a < REPEAT; a++)
	{
		m = VECTYPE::Ones(VECSIZE) + 0.00005 * (m.cwise().square() + m/4);
	}
	cout << m[0] << endl;
	return 0;
}


#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/MatrixFunctions>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/utility.hpp>

namespace common_robotics_utilities
{
namespace math
{
bool Equal3d(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
  if ((v1.x() == v2.x()) && (v1.y() == v2.y()) && (v1.z() == v2.z()))
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool Equal4d(const Eigen::Vector4d& v1, const Eigen::Vector4d& v2)
{
  if ((v1(0) == v2(0)) && (v1(1) == v2(1))
      && (v1(2) == v2(2)) && (v1(3) == v2(3)))
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool CloseEnough(const double p1, const double p2, const double threshold)
{
  const double real_threshold = std::abs(threshold);
  const double abs_delta = std::abs(p2 - p1);
  if (abs_delta <= real_threshold)
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool CloseEnough(const Eigen::Vector3d& v1,
                 const Eigen::Vector3d& v2,
                 const double threshold)
{
  const double real_threshold = std::abs(threshold);
  if (std::abs(v1.x() - v2.x()) > real_threshold)
  {
    return false;
  }
  if (std::abs(v1.y() - v2.y()) > real_threshold)
  {
    return false;
  }
  if (std::abs(v1.z() - v2.z()) > real_threshold)
  {
    return false;
  }
  return true;
}

Eigen::Vector3d RotateVector(const Eigen::Quaterniond& quat,
                             const Eigen::Vector3d& vec)
{
  const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
  const Eigen::Quaterniond res = quat * (temp * quat.inverse());
  return Eigen::Vector3d(res.x(), res.y(), res.z());
}

Eigen::Vector3d RotateVectorReverse(const Eigen::Quaterniond& quat,
                                    const Eigen::Vector3d& vec)
{
  const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
  const Eigen::Quaterniond res = quat.inverse() * (temp * quat);
  return Eigen::Vector3d(res.x(), res.y(), res.z());
}

double EnforceContinuousRevoluteBounds(const double value)
{
  if ((value <= -M_PI) || (value > M_PI))
  {
    const double remainder = std::fmod(value, 2.0 * M_PI);
    if (remainder <= -M_PI)
    {
      return (remainder + (2.0 * M_PI));
    }
    else if (remainder > M_PI)
    {
      return (remainder - (2.0 * M_PI));
    }
    else
    {
      return remainder;
    }
  }
  else
  {
    return value;
  }
}

Eigen::VectorXd SafeNormal(const Eigen::VectorXd& vec)
{
  if (vec.size() > 0)
  {
    const double norm = vec.norm();
    if (norm > std::numeric_limits<double>::epsilon())
    {
      return vec / norm;
    }
    else
    {
      return Eigen::VectorXd::Zero(vec.size());
    }
  }
  else
  {
    return Eigen::VectorXd::Zero(vec.size());
  }
}

double SquaredNorm(const std::vector<double>& vec)
{
  double squared_norm = 0.0;
  for (size_t idx = 0; idx < vec.size(); idx++)
  {
    const double element = vec[idx];
    squared_norm += (element * element);
  }
  return squared_norm;
}

double Norm(const std::vector<double>& vec)
{
  return std::sqrt(SquaredNorm(vec));
}

std::vector<double> Abs(const std::vector<double>& vec)
{
  std::vector<double> absed(vec.size(), 0.0);
  for (size_t idx = 0; idx < absed.size(); idx++)
  {
    absed[idx] = std::abs(vec[idx]);
  }
  return absed;
}

std::vector<double> Multiply(const std::vector<double>& vec,
                             const double scalar)
{
  std::vector<double> multiplied(vec.size(), 0.0);
  for (size_t idx = 0; idx < multiplied.size(); idx++)
  {
    const double element = vec[idx];
    multiplied[idx] = element * scalar;
  }
  return multiplied;
}

std::vector<double> Multiply(const std::vector<double>& vec1,
                             const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> multiplied(vec1.size(), 0.0);
    for (size_t idx = 0; idx < multiplied.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      multiplied[idx] = element1 * element2;
    }
    return multiplied;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

std::vector<double> Divide(const std::vector<double>& vec, const double scalar)
{
  const double inv_scalar = 1.0 / scalar;
  return Multiply(vec, inv_scalar);
}

std::vector<double> Divide(const std::vector<double>& vec1,
                           const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> divided(vec1.size(), 0.0);
    for (size_t idx = 0; idx < divided.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      divided[idx] = element1 / element2;
    }
    return divided;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

std::vector<double> Add(const std::vector<double>& vec, const double scalar)
{
  std::vector<double> added(vec.size(), 0.0);
  for (size_t idx = 0; idx < added.size(); idx++)
  {
    added[idx] = vec[idx] + scalar;
  }
  return added;
}

std::vector<double> Add(const std::vector<double>& vec1,
                        const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> added(vec1.size(), 0.0);
    for (size_t idx = 0; idx < added.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      added[idx] = element1 + element2;
    }
    return added;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

std::vector<double> Sub(const std::vector<double>& vec, const double scalar)
{
  std::vector<double> subed(vec.size(), 0.0);
  for (size_t idx = 0; idx < subed.size(); idx++)
  {
    subed[idx] = vec[idx] - scalar;
  }
  return subed;
}

std::vector<double> Sub(const std::vector<double>& vec1,
                        const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> subed(vec1.size(), 0.0);
    for (size_t idx = 0; idx < subed.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      subed[idx] = element1 - element2;
    }
    return subed;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

double Sum(const std::vector<double>& vec)
{
  double sum = 0.0;
  for (size_t idx = 0; idx < vec.size(); idx++)
  {
    const double element = vec[idx];
    sum += element;
  }
  return sum;
}

Eigen::Matrix3d Skew(const Eigen::Vector3d& vector)
{
  Eigen::Matrix3d skewed;
  skewed << 0.0, -vector.z(), vector.y(),
        vector.z(), 0.0, -vector.x(),
        -vector.y(), vector.x(), 0.0;
  return skewed;
}

Eigen::Vector3d Unskew(const Eigen::Matrix3d& matrix)
{
  const Eigen::Matrix3d matrix_symetric = (matrix - matrix.transpose()) / 2.0;
  const Eigen::Vector3d unskewed(matrix_symetric(2, 1),
                                 matrix_symetric(0, 2),
                                 matrix_symetric(1, 0));
  return unskewed;
}

Eigen::Matrix4d TwistHat(const Eigen::Matrix<double, 6, 1>& twist)
{
  const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
  const Eigen::Matrix3d hatted_rot_velocity = Skew(twist.segment<3>(3));
  Eigen::Matrix4d hatted_twist = Eigen::Matrix4d::Zero();
  hatted_twist.block<3, 3>(0, 0) = hatted_rot_velocity;
  hatted_twist.block<3, 1>(0, 3) = trans_velocity;
  return hatted_twist;
}

Eigen::Matrix<double, 6, 1> TwistUnhat(const Eigen::Matrix4d& hatted_twist)
{
   const Eigen::Vector3d trans_velocity = hatted_twist.block<3, 1>(0, 3);
   const Eigen::Vector3d rot_velocity = Unskew(hatted_twist.block<3, 3>(0, 0));
   Eigen::Matrix<double, 6, 1> twist;
   twist.segment<3>(0) = trans_velocity;
   twist.segment<3>(3) = rot_velocity;
   return twist;
}

Eigen::Matrix<double, 6, 6> AdjointFromTransform(
    const Eigen::Isometry3d& transform)
{
  const Eigen::Matrix3d rotation = transform.matrix().block<3, 3>(0, 0);
  const Eigen::Vector3d translation = transform.matrix().block<3, 1>(0, 3);
  const Eigen::Matrix3d translation_hat = Skew(translation);
  // Assemble the adjoint matrix
  Eigen::Matrix<double, 6, 6> adjoint;
  adjoint.block<3, 3>(0, 0) = rotation;
  adjoint.block<3, 3>(0, 3) = translation_hat * rotation;
  adjoint.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
  adjoint.block<3, 3>(3, 3) = rotation;
  return adjoint;
}

Eigen::Matrix<double, 6, 1> TransformTwist(
    const Eigen::Isometry3d& transform,
    const Eigen::Matrix<double, 6, 1>& initial_twist)
{
  return (Eigen::Matrix<double, 6, 1>)(AdjointFromTransform(transform)
                                       * initial_twist);
}

Eigen::Matrix<double, 6, 1> TwistBetweenTransforms(
    const Eigen::Isometry3d& start,
    const Eigen::Isometry3d& end)
{
  const Eigen::Isometry3d t_diff = start.inverse() * end;
  return TwistUnhat(t_diff.matrix().log());
}

Eigen::Matrix3d ExpMatrixExact(const Eigen::Matrix3d& hatted_rot_velocity,
                               const double delta_t)
{
  if (std::abs(Unskew(hatted_rot_velocity).norm() - 1.0) < 1e-10)
  {
    const Eigen::Matrix3d exp_matrix = Eigen::Matrix3d::Identity()
      + (hatted_rot_velocity * sin(delta_t))
      + (hatted_rot_velocity * hatted_rot_velocity * (1.0 - cos(delta_t)));
    return exp_matrix;
  }
  else
  {
    throw std::invalid_argument("Invalid hatted_rot_velocity: "
                                "std::abs(Unskew(hatted_rot_velocity).norm()"
                                " - 1.0) >= 1e-10");
  }
}

Eigen::Isometry3d ExpTwist(const Eigen::Matrix<double, 6, 1>& twist,
                           const double delta_t)
{
  const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
  const Eigen::Vector3d rot_velocity = twist.segment<3>(3);
  const double trans_velocity_norm = trans_velocity.norm();
  const double rot_velocity_norm = rot_velocity.norm();
  Eigen::Matrix4d raw_transform = Eigen::Matrix4d::Identity();
  if (rot_velocity_norm >= 1e-100)
  {
    const double scaled_delta_t = delta_t * rot_velocity_norm;
    const Eigen::Vector3d scaled_trans_velocity
        = trans_velocity / rot_velocity_norm;
    const Eigen::Vector3d scaled_rot_velocity
        = rot_velocity / rot_velocity_norm;
    const Eigen::Matrix3d rotation_displacement
        = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
    const Eigen::Vector3d translation_displacement
        = ((Eigen::Matrix3d::Identity() - rotation_displacement)
           * scaled_rot_velocity.cross(scaled_trans_velocity))
          + (scaled_rot_velocity * scaled_rot_velocity.transpose()
             * scaled_trans_velocity * scaled_delta_t);
    raw_transform.block<3, 3>(0, 0) = rotation_displacement;
    raw_transform.block<3, 1>(0, 3) = translation_displacement;
  }
  else
  {
    if ((trans_velocity_norm >= 1e-100) || (rot_velocity_norm == 0.0))
    {
      raw_transform.block<3, 1>(0, 3) = trans_velocity * delta_t;
    }
    else
    {
      std::cerr << "YOU MAY ENCOUNTER NUMERICAL INSTABILITY IN EXPTWIST(...)"
                   " WITH TRANS & ROT VELOCITY NORM < 1e-100 ***" << std::endl;
      const double scaled_delta_t = delta_t * rot_velocity_norm;
      const Eigen::Vector3d scaled_trans_velocity
          = trans_velocity / rot_velocity_norm;
      const Eigen::Vector3d scaled_rot_velocity
          = rot_velocity / rot_velocity_norm;
      const Eigen::Matrix3d rotation_displacement
          = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
      const Eigen::Vector3d translation_displacement
          = ((Eigen::Matrix3d::Identity() - rotation_displacement)
             * scaled_rot_velocity.cross(scaled_trans_velocity))
            + (scaled_rot_velocity * scaled_rot_velocity.transpose()
               * scaled_trans_velocity * scaled_delta_t);
      raw_transform.block<3, 3>(0, 0) = rotation_displacement;
      raw_transform.block<3, 1>(0, 3) = translation_displacement;
    }
  }
  Eigen::Isometry3d transform;
  transform = raw_transform;
  return transform;
}

double Interpolate(const double p1, const double p2, const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((p1 * (1.0 - real_ratio)) + (p2 * real_ratio));
}

double InterpolateContinuousRevolute(const double p1,
                                     const double p2,
                                     const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Safety check args
  const double real_p1 = EnforceContinuousRevoluteBounds(p1);
  const double real_p2 = EnforceContinuousRevoluteBounds(p2);
  // Interpolate
  double interpolated = 0.0;
  double diff = real_p2 - real_p1;
  if (std::abs(diff) <= M_PI)
  {
    interpolated = real_p1 + diff * real_ratio;
  }
  else
  {
    if (diff > 0.0)
    {
      diff = 2.0 * M_PI - diff;
    }
    else
    {
      diff = -2.0 * M_PI - diff;
    }
    interpolated = real_p1 - diff * real_ratio;
    // Input states are within bounds, so the following check is sufficient
    if (interpolated > M_PI)
    {
      interpolated -= 2.0 * M_PI;
    }
    else
    {
      if (interpolated < -M_PI)
      {
        interpolated += 2.0 * M_PI;
      }
    }
  }
  return interpolated;
}

std::vector<double> Interpolate(const std::vector<double>& v1,
                                const std::vector<double>& v2,
                                const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Safety check inputs
  const size_t len = v1.size();
  if (len != v2.size())
  {
    throw std::invalid_argument("Vectors v1 and v2 must be the same size");
  }
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  std::vector<double> interped(len, 0);
  for (size_t idx = 0; idx < len; idx++)
  {
    interped[idx] = ((v1[idx] * (1.0 - real_ratio)) + (v2[idx] * real_ratio));
  }
  return interped;
}

Eigen::Quaterniond Interpolate(const Eigen::Quaterniond& q1,
                               const Eigen::Quaterniond& q2,
                               const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  return q1.slerp(real_ratio, q2);
}

Eigen::VectorXd InterpolateXd(const Eigen::VectorXd& v1,
                              const Eigen::VectorXd& v2,
                              const double ratio)
{
  // Safety check sizes
  if (v1.size() != v2.size())
  {
    throw std::invalid_argument("Vectors v1 and v2 must be the same size");
  }
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

Eigen::Vector3d Interpolate3d(const Eigen::Vector3d& v1,
                              const Eigen::Vector3d& v2,
                              const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

Eigen::Vector4d Interpolate4d(const Eigen::Vector4d& v1,
                              const Eigen::Vector4d& v2,
                              const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& t1,
                              const Eigen::Isometry3d& t2,
                              const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  const Eigen::Vector3d v1 = t1.translation();
  const Eigen::Quaterniond q1(t1.rotation());
  const Eigen::Vector3d v2 = t2.translation();
  const Eigen::Quaterniond q2(t2.rotation());
  const Eigen::Vector3d vint = Interpolate3d(v1, v2, real_ratio);
  const Eigen::Quaterniond qint = Interpolate(q1, q2, real_ratio);
  const Eigen::Isometry3d tint = ((Eigen::Translation3d)vint) * qint;
  return tint;
}

double SquaredDistance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
{
  const double xd = v2.x() - v1.x();
  const double yd = v2.y() - v1.y();
  return ((xd * xd) + (yd * yd));
}

double Distance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
{
  return std::sqrt(SquaredDistance(v1, v2));
}

double SquaredDistance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
  const double xd = v2.x() - v1.x();
  const double yd = v2.y() - v1.y();
  const double zd = v2.z() - v1.z();
  return ((xd * xd) + (yd * yd) + (zd * zd));
}

double Distance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
  return std::sqrt(SquaredDistance(v1, v2));
}

double SquaredDistance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
{
  if (v1.size() == v2.size())
  {
    return (v2 - v1).squaredNorm();
  }
  else
  {
    throw std::invalid_argument("v1.size() != v2.size()");
  }
}

double Distance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
{
  return std::sqrt(SquaredDistance(v1, v2));
}

double Distance(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2)
{
  const double dq = std::abs((q1.w() * q2.w())
                             + (q1.x() * q2.x())
                             + (q1.y() * q2.y())
                             + (q1.z() * q2.z()));
  if (dq < (1.0 - std::numeric_limits<double>::epsilon()))
  {
    return std::acos(2.0 * (dq * dq) - 1.0);
  }
  else
  {
    return 0.0;
  }
}

double Distance(const Eigen::Isometry3d& t1,
                const Eigen::Isometry3d& t2,
                const double alpha)
{
  const double real_alpha = utility::ClampValue(alpha, 0.0, 1.0);
  const Eigen::Vector3d v1 = t1.translation();
  const Eigen::Quaterniond q1(t1.rotation());
  const Eigen::Vector3d v2 = t2.translation();
  const Eigen::Quaterniond q2(t2.rotation());
  const double vdist = Distance(v1, v2) * (1.0 - real_alpha);
  const double qdist = Distance(q1, q2) * (real_alpha);
  return vdist + qdist;
}

double SquaredDistance(const std::vector<double>& p1,
                       const std::vector<double>& p2)
{
  if (p1.size() == p2.size())
  {
    double distance = 0.0;
    for (size_t idx = 0; idx < p1.size(); idx++)
    {
      distance += (p2[idx] - p1[idx]) * (p2[idx] - p1[idx]);
    }
    return distance;
  }
  else
  {
    throw std::invalid_argument("p1.size() != p2.size()");
  }
}

double Distance(const std::vector<double>& p1, const std::vector<double>& p2)
{
  if (p1.size() == p2.size())
  {
    return std::sqrt(SquaredDistance(p1, p2));
  }
  else
  {
    throw std::invalid_argument("p1.size() != p2.size()");
  }
}

double ContinuousRevoluteSignedDistance(const double p1, const double p2)
{
  // Safety check args
  const double real_p1 = EnforceContinuousRevoluteBounds(p1);
  const double real_p2 = EnforceContinuousRevoluteBounds(p2);
  const double raw_distance = real_p2 - real_p1;
  if ((raw_distance <= -M_PI) || (raw_distance > M_PI))
  {
    if (raw_distance <= -M_PI)
    {
      return (-(2.0 * M_PI) - raw_distance);
    }
    else if (raw_distance > M_PI)
    {
      return ((2.0 * M_PI) - raw_distance);
    }
    else
    {
      return raw_distance;
    }
  }
  else
  {
    return raw_distance;
  }
}

double ContinuousRevoluteDistance(const double p1, const double p2)
{
  return std::abs(ContinuousRevoluteSignedDistance(p1, p2));
}

double AddContinuousRevoluteValues(const double start, const double change)
{
  return EnforceContinuousRevoluteBounds(start + change);
}

double GetContinuousRevoluteRange(const double start, const double end)
{
  const double raw_range = ContinuousRevoluteSignedDistance(start, end);
  if (raw_range >= 0.0)
  {
    return raw_range;
  }
  else
  {
    return (2.0 * M_PI) + raw_range;
  }
}

bool CheckInContinuousRevoluteRange(const double start,
                                    const double range,
                                    const double val)
{
  const double real_val = EnforceContinuousRevoluteBounds(val);
  const double real_start = EnforceContinuousRevoluteBounds(start);
  const double delta = ContinuousRevoluteSignedDistance(real_start, real_val);
  if (delta >= 0.0)
  {
    if (delta <= range)
    {
      return true;
    }
    else
    {
      return false;
    }
  }
  else
  {
    const double real_delta = (2.0 * M_PI) + delta;
    if (real_delta <= range)
    {
      return true;
    }
    else
    {
      return false;
    }
  }
}

bool CheckInContinuousRevoluteBounds(const double start,
                                     const double end,
                                     const double val)
{
  const double range = GetContinuousRevoluteRange(start, end);
  return CheckInContinuousRevoluteRange(start, range, val);
}

double AverageStdVectorDouble(const std::vector<double>& values,
                              const std::vector<double>& weights)
{
  // Get the weights
  if (values.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  if ((weights.size() != values.size()) && (weights.size() != 0))
  {
    throw std::invalid_argument("Provided weights must be empty"
                                " or same size to provided vector");
  }
  const bool use_weights = (weights.size() != 0);
  // Find the first element with non-zero weight
  size_t starting_idx = 0;
  while (starting_idx < weights.size()
         && std::abs(weights[starting_idx]) == 0.0)
  {
    starting_idx++;
  }
  // If all weights are zero, result is undefined
  if (starting_idx >= values.size())
  {
    throw std::invalid_argument("Provided weights are all zero");
  }
  // Start the recursive definition with the base case
  double average = values[starting_idx];
  const double starting_weight = use_weights ? std::abs(weights[starting_idx])
                                             : 1.0;
  double weights_running_sum = starting_weight;
  // Do the weighted averaging on the rest of the vectors
  for (size_t idx = starting_idx + 1; idx < values.size(); idx++)
  {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    weights_running_sum += weight;
    const double effective_weight = weight / weights_running_sum;
    const double prev_average = average;
    const double current = values[idx];
    average = prev_average + (effective_weight * (current - prev_average));
  }
  return average;
}

double ComputeStdDevStdVectorDouble(const std::vector<double>& values,
                                    const double mean)
{
  if (values.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  else if (values.size() == 1)
  {
    return 0.0;
  }
  else
  {
    const double inv_n_minus_1 = 1.0 / (double)(values.size() - 1);
    double stddev_sum = 0.0;
    for (size_t idx = 0; idx < values.size(); idx++)
    {
      const double delta = values[idx] - mean;
      stddev_sum += (delta * delta);
    }
    return std::sqrt(stddev_sum * inv_n_minus_1);
  }
}

double ComputeStdDevStdVectorDouble(const std::vector<double>& values)
{
  const double mean = AverageStdVectorDouble(values);
  return ComputeStdDevStdVectorDouble(values, mean);
}

double AverageContinuousRevolute(const std::vector<double>& angles,
                                 const std::vector<double>& weights)
{
  return AverageStdVectorDouble(angles, weights);
}

Eigen::Vector3d AverageEigenVector3d(const VectorVector3d& vectors,
                                     const std::vector<double>& weights)
{
  return AverageEigenVector(vectors, weights);
}

Eigen::VectorXd AverageEigenVectorXd(
    const std::vector<Eigen::VectorXd>& vectors,
    const std::vector<double>& weights)
{
  return AverageEigenVector(vectors, weights);
}

// Implementation of method described in (http://stackoverflow.com/a/27410865)
// See paper at (http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf) for more
Eigen::Quaterniond AverageEigenQuaterniond(const VectorQuaterniond& quaternions,
                                           const std::vector<double>& weights)
{
  // Get the weights
  const bool use_weights = weights.size() == quaternions.size() ? true : false;
  if (quaternions.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  if ((weights.size() != quaternions.size()) && (weights.size() != 0))
  {
    throw std::invalid_argument("Provided weights must be empty"
                                " or same size to provided vector");
  }
  // Shortcut the process if there is only 1 quaternion
  if (quaternions.size() == 1)
  {
    if (weights.size() > 0)
    {
      if (std::abs(weights[0]) == 0.0)
      {
        throw std::invalid_argument("Single quaternion with zero weight");
      }
    }
    return quaternions[0];
  }
  // Build the averaging matrix
  Eigen::MatrixXd q_matrix(4, quaternions.size());
  for (size_t idx = 0; idx < quaternions.size(); idx++)
  {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    const Eigen::Quaterniond& q = quaternions[idx];
    q_matrix.col((ssize_t)idx) << weight * q.w(),
                                  weight * q.x(),
                                  weight * q.y(),
                                  weight * q.z();
  }
  // Make the matrix square
  const Eigen::Matrix<double, 4, 4> qqtranspose_matrix
      = q_matrix * q_matrix.transpose();
  // Compute the eigenvectors and eigenvalues of the qqtranspose matrix
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>
      solver(qqtranspose_matrix);
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType
      eigen_values = solver.eigenvalues();
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
      eigen_vectors = solver.eigenvectors();
  // Extract the eigenvector corresponding to the largest eigenvalue
  double max_eigenvalue = -std::numeric_limits<double>::infinity();
  int64_t max_eigenvector_index = -1;
  for (size_t idx = 0; idx < 4; idx++)
  {
    const double current_eigenvalue = eigen_values((long)idx).real();
    if (current_eigenvalue > max_eigenvalue)
    {
      max_eigenvalue = current_eigenvalue;
      max_eigenvector_index = (int64_t)idx;
    }
  }
  if (max_eigenvector_index < 0)
  {
    throw std::runtime_error("Failed to find max eigenvector");
  }
  // Note that these are already normalized!
  const Eigen::Vector4cd best_eigenvector
      = eigen_vectors.col((long)max_eigenvector_index);
  // Convert back into a quaternion
  const Eigen::Quaterniond average_q(best_eigenvector(0).real(),
                                     best_eigenvector(1).real(),
                                     best_eigenvector(2).real(),
                                     best_eigenvector(3).real());
  return average_q;
}

Eigen::Isometry3d AverageEigenIsometry3d(const VectorIsometry3d& transforms,
                                         const std::vector<double>& weights)
{
  if (transforms.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  if ((weights.size() != transforms.size()) && (weights.size() != 0))
  {
    throw std::invalid_argument("Provided weights must be empty"
                                " or same size to provided vector");
  }
  // Shortcut the process if there is only 1 transform
  if (transforms.size() == 1)
  {
    if (weights.size() > 0)
    {
      if (std::abs(weights[0]) == 0.0)
      {
        throw std::invalid_argument("Single transform with zero weight");
      }
    }
    return transforms[0];
  }
  // Extract components
  VectorVector3d translations(transforms.size());
  VectorQuaterniond rotations(transforms.size());
  for (size_t idx = 0; idx < transforms.size(); idx++)
  {
    translations[idx] = transforms[idx].translation();
    rotations[idx] = Eigen::Quaterniond(transforms[idx].rotation());
  }
  // Average
  const Eigen::Vector3d average_translation
      = AverageEigenVector(translations, weights);
  const Eigen::Quaterniond average_rotation
      = AverageEigenQuaterniond(rotations, weights);
  // Make the average transform
  const Eigen::Isometry3d average_transform
      = (Eigen::Translation3d)average_translation * average_rotation;
  return average_transform;
}

double WeightedDotProduct(const Eigen::VectorXd& vec1,
                          const Eigen::VectorXd& vec2,
                          const Eigen::VectorXd& weights)
{
  return vec1.cwiseProduct(weights).dot(vec2);
}

double WeightedSquaredNorm(const Eigen::VectorXd& vec,
                           const Eigen::VectorXd weights)
{
  return WeightedDotProduct(vec, vec, weights);
}

double WeightedNorm(const Eigen::VectorXd& vec, const Eigen::VectorXd& weights)
{
  return std::sqrt(WeightedSquaredNorm(vec, weights));
}

double WeightedCosineAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                         const Eigen::VectorXd& vec2,
                                         const Eigen::VectorXd& weights)
{
  const double vec1_norm = WeightedNorm(vec1, weights);
  const double vec2_norm = WeightedNorm(vec2, weights);
  if (vec1_norm > 0 && vec2_norm > 0)
  {
    const double result
        = WeightedDotProduct(vec1, vec2, weights) / (vec1_norm * vec2_norm);
    return std::max(-1.0, std::min(result, 1.0));;
  }
  else
  {
    throw std::invalid_argument("One or more input vectors has zero norm");
  }
}

double WeightedAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                   const Eigen::VectorXd& vec2,
                                   const Eigen::VectorXd& weights)
{
  return std::acos(WeightedCosineAngleBetweenVectors(vec1, vec2, weights));
}
}  // namespace math
}  // namespace common_robotics_utilities


#define BOOST_TEST_MODULE "Testing Euclidean Functions"

#include <boost/test/unit_test.hpp>

#include "distance/Euclidean.hpp"

using namespace genex;
#define TOLERANCE 1e-9

BOOST_AUTO_TEST_CASE( eucdist, *boost::unit_test::tolerance(TOLERANCE) )
{
   Euclidean d;

   TimeSeries ts_1 {NULL, 0, 0, 2};
   TimeSeries ts_2 {NULL, 0, 0, 2};

   data_t a = d.dist(100.0, 110.0);

   data_t first = d.init();
   data_t second = d.reduce(first, first, 4.0, 0.0);
   data_t third = d.reduce(second, second, 7.0, 10.0);
   data_t c = d.norm(third, ts_1, ts_2);

   d.clean(third);

   BOOST_TEST( a == 100 );
   BOOST_TEST( c == sqrt(25.0 / 2) );
}


#include "slick/scene/triangulation.h"
#include <iostream>
#include <Eigen/Core>

#include "slick/math/se3.h"

int test_triangulate() {
  std::cout << "generting 3D points..." << std::endl;
  std::vector<Eigen::Matrix<slick::SlickScalar, 3, 1>> v3_points(100);
  for (size_t i = 0; i < v3_points.size(); ++i) {
    v3_points[i].setRandom();
    v3_points[i] *= 20.0; // scale up to value range to [-100,100]
    if (v3_points[i][2] < 0)
      v3_points[i][2] = -v3_points[i][2];
    v3_points[i][2] = 17.0; // all 3D points are on the same plane
  }
  // generates a  3degree rotation around axis-y & 1unit translate along axis-a
  slick::SE3 pose1;
  Eigen::Matrix<slick::SlickScalar, 3, 1> v3(0, 3.0 * M_PI / 180.0, 0);
  slick::SO3Group<slick::SlickScalar> so3_r(v3);
  slick::SE3Group<slick::SlickScalar> pose2(
      so3_r, Eigen::Matrix<slick::SlickScalar, 3, 1>(-1.0, 0.0, 0.0));

  std::vector<slick::SlickScalar> norms;
  for (int i = 0; i < v3_points.size(); ++i) {
    Eigen::Matrix<slick::SlickScalar, 2, 1> v2_p1 = slick::project(pose1 * v3_points[i]);
    Eigen::Matrix<slick::SlickScalar, 2, 1> v2_p2 = slick::project(pose2 * v3_points[i]);
    std::pair<Eigen::Matrix<slick::SlickScalar, 3, 1>, slick::SlickScalar> res =
        slick::triangulate(v2_p1, v2_p2, pose1, pose2);

    std::cout << "norm(real-estimate):" << (v3_points[i] - res.first).norm()
              << std::endl;
    norms.push_back((v3_points[i] - res.first).norm());
  }

  slick::SlickScalar sum_norm(0.0);
  for (std::vector<slick::SlickScalar>::iterator j = norms.begin();
       j != norms.end(); ++j)
    sum_norm += *j;
  std::cout << "sum_norm:"
            << "mean_norm:" << sum_norm / norms.size() << std::endl;
  if (sum_norm / norms.size() > 1.e-9)
    return -1;
  return 1;
}

int main(int argc, char **argv) {
  bool ok = true, final_ok = true;
  ok = test_triangulate();
  std::cout << "test_triangulate():" << ok << std::endl;
  final_ok &= ok;

  return (final_ok) ? 0 : -1;
}


/*
 * test_EMA.cpp
 *
 *  Created on: 2013-4-10
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/indicator/crt/EMA.h>
#include <hikyuu/indicator/crt/PRICELIST.h>
#include <hikyuu/indicator/crt/MA.h>

using namespace hku;

/**
 * @defgroup test_indicator_EMA test_indicator_EMA
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_EMA ) {
    PriceList d;
    for (int i = 0; i < 30; ++i) {
        d.push_back(i);
    }

    /** @arg n = 1 */
    Indicator data = PRICELIST(d);
    Indicator ema = EMA(data, 1);
    BOOST_CHECK(ema.discard() == 0);
    BOOST_CHECK(ema.size() == 30);
    BOOST_CHECK(ema[0] == 0.0);
    BOOST_CHECK(ema[10] == 10.0);
    BOOST_CHECK(ema[29] == 29.0);

    /** @arg n = 2 */
    ema = EMA(data, 2);
    BOOST_CHECK(ema.discard() == 0);
    BOOST_CHECK(ema.size() == 30);
    BOOST_CHECK(ema[0] == 0.0);
    BOOST_CHECK(std::fabs(ema[1] - 0.66667) < 0.0001);
    BOOST_CHECK(std::fabs(ema[2] - 1.55556) < 0.0001);
    BOOST_CHECK(std::fabs(ema[3] - 2.51852) < 0.0001);
    BOOST_CHECK(std::fabs(ema[4] - 3.50617) < 0.0001);

    /** @arg n = 10 */
    ema = EMA(data, 10);
    BOOST_CHECK(ema.discard() == 0);
    BOOST_CHECK(ema.size() == 30);
    BOOST_CHECK(ema[0] == 0.0);
    BOOST_CHECK(std::fabs(ema[1] - 0.18182) < 0.0001);
    BOOST_CHECK(std::fabs(ema[2] - 0.51240) < 0.0001);

#if 0 //由于修改了SMA计算过程，MA的抛弃数量为零，下面的测试用例失效
    /** @arg 待计算的数据存在非零的discard */
    Indicator ma = MA(data, 2);
    ema = EMA(ma, 2);
    BOOST_CHECK(ma.discard() == 1);
    BOOST_CHECK(ema.discard() == 1);
    BOOST_CHECK(ma[0] == Null<price_t>());
    BOOST_CHECK(ma[1] == 0.5);
    BOOST_CHECK(ma[2] == 1.5);
    BOOST_CHECK(ema[0] == Null<price_t>());
    BOOST_CHECK(ema[1] == 0.5);
    BOOST_CHECK(std::fabs(ema[2] - 1.16667) < 0.0001 );
#endif

    /** @arg operator() */
    Indicator ma = MA(data, 2);
    Indicator expect = EMA(ma, 2);
    ema = EMA(2);
    Indicator result = ema(ma);
    BOOST_CHECK(result.size() == expect.size());
    for (size_t i = 0; i < expect.size(); ++i) {
        BOOST_CHECK(result[i] == expect[i]);
    }
}

/** @} */


#include "simulate.h"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <random>
#include <string>
#include <vector>
#include <tuple>

#include <armadillo>
#include <boost/math/distributions/normal.hpp>

using namespace std;

// constants
const double ALPHA = 0.05;

int main(int argc, char *argv[]) {
  clock_t startTime = clock();
  ios::sync_with_stdio(false); cin.tie(NULL);
  // parse arguments
  int ITERATIONS = stoi(argv[1]); // simulation runs
  int N = stoi(argv[2]);          // 1/2 sample size
  double POWER = stod(argv[3]);    // power

  // calculate variance used for generators
  double variance = calculateVariance(N, ALPHA, POWER);
  double standardDeviation = sqrt(variance);

  // set up generators
  random_device randomDevice;
  mt19937_64 rng;
  rng.seed(randomDevice());
  normal_distribution<double> normalMean0(0, standardDeviation);
  normal_distribution<double> normalMean1(1, standardDeviation);

  // store and print results
  vector<tuple<double, double, int, int, double, double>> results(ITERATIONS); // initial p-value, "cheated" beta, set size, subset size, balance p-value, "cheated" p-value
  cout << setprecision(12) << fixed;
  cout << "2N\tpower\tp.value\tbeta\tset.size\tsubset.size\tbalance.p.value\tfake.p.value" << endl;
  // randomly assign treatment
  vector<int> X(2*N);
  for (int i = 0; i < N; ++i) X[i] = 1;
  shuffle(X.begin() , X.end(), rng);
  arma::mat Z(2*N, 2*N, arma::fill::zeros); // pre allocate Z matrix
  for (int i = 0; i < ITERATIONS; ++i) {
    // generate response
    arma::Col<double> Y(2*N);
    for (int j = 0; j < 2*N; ++j) {
      Y(j) = X[j] == 0 ? normalMean0(rng) : normalMean1(rng);
    }
    results[i] = simulate(Y, X, standardDeviation, true, Z, rng);
    cout << 2*N << '\t' << POWER << '\t'
         << get<0>(results[i]) << '\t'
         << get<1>(results[i]) << '\t'
         << get<2>(results[i]) << '\t'
         << get<3>(results[i]) << '\t'
         << get<4>(results[i]) << '\t'
         << get<5>(results[i]) << endl;
  }

  double duration = (clock() - startTime) / (double) CLOCKS_PER_SEC;
  cerr << "Time taken (seconds): " << duration << endl;
  return 0;
}


// requires LAPACK v. 3.0

#include <cstddef>
#include <iostream>
#include <algorithm>
#include <boost/numeric/bindings/lapack/driver/gesdd.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

typedef double real_t;
typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::vector<real_t> v_t;

int main() {

  cout << endl;

  size_t m = 3, n = 4;
  size_t minmn = m < n ? m : n;
  m_t a (m, n);
  a(0,0) = 2.;  a(0,1) = 2.;  a(0,2) = 2.;   a(0,3) = 2.;
  a(1,0) = 1.7; a(1,1) = 0.1; a(1,2) = -1.7; a(1,3) = -0.1;
  a(2,0) = 0.6; a(2,1) = 1.8; a(2,2) = -0.6; a(2,3) = -1.8;

  m_t a2 (a); // for parts 2 & 3

  print_m (a, "A");
  cout << endl;

  v_t s (minmn);
  m_t u (m, minmn);
  m_t vt (minmn, n);

//  lapack::gesdd (a, s, u, vt);
  lapack::gesdd ('S', a, s, u, vt);

  print_v (s, "s");
  cout << endl;
  print_m (u, "U");
  cout << endl;
  print_m (vt, "V^T");
  cout << endl << endl;

  // part 2

  // singular values and singular vectors satisfy  A v_i == s_i u_i
  for (int i = 0; i < minmn; ++i) {
    cout << "A v_" << i << " == s_" << i << " u_" << i << endl;
    v_t avi = ublas::prod (a2, ublas::row (vt, i));
    print_v (avi);
    v_t siui = s[i] * ublas::column (u, i);
    print_v (siui);
    cout << endl;
  }
  cout << endl;

  // singular values and singular vectors satisfy  A^T u_i == s_i v_i
  for (int i = 0; i < minmn; ++i) {
    cout << "A^T u_" << i << " == s_" << i << " v_" << i << endl;
    v_t atui = ublas::prod (trans (a2), ublas::column (u, i));
    print_v (atui);
    v_t sivi = s[i] * ublas::row (vt, i);
    print_v (sivi);
    cout << endl;
  }
  cout << endl;

  // part 3

//  lapack::gesdd (a2, s);
  lapack::gesdd ('N', a2, s, u, vt);

  print_v (s, "singular values only");
  cout << endl;
}


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/ModelCoefficients.h>
#include <vector>

#include <time.h>

#define PI 3.1415926

class Point
{
    public:
    double x;
    double y;
    double z;
    Point(double v[]):x(v[0]),y(v[1]),z(v[2]){}
};

//求法向量
double* get_normal(Point p1, Point p2, Point p3)
{
    double* norm_vec;
    norm_vec = new double[3];
    double a = ( (p2.y-p1.y)*(p3.z-p1.z)-(p2.z-p1.z)*(p3.y-p1.y));

    double b = ( (p2.z-p1.z)*(p3.x-p1.x)-(p2.x-p1.x)*(p3.z-p1.z));

    double c = ( (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x));

    double norm = std::sqrt(a*a+b*b+c*c);

    norm_vec[0] = a/norm;
    norm_vec[1] = b/norm;
    norm_vec[2] = c/norm;
    // double v[3] = {a/norm, b/norm, c/norm};

    return norm_vec;
}

//求旋转矩阵4*4

double get_theta_from_2_vec(double*v1,double*v2)
{
    double costheta = (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) / (0.000001+std::sqrt((v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2])*(v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2])));
    return std::acos(costheta);
}

Eigen::Matrix4d get_mat(Eigen::VectorXd coed)
{
    double z[3] = {0,0,1};
    // double a[3] = {-0.0554246, 0.0372348, 0.997768};
    // double a[3] = {-0.0842539, 0.882628, 0.462459};
    double a[3] = {coed[0], coed[1], coed[2]};

    std::cout<<"normal vector: "<<coed[0]<<", "<<coed[1]<<", "<<coed[2]<<std::endl;
    // double za = z[0]*a[0] + z[1]*a[1] + z[2]*a[2];

    double o[3] = {0,0,0};
    Point p1(z);
    Point p2(a);
    Point p3(o);
    double* axis_line = get_normal(p1, p2, p3);
    std::cout<<axis_line[1] << std::endl;

    // double a_xOy[3] = {a[0], a[1],0};
    // double x_vec[3] = {1,0,0};
    // double thetaxOz= get_theta_from_2_vec(a_xOy,x_vec);
    // //Eigen::Quaterniond q_rotate_to_xOz (std::cos(thetaxOz/2),std::sin(thetaxOz/2),0,0);
    // Eigen::Quaterniond q_rotate_to_xOz (std::cos(thetaxOz/2),0,0,std::sin(thetaxOz/2));

    // double a_xOy_to_z[3] = {a[0],0, a[2]};
    // double z_vec[3] = {0,0,1};
    // double theta_toz = 1.0*get_theta_from_2_vec(a_xOy_to_z, z_vec);
    // Eigen::Quaterniond q_rotate_to_z(std::cos(theta_toz/2),0,std::sin(theta_toz/2),0);

    // double a_z0y_to_x[3] = {0, a[1], a[2]};
    // double y_vec[3] = {0,1,0};
    // double theta_yox = get_theta_from_2_vec(a_z0y_to_x,y_vec);
    // Eigen::Quaterniond q_rotate_to_x(std::cos(theta_yox/2),std::sin(theta_yox/2),0,0);

    //四元数{cos(r/2), nx*sin(r/2), ny*sin(r/2), nz*sin(r/2)}
    //Eigen::Quaterniond q = q_rotate_to_xOz*q_rotate_to_z;
    //Eigen::Quaterniond q = q_rotate_to_xOz*q_rotate_to_z*q_rotate_to_x;

    double theta_all = -1.0*get_theta_from_2_vec(a, z);
    Eigen::Quaterniond q(std::cos(theta_all/2),axis_line[0]*std::sin(theta_all/2),axis_line[1]*std::sin(theta_all/2),axis_line[2]*std::sin(theta_all/2));
    // Eigen::Quaterniond q(std::cos(theta_all/2),-0.557652*std::sin(theta_all/2),-0.830075*std::sin(theta_all/2),0);

    // Eigen::Quaterniond q = q_rotate_to_xOz;
    Eigen::Matrix3d mat3 = q.toRotationMatrix();
    Eigen::Matrix4d mat4 = Eigen::Matrix4d::Identity();
    mat4.block(0,0,3,3) = mat3;
    mat4(2, 3) = coed[3];

    std::cout << mat3 << std::endl;

    return mat4;
}

void
showHelp ()
{
  std::cout << std::endl;
  std::cout << "Usage: ./rotate_pcd input.pcd [Options]"<< std::endl;
  std::cout << "Options:" << std::endl;
  std::cout << "     -h:                        Show this help." << std::endl;
  std::cout << "     -v:                        Show pcl viewer." << std::endl;
  std::cout << "     -o  output.pcd:            Save inlier points to pcd file." << std::endl;
  std::cout << "     -t  threshold:             Distance threshold (default 0.1)" << std::endl;
  std::cout << "     -cs coord_scale:           Coordinate Scale of pcl viewer (default 100.0)" << std::endl;
}

int main (int argc, char** argv)
{

    std::clock_t t1 = std::clock();
    //creates a PointCloud<PointXYZ> boost shared pointer and initializes it.
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

    if (pcl::console::find_switch (argc, argv, "-h"))
    {
        showHelp ();
        exit (0);
    }
    std::string input_path;
    if (argc < 2)
    {
        showHelp ();
        exit (0);
    }
    else
    {
        input_path = argv[1];
        if (pcl::io::loadPCDFile<pcl::PointXYZ> (input_path, *source_cloud) == -1) //* load the file
        {
            std::cerr << "Input path: " << input_path << std::endl;
            PCL_ERROR ("Couldn't read file model, please try again. \n");
            showHelp();
            return (-1);
        }
    }

    std::size_t sz = source_cloud->points.size();
    std::cout<<"Points size: "<<sz<<std::endl;

    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (source_cloud));
    Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1);
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);

    double threshold;
    if (pcl::console::parse_argument (argc, argv, "-t", threshold) != -1)
    {
      std::cout<<"Distance threshold: "<<threshold<<std::endl;
    }
    else
    {
      threshold = 0.1;
      std::cout<<"Distance threshold: 0.1"<<std::endl;
    }

    ransac.setDistanceThreshold (threshold);
    ransac.computeModel();
    // ransac.getInliers(inliers);
    ransac.getModelCoefficients(coef);

    Eigen::VectorXd coed = coef.cast<double>();

    std::clock_t t2 = std::clock();

    Eigen::Matrix4d transform_1_d = get_mat(coed);

    // std::cout << transform_1_d << std::endl;

    // Executing the transformation
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    /*
    void pcl::transformPointCloud(const pcl::PointCloud< PointT > & cloud_in,
                                    pcl::PointCloud< PointT > &  cloud_out,
                                    const Eigen::Matrix4f &  transform  )
    */
    // Apply an affine transform defined by an Eigen Transform.

    Eigen::Matrix4f transform_1_f = transform_1_d.cast<float>();

    pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_1_f);

    std::clock_t t3 = std::clock();
    // pcl::io::savePCDFileASCII("model_1_transform.pcd", *transformed_cloud);
    std::string output_path;
    if (pcl::console::parse_argument (argc, argv, "-o", output_path) != -1)
    {
        std::cout << "Rotated model will saved at: "<<output_path<<std::endl;

    }
    else
    {
        output_path = "transform.pcd";
        std::cout << "Rotated model will saved at: transform.pcd" <<std::endl;
    }
    if (pcl::io::savePCDFileASCII(output_path, *transformed_cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't save file model, please try again. \n");
        return (-1);
    }

    std::clock_t t4 = std::clock();

    std::cout <<"Full cost: "<<(double)(t4-t1)/CLOCKS_PER_SEC<<std::endl;

    std::cout <<"RANSaC cost: "<<(double)(t2-t1)/CLOCKS_PER_SEC<<std::endl;
    std::cout <<"Transform cost: "<<(double)(t3-t1)/CLOCKS_PER_SEC<<std::endl;
    // Visualization
    if (pcl::console::find_switch (argc, argv, "-v"))
    {
        printf(  "\nPoint cloud colors :  white  = original point cloud\n"
        "                        red  = transformed point cloud\n");
        pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

        // Define R,G,B colors for the point cloud
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
        //We add the point cloud to the viewer and pass the color handler
        viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
        viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

        double coord_scale;
        if (pcl::console::parse_argument (argc, argv, "-cs", coord_scale) != -1)
        {
            std::cout<<"Display coordinate scale: "<<coord_scale<<std::endl;
        }
        else
        {
            coord_scale = 100.0;
            std::cout<<"Display coordinate scale: 100.0"<<std::endl;
        }
        viewer.addCoordinateSystem (coord_scale, 0);  //Adds 3D axes describing a coordinate system to screen at 0,0,0.
        viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");

        viewer.setPosition(800, 400); // Setting visualiser window position

        while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
        viewer.spinOnce ();
        }
    }

  return 0;
}


#include <igl/boundary_facets.h>
#include <igl/cotmatrix.h>
#include <igl/invert_diag.h>
#include <igl/jet.h>
#include <igl/massmatrix.h>
#include <igl/min_quad_with_fixed.h>
#include <igl/readOFF.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <iostream>
#include "tutorial_shared_path.h"

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  MatrixXd V;
  MatrixXi F;
  igl::readOFF(TUTORIAL_SHARED_PATH "/cheburashka.off",V,F);

  // Two fixed points
  VectorXi b(2,1);
  // Left hand, left foot
  b<<4331,5957;
  VectorXd bc(2,1);
  bc<<1,-1;

  // Construct Laplacian and mass matrix
  SparseMatrix<double> L,M,Minv,Q;
  igl::cotmatrix(V,F,L);
  igl::massmatrix(V,F,igl::MASSMATRIX_TYPE_VORONOI,M);
  igl::invert_diag(M,Minv);
  // Bi-Laplacian
  Q = L * (Minv * L);
  // Zero linear term
  VectorXd B = VectorXd::Zero(V.rows(),1);

  VectorXd Z,Z_const;
  {
    // Alternative, short hand
    igl::min_quad_with_fixed_data<double> mqwf;
    // Empty constraints
    VectorXd Beq;
    SparseMatrix<double> Aeq;
    igl::min_quad_with_fixed_precompute(Q,b,Aeq,true,mqwf);
    igl::min_quad_with_fixed_solve(mqwf,B,bc,Beq,Z);
  }

  {
    igl::min_quad_with_fixed_data<double> mqwf;
    // Constraint forcing difference of two points to be 0
    SparseMatrix<double> Aeq(1,V.rows());
    // Right hand, right foot
    Aeq.insert(0,6074) = 1;
    Aeq.insert(0,6523) = -1;
    Aeq.makeCompressed();
    VectorXd Beq(1,1);
    Beq(0) = 0;
    igl::min_quad_with_fixed_precompute(Q,b,Aeq,true,mqwf);
    igl::min_quad_with_fixed_solve(mqwf,B,bc,Beq,Z_const);
  }

  // Pseudo-color based on solution
  struct Data{
    MatrixXd C,C_const;
  } data;
  // Use same color axes
  const double min_z = std::min(Z.minCoeff(),Z_const.minCoeff());
  const double max_z = std::max(Z.maxCoeff(),Z_const.maxCoeff());
  igl::jet(      Z,min_z,max_z,data.C);
  igl::jet(Z_const,min_z,max_z,data.C_const);

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().show_lines = false;
  viewer.data().set_colors(data.C);

  viewer.callback_key_down =
    [](igl::opengl::glfw::Viewer& viewer,unsigned char key,int mod)->bool
    {
      if(key == ' ')
      {
        Data & data = *static_cast<Data*>(viewer.callback_key_down_data);
        static bool toggle = true;
        viewer.data().set_colors(toggle?data.C_const:data.C);
        toggle = !toggle;
        return true;
      }else
      {
        return false;
      }
    };
  viewer.callback_key_down_data = &data;
  cout<<
    "Press [space] to toggle between unconstrained and constrained."<<endl;
  viewer.launch();
}


#include <complex>
#include <limits>

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <rsvd/GramSchmidt.hpp>

using Eigen::Index;
using Rsvd::Internal::modifiedGramSchmidt;

template <typename T> struct GramSchmidt : public ::testing::Test {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
  using RealType = typename Eigen::NumTraits<T>::Real;

  const unsigned int prngSeed = 777;

  const RealType macheps = std::numeric_limits<RealType>::epsilon();
};

using NumericalTypes = ::testing::Types<float, double, std::complex<float>, std::complex<double>>;

TYPED_TEST_CASE(GramSchmidt, NumericalTypes, );

/// \brief MGS function asserts that the number of columns is less or equal than the number of
/// columns. Otherwise, the columns are linearly dependent.
TYPED_TEST(GramSchmidt, MoreColumnsThanRows) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType a = MatrixType::Zero(4, 6);
  ASSERT_DEATH(modifiedGramSchmidt(a),
               "Assertion `\\w+\\.cols\\(\\) <= \\w+\\.rows\\(\\)' failed");
}

/// \brief Test deflation on linearly dependent columns. All linearly-dependent columns must be
/// filled with zeros.
TYPED_TEST(GramSchmidt, LinearlyDependentColumnsEasy) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType testee = MatrixType::Ones(10, 7);
  modifiedGramSchmidt(testee);

  MatrixType reference = MatrixType::Zero(10, 7);
  reference.col(0).setOnes();
  reference.col(0) /= sqrt(10);

  ASSERT_TRUE(testee.isApprox(reference));
}

/// \brief Test the deflation case when (new) independent directions come after linearly dependent
/// ones.
TYPED_TEST(GramSchmidt, LinearlyDependentColumnsOrder) {
  using MatrixType = typename TestFixture::MatrixType;

  MatrixType testee(5, 5);
  testee << 1, 1, 1, 1, 1, // 1
      0, 1, 1, 1, 1,       // 2
      0, 0, 0, 1, 1,       // 3
      0, 0, 0, 0, 1,       // 4
      0, 0, 0, 0, 0;       // 5
  modifiedGramSchmidt(testee);

  MatrixType reference(5, 5);
  reference << 1, 0, 0, 0, 0, // 1
      0, 1, 0, 0, 0,          // 2
      0, 0, 0, 1, 0,          // 3
      0, 0, 0, 0, 1,          // 4
      0, 0, 0, 0, 0;          // 5

  ASSERT_TRUE(testee.isApprox(reference));
}

/// \brief Test the deflation case when the columns are numerically linearly dependent.
TYPED_TEST(GramSchmidt, LinearlyDependentColumnsNumericalRankLoss) {
  using MatrixType = typename TestFixture::MatrixType;

  const auto e = 10 * TestFixture::macheps;

  MatrixType testee(5, 5);
  testee << 1, 1, 1, 1, 1, // 1
      0, 1, 1, 1, 1,       // 2
      0, 0, e, 1, 1,       // 3
      0, 0, 0, 0, 1,       // 4
      0, 0, 0, 0, 0;       // 5
  modifiedGramSchmidt(testee);

  MatrixType reference(5, 5);
  reference << 1, 0, 0, 0, 0, // 1
      0, 1, 0, 0, 0,          // 2
      0, 0, 0, 1, 0,          // 3
      0, 0, 0, 0, 1,          // 4
      0, 0, 0, 0, 0;          // 5

  ASSERT_TRUE(testee.isApprox(reference));
}

/// \brief A matrix which is already orthonormal must remain the same.
TYPED_TEST(GramSchmidt, AlreadyOrthogonalIdentity) {
  using MatrixType = typename TestFixture::MatrixType;

  const MatrixType reference = MatrixType::Identity(10, 4);
  MatrixType testee = MatrixType::Identity(10, 4);

  modifiedGramSchmidt(testee);

  ASSERT_TRUE(testee.isApprox(reference));
}

/// \brief A matrix which is already orthonormal must remain the same, larger matrix without
/// structure.
TYPED_TEST(GramSchmidt, AlreadyOrthogonalRandom) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);

  const Index numRows = 50;
  const Index numCols = 25;

  auto source = MatrixType::Random(numRows, numCols);
  Eigen::ColPivHouseholderQR<MatrixType> qr(source);
  const auto reference = qr.householderQ() * MatrixType::Identity(numRows, numCols);

  auto testee = reference;
  modifiedGramSchmidt(testee);

  ASSERT_TRUE(testee.isApprox(reference));
}

/// \brief Test if the matrix columns have unit norm after orthonormalization.
TYPED_TEST(GramSchmidt, CorrectNormalization) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);

  const Index numRows = 50;
  const Index numCols = 25;

  MatrixType testee = MatrixType::Random(numRows, numCols);
  modifiedGramSchmidt(testee);

  for (Index i = 0; i < numCols; ++i) {
    ASSERT_NEAR(testee.col(i).norm(), 1, 2 * TestFixture::macheps);
  }
}

/// \brief Test if the matrix columns are mutually orthogonal after orthonormalization.
TYPED_TEST(GramSchmidt, CorrectOrthogonality) {
  using MatrixType = typename TestFixture::MatrixType;

  std::srand(TestFixture::prngSeed);

  const Index numRows = 50;
  const Index numCols = 25;

  MatrixType testee = MatrixType::Random(numRows, numCols);
  modifiedGramSchmidt(testee);

  for (Index i = 0; i < numCols; ++i) {
    for (Index j = 0; j < i; ++j) {
      const auto res = std::abs(testee.col(i).dot(testee.col(j)));
      ASSERT_NEAR(res, 0, 2 * TestFixture::macheps);
    }
  }
}


//******************************************************************************
//  project:		Weather-based simulation framework (WBSF)
//	Programmer:     Rémi Saint-Amant
//
//  It under the terms of the GNU General Public License as published by
//     the Free Software Foundation
//  It is provided "as is" without express or implied warranty.
//
// Class:	CStatistic, CStatisticEx, CStatisticXW, CStatisticXY, CStatisticXYEx, CStatisticXYW
//
//****************************************************************************
// 02/12/2015   Rémi Saint-Amant  Add weathed statistic and quartile
// 22/12/2014   Rémi Saint-Amant  Add RANGE as statistic type
// 24/05/2013	Rémi Saint-Amant  Add standart deviation over population (N)
// 20/02/2009	Rémi Saint-Amant  Add classification class
// 04/03/2004	Rémi Saint-Amant  Remove the old GetVaporPressureDeficit
// 20/11/2003	Rémi Saint-Amant  Initiale Version
//****************************************************************************

#include "stdafx.h"
#include <math.h>
#include <float.h>
#include <algorithm>
#include <boost/math/distributions/normal.hpp>

#include "Basic/Statistic.h"
#include "Basic/UtilMath.h"
#include "WeatherBasedSimulationString.h"

using namespace std;

namespace WBSF
{

	//this static variable is share by CStatistic and CStatisticXY
	double STAT_VMISS = -9999999.0;

	const char* CStatistic::NAME[NB_STATXY_TYPE_EX] =
	{
		"Lowest", "Mean", "Sum", "Sum²", "StandardDeviation (N-1)", "StandardDeviation (N)", "StandardError", "CoeficientOfVariation", "Variance", "Highest", "TotalSumOfSquares", "QuadraticMean", "Range", "NbValue",
		"MeanAbsoluteDeviation", "Skewness", "Kurtosis", "Median", "Mode", "Lo", "Q¹", "Q³", "Hi", "IQR",
		"MeanX", "MeanY", "Intercept", "Slope", "Covariance", "Correlation", "Bias", "MeanAbsolutError", "RootMeanSquareError", "ResidualSumOfSquare", "CoeficientOfDetermination", "CoeficientOfCorrelation", "R²",
		"TheilSenIntercept", "TheilSenSlope", "Likelihood"
		//, "LogLikelihood2"
	};

	StringVector CStatistic::TITLE;
	const char* CStatistic::GetTitle(size_t s)
	{
		assert(s < NB_STATXY_TYPE_EX);
		if (TITLE.empty())
			TITLE.LoadString(IDS_STR_STATISTIC, "|;");

		assert(TITLE.size() == NB_STATXY_TYPE_EX);

		return TITLE[s].c_str();
	}

	void CStatistic::ReloadString()
	{
		TITLE.clear();
	}

	CStatistic::CStatistic()
	{
		Reset();
	}

	void CStatistic::Reset()
	{
		m_nbValues = 0;

		m_lowest = DBL_MAX;
		m_sum = 0;
		m_sum² = 0;
		m_hightest = -DBL_MAX;
	}

	CStatistic& CStatistic::operator=(const CStatistic& in)
	{
		if (&in != this)
		{
			m_nbValues = in.m_nbValues;

			m_lowest = in.m_lowest;
			m_sum = in.m_sum;
			m_sum² = in.m_sum²;
			m_hightest = in.m_hightest;
		}

		return *this;
	}

	CStatistic& CStatistic::operator+=(double value)
	{
		_ASSERTE(!_isnan(value));

		m_nbValues++;

		if (value < m_lowest)
			m_lowest = value;

		m_sum += value;
		m_sum² += Square(value);
		if (value > m_hightest)
			m_hightest = value;

		return *this;
	}

	CStatistic& CStatistic::operator+=(const CStatistic& statistic)
	{
		m_nbValues += statistic.m_nbValues;

		if (statistic.m_lowest < m_lowest)
			m_lowest = statistic.m_lowest;

		m_sum += statistic.m_sum;
		m_sum² += statistic.m_sum²;
		if (statistic.m_hightest > m_hightest)
			m_hightest = statistic.m_hightest;

		return *this;
	}

	double CStatistic::operator[](size_t type)const
	{
		_ASSERTE(type >= 0 && type < NB_STAT_TYPE);
		const CStatistic& me = *this;

		double value = STAT_VMISS;

		if (m_nbValues > 0 || type == NB_VALUE)
		{
			size_t nbVal = m_nbValues == 1 ? m_nbValues : m_nbValues - 1;
			switch (type)
			{
			case LOWEST: value = m_lowest; break;
			case SUM:value = m_sum; break;
			case SUM²:value = m_sum²; break;
			case MEAN:value = m_sum / m_nbValues; break;
			case STD_DEV:value = sqrt(std::max(0.0, (m_sum² - Square(m_sum) / m_nbValues)) / (nbVal)); break;
			case STD_DEV_OVER_POP:value = sqrt(std::max(0.0, (m_sum² - Square(m_sum) / m_nbValues)) / (m_nbValues)); break;
			case STD_ERR:value = (me[STD_DEV] / sqrt((double)m_nbValues)); break;
			case TSS: value = std::max(0.0, m_sum² - Square(m_sum) / m_nbValues); break;
			case COEF_VAR:
				if (!(m_sum² > 0 && m_sum == 0))
				{
					value = m_sum != 0 ? me[STD_DEV] / me[MEAN] : 0;//est-ce que doit accepter les valeur négative???
					break;
				}
			case VARIANCE:value = std::max(0.0, (m_sum² - Square(m_sum) / m_nbValues) / (nbVal)); break;
			case HIGHEST:value = m_hightest; break;
			case QUADRATIC_MEAN:  value = sqrt(m_sum² / m_nbValues); break;
			case RANGE: value = m_hightest - m_lowest; break;
			case NB_VALUE:value = (double)m_nbValues; break;
			default: _ASSERTE(false);//not supported by this statistic class
			}
		}

		_ASSERTE(!_isnan(value));
		return value;
	}

	double CStatistic::GetVMiss(){ return STAT_VMISS; }
	void CStatistic::SetVMiss(double vmiss){ STAT_VMISS = vmiss; }

	bool CStatistic::operator==(const CStatistic& in)const
	{
		bool bEqual = true;
		if (m_nbValues != in.m_nbValues)bEqual = false;
		if (fabs(m_lowest - in.m_lowest) > EPSILON_DATA)bEqual = false;
		if (fabs(m_sum - in.m_sum) > EPSILON_DATA)bEqual = false;
		if (fabs(m_sum² - in.m_sum²) > EPSILON_DATA)bEqual = false;
		if (fabs(m_hightest - in.m_hightest) > EPSILON_DATA)bEqual = false;

		return bEqual;
	}
	//*************************************************************
	//CStatisticEx : keep in memory all elements for MAD computation

	CStatisticEx::CStatisticEx()
	{
		Reset();
	}

	void CStatisticEx::Reset()
	{
		CStatistic::Reset();
		m_values.clear();
		m_sorted.clear();
	}

	CStatisticEx& CStatisticEx::operator=(const CStatisticEx& in)
	{
		CStatistic::operator=(in);
		m_values = in.m_values;
		m_sorted.clear();

		return *this;
	}

	CStatisticEx&  CStatisticEx::operator+=(double value)
	{
		CStatistic::operator+=(value);
		m_values.push_back(value);
		m_sorted.clear();

		return *this;
	}

	CStatisticEx&  CStatisticEx::operator+=(const CStatisticEx& statistic)
	{
		CStatistic::operator+=(statistic);
		m_values.insert(m_values.end(), statistic.m_values.begin(), statistic.m_values.end());
		m_sorted.clear();

		return *this;
	}

	void CStatisticEx::sort()const
	{
		if (m_sorted.empty())
		{
			CStatisticEx& me = const_cast<CStatisticEx&>(*this);
			me.m_sorted = m_values;
			std::sort(me.m_sorted.begin(), me.m_sorted.end());
		}

		ASSERT(m_sorted.size() == m_values.size());
	}

	double CStatisticEx::operator[](size_t type)const
	{
		_ASSERTE(m_values.size() == m_nbValues);
		_ASSERTE(type >= 0 && type < NB_STAT_TYPE_EX);
		const CStatisticEx& me = *this;

		double value = STAT_VMISS;

		if (m_nbValues > 0 || type == NB_VALUE)
		{
			if (type == MAD)
			{
				value = 0;
				double mean = CStatistic::operator[](MEAN);
				for (int i = 0; i < (int)m_values.size(); i++)
					value += fabs(m_values[i] - mean);

				value /= m_values.size();
			}
			else if (type == SKEWNESS)
			{
				value = 0;
				double mean = CStatistic::operator[](MEAN);
				for (int i = 0; i < (int)m_values.size(); i++)
					value += pow(m_values[i] - mean, 3);

				double SD = CStatistic::operator[](STD_DEV);
				double tmp = ((m_values.size() - 1)*pow(SD, 3));

				if (tmp != 0)
					value /= tmp;
			}
			else if (type == KURTOSIS)
			{
				value = 0;
				double mean = CStatistic::operator[](MEAN);
				for (int i = 0; i < (int)m_values.size(); i++)
					value += pow(m_values[i] - mean, 4);

				double SD = CStatistic::operator[](STD_DEV);
				double tmp = ((m_values.size() - 1)*pow(SD, 4));

				if (tmp != 0)
					value /= tmp;
			}
			else if (type == MEDIAN || type == Q²)
			{
				sort();

				size_t N1 = (m_sorted.size() + 1) / 2 - 1;
				size_t N2 = m_sorted.size() / 2 + 1 - 1;
				ASSERT(N2 == N1 || N2 == N1 + 1);

				value = (m_sorted[N1] + m_sorted[N2]) / 2.0;
			}
			else if (type == MODE )
			{
				sort();

				int maxOcc = 0;
				int occ = 0;
				double lastVal = -DBL_MAX;
				for (size_t i = 0; i < m_sorted.size(); i++)
				{
					if (fabs(m_sorted[i] - lastVal) < 0.000001)
					{
						occ++;
					}
					else
					{
						occ = 1;
						lastVal = m_sorted[i];
					}

					if (occ > maxOcc)
					{
						maxOcc = occ;
						value = lastVal;
					}
				}
			}
			else if (type == Qᴸ)
			{
				sort();

				double Q1 = me[Q¹];
				double IQR = me[INTER_Q];
				value = max(Q1 - 1.5*IQR, m_sorted.front());
			}
			else if (type == Q¹)
			{
				size_t N1 = (m_sorted.size() + 1) / 4 - 1;
				size_t N2 = m_sorted.size() / 4 + 1 - 1;
				ASSERT(N2 == N1 || N2 == N1 + 1);

				sort();
				value = (m_sorted[N1] + m_sorted[N2]) / 2.0;
			}
			else if (type == Q³)
			{
				size_t N1 = 3 * (m_sorted.size() + 1) / 4 - 1;
				size_t N2 = 3 * m_sorted.size() / 4 + 1 - 1;
				ASSERT(N2 == N1 || N2 == N1 + 1);

				sort();
				value = (m_sorted[N1] + m_sorted[N2]) / 2.0;
			}
			else if (type == Qᴴ)
			{
				sort();

				double Q3 = me[Q³];
				double IQR = me[INTER_Q];
				value = min(Q3 + 1.5*IQR, m_sorted.back());
			}
			else if (type == INTER_Q)
			{
				value = me[Q³] - me[Q¹];
			}
			else
			{
				value = CStatistic::operator[](type);
			}
		}

		_ASSERTE(!_isnan(value));
		return value;
	}

	double CStatisticEx::percentil(double p)const
	{
		ASSERT(p >= 0 && p <= 100);
		sort();//create sorted array

		double value = STAT_VMISS;

		if (!m_sorted.empty())
		{
			size_t n = max(size_t(1), size_t(ceil(p / 100 * m_sorted.size()))) - size_t(1);
			value = m_sorted[n];
		}

		return value;
	}

	//*************************************************************
	//Weighted CStatistic
	CStatisticXW::CStatisticXW()
	{
		m_wx = 0;
		m_wx² = 0;
	}

	void CStatisticXW::Reset()
	{
		m_x.clear();
		m_w.clear();
		m_wx = 0;
		m_wx² = 0;
	}

	CStatisticXW& CStatisticXW::operator=(const CStatisticXW& in)
	{
		m_x = in.m_x;
		m_w = in.m_w;
		m_wx = in.m_wx;
		m_wx² = in.m_wx²;

		return *this;
	}

	CStatisticXW& CStatisticXW::Add(double x, double w)
	{
		m_x.insert(x);
		m_w.insert(w);
		m_wx += w*x;
		m_wx² += w*x*x;

		return *this;
	}

	CStatisticXW& CStatisticXW::operator+=(const CStatisticXW& statistic)
	{
		m_x += statistic.m_x;
		m_w += statistic.m_w;
		m_wx += statistic.m_wx;
		m_wx² += statistic.m_wx²;

		return *this;
	}

	double CStatisticXW::operator[](size_t type)const
	{
		ASSERT(m_x[NB_VALUE] == m_w[NB_VALUE]);
		ASSERT(type >= 0 && type < NB_STAT_TYPE);

		double value = STAT_VMISS;

		const CStatisticXW& me = *this;
		double nbValues = m_x[NB_VALUE];

		if (nbValues > 0 || type == NB_VALUE)
		{
			switch (type)
			{
			case LOWEST:	value = m_x[LOWEST]; break;
			case SUM:		value = m_wx; break;
			case SUM²:		value = m_wx²; break;
			case MEAN:		value = m_wx / m_w[SUM]; break;
				//from https://stat.ethz.ch/pipermail/r-help/2008-July/168762.html
			case STD_DEV:
			case STD_DEV_OVER_POP:
			{
				if ((Square(m_w[SUM]) - m_w[SUM²]) != 0)
				{
					double mean = me[MEAN];
					double sum = 0;
					for (size_t i = 0; i < m_x().size(); i++)
						sum += m_w(i) * Square(m_x(i) - mean);

					value = m_w[SUM] / (Square(m_w[SUM]) - m_w[SUM²]) *sum;
				}
				break;
			}
			case STD_ERR:value = (me[STD_DEV] / sqrt(m_w[SUM])); break;
			case TSS: value = std::max(0.0, m_wx² * m_w[SUM] - Square(m_wx)); break;
			case COEF_VAR:
				if (!(m_wx²>0 && m_wx == 0))
				{
					value = m_wx > 0 ? me[STD_DEV] / me[MEAN] : 0;
					break;
				}

			case VARIANCE: value = ((Square(m_w[SUM]) - m_w[SUM²]) != 0) ? (m_wx² * m_w[SUM] - Square(m_wx)) / (Square(m_w[SUM]) - m_w[SUM²]) : VMISS; break;
			case HIGHEST: value = m_x[HIGHEST]; break;
			case QUADRATIC_MEAN:  value = sqrt(m_wx² / m_w[SUM]); break;
			case RANGE: value = m_x[RANGE]; break;
			case NB_VALUE: value = nbValues; break;
			default: ASSERT(false);//not supported by this class
			}
		}

		_ASSERTE(!_isnan(value));

		return value;
	}

	//*************************************************************
	//CStatisticXY: statistic of 2 variables

	CStatisticXY::CStatisticXY()
	{
		Reset();
	}

	CStatisticXY& CStatisticXY::operator=(const CStatisticXY& in)
	{
		if (&in != this)
		{
			m_x = in.m_x;
			m_y = in.m_y;
			m_lowest = in.m_lowest;
			m_xy = in.m_xy;
			m_e = in.m_e;
			m_rs = in.m_rs;
			m_ae = in.m_ae;
			m_hightest = in.m_hightest;
		}

		return *this;
	}

	void CStatisticXY::Reset()
	{
		m_x.clear();
		m_y.clear();

		m_lowest = DBL_MAX;
		m_xy = 0;
		m_e = 0;
		m_rs = 0;
		m_ae = 0;
		m_hightest = -DBL_MAX;
	}

	//x = pred, y = obs
	CStatisticXY& CStatisticXY::Add(double x, double y)
	{
		m_x += x;
		m_y += y;

		double e = (x - y);
		if (e < m_lowest)
			m_lowest = e;

		m_xy += x*y;
		m_e += (x - y);
		m_rs += Square(x - y);
		m_ae += abs(x - y);

		if (e > m_hightest)
			m_hightest = e;

		return *this;
	}

	CStatisticXY& CStatisticXY::operator+=(const CStatisticXY& in)
	{
		m_x += in.m_x;
		m_y += in.m_y;

		if (in.m_lowest < m_lowest)
			m_lowest = in.m_lowest;

		m_xy += in.m_xy;
		m_e += in.m_e;
		m_rs += in.m_rs;
		m_ae += in.m_ae;

		if (in.m_hightest > m_hightest)
			m_hightest = in.m_hightest;

		return *this;
	}

	double CStatisticXY::operator[](size_t type)const
	{
		_ASSERTE(m_x[NB_VALUE] == m_y[NB_VALUE]);
		_ASSERTE(type >= 0 && type < NB_STATXY_TYPE);

		double value = STAT_VMISS;

		const CStatisticXY& me = *this;
		double nbValues = m_x[NB_VALUE];

		if (nbValues > 0 || type == NB_VALUE)
		{
			double nbVal = (nbValues == 1) ? nbValues : nbValues - 1;
			switch (type)
			{
			case LOWEST:	value = m_lowest; break;
			case MEAN:		value = m_e / nbValues; break;//mean of error (bias) (m_y[MEAN] + m_x[MEAN]) / 2; break;
			case MEAN_X:	value = m_x[MEAN]; break;
			case MEAN_Y:	value = m_y[MEAN]; break;
			case TSS:		value = m_y[TSS]; break;
			case INTERCEPT: value = m_y[MEAN] - m_x[MEAN] * me[SLOPE]; break;
			case SLOPE:
				if (m_x[TSS] != 0)
					value = (m_xy - (m_x[SUM] * m_y[SUM] / nbValues)) / m_x[TSS];
				break;
			case COVARIANCE: value = ((m_xy - m_x[SUM] * m_y[MEAN] - m_y[SUM] * m_x[MEAN] + nbValues*m_x[MEAN] * m_y[MEAN])) / nbVal; break;
			case CORRELATION:
			{
				double stdDev = (m_x[STD_DEV] * m_y[STD_DEV]);
				if (stdDev != 0)
					value = me[COVARIANCE] / stdDev;
				break;
			}
			case BIAS:		value = m_e / nbValues; break;
			case MAE:		value = m_ae / nbValues; break;
			case RMSE:		value = sqrt(m_rs / nbValues); break;
			case RSS:		value = m_rs; break;
			case COEF_D:	value = me[TSS] > 0 ? (1 - me[RSS] / me[TSS]) : STAT_VMISS; break;
			case COEF_C:
			{
				double d = sqrt(nbValues*m_x[SUM²] - Square(m_x[SUM]))*sqrt(nbValues*m_y[SUM²] - Square(m_y[SUM]));
				if (d != 0)
					value = (nbValues*m_xy - m_x[SUM] * m_y[SUM]) / d;
				break;
			}
			case STAT_R²:
			{
				double c = me[COEF_C];
				if (c > STAT_VMISS)
					value = Square(c);
				break;
			}
			case HIGHEST:	value = m_hightest; break;
			case RANGE:		value = m_hightest - m_lowest; break;
			case NB_VALUE:	value = nbValues; break;
			default: _ASSERTE(false);
			}
		}

		_ASSERTE(!_isnan(value));

		return value;
	}

	double CStatisticXY::GetVMiss(){ return STAT_VMISS; }
	void CStatisticXY::SetVMiss(double vmiss){ STAT_VMISS = vmiss; }

	//*****************************************************************
	//CStatisticXYEx: statistic of 2 variables, keeps all values in memory
	CStatisticXYEx::CStatisticXYEx()
	{
		Reset();
	}

	void CStatisticXYEx::Reset()
	{
		CStatisticXY::Reset();
		m_xValues.clear();
		m_yValues.clear();
	}

	CStatisticXYEx& CStatisticXYEx::operator=(const CStatisticXYEx& in)
	{
		if (&in != this)
		{
			CStatisticXY::operator=(in);
			m_xValues = m_xValues;
			m_yValues = m_yValues;
		}

		return *this;
	}

	double CStatisticXYEx::operator[](size_t type)const
	{
		_ASSERTE(m_xValues.size() == m_yValues.size());
		_ASSERTE(m_xValues.size() == GetX()[NB_VALUE]);
		_ASSERTE(m_yValues.size() == GetY()[NB_VALUE]);
		_ASSERTE(type >= 0 && type < NB_STATXY_TYPE_EX);

		const CStatisticXYEx& me = *this;
		double value = STAT_VMISS;

		if (m_xValues.size() > 0)
		{
			if (type == INTERCEPT_THEIL_SEN)
			{
				value = m_yValues[MEDIAN] - m_xValues[MEDIAN] * me[SLOPE_THEIL_SEN];
			}
			else if (type == SLOPE_THEIL_SEN)
			{
				CStatisticEx stat;
				ASSERT(m_xValues.size() == m_yValues.size());
				for (size_t i = 0; i < m_xValues.size(); i++)
				{
					for (size_t j = i + 1; j < m_xValues.size(); j++)
					{
						double deltaX = (m_xValues[i] - m_xValues[j]);
						double deltaY = (m_yValues[i] - m_yValues[j]);
						if (deltaX != 0)
							stat += deltaY / deltaX;
					}
				}

				value = stat[MEDIAN];
			}
			//else if (type == LOG_LIKELIHOOD1)
			//{
			//	//double xSum = m_x[SUM];
			//	double ySum = m_y[SUM];
			//	if (ySum > 0)
			//	{
			//		double LL = 0;
			//		//likelihood with classical sigma hat
			//		double sigma = sqrt(me[RSS] / (m_xValues.size() - 1))*(m_xValues.size()) / (m_xValues.size() - 1);
			//		for (size_t i = 0; i < m_xValues.size(); i++)
			//		{
			//			double m = m_xValues[i];
			//			boost::math::normal_distribution<> N(m, sigma);

			//			double x = m_yValues[i];
			//			double p = boost::math::pdf(N, x);
			//			ASSERT(p > 0);
			//			LL += log(p);
			//		}

			//		value = LL;
			//	}
			//}
			else if (type == LIKELIHOOD)
			{
				double xSum = m_x[SUM];
				double ySum = m_y[SUM];
				if (ySum > 0)
				{
					double LL = LogFactorial(xSum);
					for (size_t i = 0; i < m_xValues.size(); i++)
					{
						double p = pow(m_yValues[i] / ySum, m_xValues[i]);
						LL += log(p);
						LL -= LogFactorial(m_xValues[i]);
					}

					value = LL;
				}
			}
			else
			{
				value = CStatisticXY::operator[](type);
			}
		}

		_ASSERTE(!_isnan(value) && _finite(value));
		return value;
	}

	//x = pred, y = obs
	CStatisticXYEx& CStatisticXYEx::Add(double x, double y)
	{
		m_CS.Enter();
		CStatisticXY::Add(x, y);

		m_xValues.push_back(x);
		m_yValues.push_back(y);
		m_CS.Leave();

		return *this;
	}

	CStatisticXYEx& CStatisticXYEx::operator+=(const CStatisticXYEx& statistic)
	{
		CStatisticXY::operator+=(statistic);
		m_xValues.insert(m_xValues.end(), statistic.m_xValues.begin(), statistic.m_xValues.end());
		m_yValues.insert(m_yValues.end(), statistic.m_yValues.begin(), statistic.m_yValues.end());

		return *this;
	}

	//cook distance
	std::vector<double> CStatisticXYEx::GetCookDistance()const
	{
		ASSERT(m_xValues.size() == m_yValues.size());

		std::vector<double> cookD(x().size());

		vector<array<double, 2>> X(x().size());
		for (size_t i = 0; i < x().size(); i++)
		{
			X[i][0] = 1;
			X[i][1] = x(i);
		}

		double d = (m_x[NB_VALUE] * m_x[SUM²]) - Square(m_x[SUM]);
		array<array<double, 2>, 2> s =
		{
			{{m_x[SUM²] / d,-m_x[SUM] / d},
			{-m_x[SUM] / d, m_x[NB_VALUE] / d}}
		};

		vector<vector<double>> P(X.size());
		for (size_t i = 0; i < X.size(); i++)
		{
			P[i].resize(X.size());
			for (size_t j = 0; j < X.size(); j++)
			{
				P[i][j] = (s[0][0] * X[i][0] + s[1][0] * X[i][1])*X[j][0] + (s[0][1] * X[i][0] + s[1][1] * X[i][1])*X[j][1];
			}
		}

		array<double, 2> b = { 0 };
		for (size_t i = 0; i < X.size(); i++)
		{
			b[0] += (s[0][0] * X[i][0] + s[1][0] * X[i][1])*y(i);
			b[1] += (s[0][1] * X[i][0] + s[1][1] * X[i][1])*y(i);
		}

		double RSS=0;
		for (size_t i = 0; i < X.size(); i++)
			RSS += Square(y(i) - (b[0] * X[i][0] + b[1] * X[i][1]));

		size_t k = 2;
		double s2 = RSS / (X.size() - k);                  //three predictors(including intercept(100 - k = 98))

		for (size_t i = 0; i < X.size(); i++)
		{
			double res² = Square(y(i) - (b[0] * X[i][0] + b[1] * X[i][1]));
			cookD[i] = (res² / (k*s2))*(P[i][i] / Square(1.0 - P[i][i]));
		}

		return cookD;

	}

	//*************************************************************
	//Weighted CStatisticXY
	CStatisticXYW::CStatisticXYW()
	{
		m_wxy = 0;
		m_we = 0;
		m_wrs = 0;
		m_wae = 0;
	}

	void CStatisticXYW::Reset()
	{
		m_xw.clear();
		m_yw.clear();
		m_wxy = 0;
		m_we = 0;
		m_wrs = 0;
		m_wae = 0;
	}

	CStatisticXYW& CStatisticXYW::operator=(const CStatisticXYW& in)
	{
		m_xw = in.m_xw;
		m_yw = in.m_yw;
		m_wxy = in.m_wxy;
		m_we = in.m_we;
		m_wrs = in.m_wrs;
		m_wae = in.m_wae;

		return *this;
	}

	CStatisticXYW& CStatisticXYW::Add(double x, double y, double w)
	{
		m_xw.insert(x, w);
		m_yw.insert(y, w);
		double e = x - y;
		m_wxy += w*x*y;
		m_we += w*e;
		m_wrs += w*e*e;
		m_wae += w*fabs(e);

		return *this;
	}

	CStatisticXYW& CStatisticXYW::operator+=(const CStatisticXYW& in)
	{
		m_xw += in.m_xw;
		m_yw += in.m_yw;
		m_wxy += in.m_wxy;
		m_we += in.m_we;
		m_wrs += in.m_wrs;
		m_wae += in.m_wae;

		return *this;
	}

	double CStatisticXYW::operator[](size_t type)const
	{
		ASSERT(m_xw[NB_VALUE] == m_yw[NB_VALUE]);
		ASSERT(type >= 0 && type < NB_STATXY_TYPE);

		double value = STAT_VMISS;

		const CStatisticXYW& me = *this;
		double nbValues = m_xw[NB_VALUE];

		if (nbValues > 0 || type == NB_VALUE)
		{
			switch (type)
			{
			case LOWEST: /*value = m_e[LOWEST];*/ break;
			case MEAN: value = m_we / m_xw.W(SUM); break;
			case MEAN_X: value = m_xw[MEAN]; break;
			case MEAN_Y:value = m_yw[MEAN]; break;
			case TSS: value = m_yw[TSS]; break;
			case INTERCEPT: value = m_yw[MEAN] - m_xw[MEAN] * me[SLOPE]; break;
			case SLOPE:
				if (m_xw[TSS] != 0)
					value = (m_wxy - (m_xw[SUM] * m_yw[SUM] / m_xw.W(SUM))) / m_xw[TSS];
				break;
			case COVARIANCE: value = ((m_wxy - m_xw[SUM] * m_yw[MEAN] - m_yw[SUM] * m_xw[MEAN] + m_xw.W(SUM)*m_xw[MEAN] * m_yw[MEAN])) / m_xw.W(SUM); break;
			case CORRELATION:
			{
				double stdDev = (m_xw[STD_DEV] * m_yw[STD_DEV]);
				if (stdDev != 0)
					value = me[COVARIANCE] / stdDev;
				break;
			}

			case BIAS: value = m_we / m_xw.W(SUM); break;
			case MAE: value = m_wae / m_xw.W(SUM); break;
			case RMSE: value = sqrt(m_wrs / m_xw.W(SUM)); break;//ici il ya une erreur dans le calcul... a faire
			case RSS: value = m_wrs / m_xw.W(SUM); break;
			case COEF_D: value = me[TSS] > 0 ? (1 - me[RSS] / me[TSS]) : STAT_VMISS; break;
			case COEF_C:
			{
				double d = sqrt(max(0.0, m_xw.W(SUM)*m_xw[SUM²] - Square(m_xw[SUM])))*sqrt(max(0.0, m_xw.W(SUM) * m_yw[SUM²] - Square(m_yw[SUM])));
				if (d != 0)
					value = (m_xw.W(SUM)*m_wxy - m_xw[SUM] * m_yw[SUM]) / d;
				break;
			}
			case STAT_R²:
			{
				double c = me[COEF_C];
				if (c > STAT_VMISS)
					value = Square(c);
				break;
			}
			case HIGHEST: value = STAT_VMISS; break;
			case RANGE: value = STAT_VMISS; break;
			case NB_VALUE: value = nbValues; break;
			default: _ASSERTE(false);
			}
		}

		_ASSERTE(!_isnan(value));

		return value;
	}

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

	CClassify::CClassify()
	{
		Reset();
	}

	void CClassify::Reset()
	{
		m_stat.clear();
		m_values.clear();
	}

	void CClassify::Add(double x, double y)
	{
		m_values.push_back(CXY<double, double>(x, y));
	}

	void CClassify::ClassifyManual(vector<double> classLimit)
	{
		_ASSERTE(classLimit.size() >= 2);
		m_stat.clear();
		m_stat.resize(classLimit.size() - 1);

		if (m_values.size() > 0)
		{
			std::sort(m_values.begin(), m_values.end());

			double first = m_values.begin()->m_x;
			double last = m_values.rbegin()->m_x;
			if (classLimit[0] == -1)
				classLimit[0] = first;

			if (classLimit[classLimit.size() - 1] == -1)
				classLimit[classLimit.size() - 1] = last;

			int j = 0;
			for (int i = 0; i<int(classLimit.size() - 1); i++)
			{
				while (j < (int)m_values.size() &&
					m_values[j].m_x >= classLimit[i] &&
					m_values[j].m_x < classLimit[i + 1])
				{
					m_stat[i].m_x += m_values[j].m_x;
					m_stat[i].m_y += m_values[j].m_y;
					j++;
				}
			}
		}
	}

	void CClassify::ClassifyEqualInterval(short nbClass)
	{
		m_stat.clear();
		m_stat.resize(nbClass);

		if (m_values.size() > 0)
		{
			std::sort(m_values.begin(), m_values.end());

			double first = m_values.begin()->m_x;
			double last = m_values.rbegin()->m_x;
			double classSize = (double)(last - first) / nbClass;
			int j = 0;
			for (int i = 0; i < nbClass; i++)
			{
				double limit = first + (i + 1)*classSize;

				while (j < (int)m_values.size() &&
					m_values[j].m_x <= limit)
				{
					m_stat[i].m_x += m_values[j].m_x;
					m_stat[i].m_y += m_values[j].m_y;
					j++;
				}
			}
		}
	}

	void CClassify::ClassifyNaturalBreak(short nbClass)
	{
		m_stat.clear();
		m_stat.resize(nbClass);

		std::sort(m_values.begin(), m_values.end());

		double classSize = (double)m_values.size() / nbClass;
		for (int i = 0; i < nbClass; i++)
		{
			int f = int(i*classSize);
			int l = int((i + 1)*classSize);
			_ASSERTE(f >= 0 && f <= (int)m_values.size());
			_ASSERTE(l >= 0 && l <= (int)m_values.size());

			for (int j = f; j < l; j++)
			{
				m_stat[i].m_x += m_values[j].m_x;
				m_stat[i].m_y += m_values[j].m_y;
			}
		}
	}

}//namespace WBSF