#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <fstream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/index/rtree.hpp>

// #define DATASET_SIZE 1000000
#define DIMENTION 2
#define ELIPSON 30
#define MIN_POINTS 10

using namespace std;

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<float, 2, bg::cs::cartesian> dataPoint;
typedef bg::model::box<dataPoint> box;
typedef std::pair<box, int> value;

class DBSCAN {
 private:
  double** dataset;
  int elipson;
  int minPoints;
  int cluster;
  int* clusters;
  double getDistance(int center, int neighbor);
  vector<int> findNeighbors(int pos);
  void expandCluster(int pointId, vector<int> &neighbors);
  bgi::rtree<value, bgi::quadratic<4>> rtree;

 public:
  DBSCAN(double ** dataset);
  void run();
  void results();
};

int main(int, char **) {
  // Generate random datasets
  double **dataset =
      (double **)malloc(sizeof(double *) * DATASET_SIZE);
  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i] = (double *)malloc(sizeof(double) * DIMENTION);
  }

  // Import Dataset from a file
  ifstream file("../dataset/dataset.txt");
  if (file.is_open()) {
    string token;
    int rowCount = 0;
    while (getline(file, token)) {
      int colCount = 0;
      char* x = (char*)token.c_str();
      char* field = strtok(x, ",");
      double tmp;
      sscanf(field, "%lf", &tmp);
      dataset[rowCount][colCount] = tmp;
      while (field) {
        colCount++;
        if(colCount == DIMENTION) break;
        field = strtok(NULL, ",");
        if (field!=NULL) {
          double tmp;
          sscanf(field,"%lf",&tmp);
          dataset[rowCount][colCount] = tmp;
        }
      }
      rowCount++;
      if(rowCount == DATASET_SIZE) break;
    }
    file.close();
  }

  // Initialize DBSCAN with dataset
  DBSCAN dbscan(dataset);

  // Run the DBSCAN algorithm
  dbscan.run();

  // Print the cluster results of DBSCAN
  dbscan.results();

  return 0;
}

DBSCAN::DBSCAN(double **loadData) {

  dataset =
      (double **)malloc(sizeof(double *) * DATASET_SIZE);
  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i] = (double *)malloc(sizeof(double) * DIMENTION);
  }
  clusters = (int *)malloc(sizeof(int) * DATASET_SIZE);
  elipson = ELIPSON;
  minPoints = MIN_POINTS;
  cluster = 0;

  for (int i = 0; i < DATASET_SIZE; i++) {
    dataset[i][0] = loadData[i][0];
    dataset[i][1] = loadData[i][1];
    clusters[i] = 0;
  }

  // Create an Rtree of the dataset
  for (int i = 0; i < DATASET_SIZE; i++) {
    // create a box for each points
    box b(dataPoint(dataset[i][0], dataset[i][1]),
          dataPoint(dataset[i][0], dataset[i][1]));
    // insert points to the rtree
    rtree.insert(std::make_pair(b, i));

  }
}

double DBSCAN::getDistance(int center, int neighbor) {
  int dist = (dataset[center][0] - dataset[neighbor][0]) *
                 (dataset[center][0] - dataset[neighbor][0]) +
             (dataset[center][1] - dataset[neighbor][1]) *
                 (dataset[center][1] - dataset[neighbor][1]);

  return sqrt(dist);
}

void DBSCAN::run() {
  // Neighbors of the point
  vector<int> neighbors;

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

    if (clusters[i] == 0) {

      // Find neighbors of point P
      neighbors = findNeighbors(i);

      // Mark noise points
      if (neighbors.size() < minPoints) {
        clusters[i] = -1;
      } else {
        // Increment cluster and initialize it will the current point
        cluster++;

        clusters[i] = cluster;

        // Expand the neighbors of point P
        for (int j = 0; j < neighbors.size(); j++) {

          // Mark neighbour as point Q
          int dataIndex = neighbors[j];

          if(clusters[dataIndex] == -1) {
            clusters[dataIndex] = cluster;
          } else if (clusters[dataIndex] == 0) {

            clusters[dataIndex] = cluster;

            // Expand more neighbors of point Q
            vector<int> moreNeighbors;
            moreNeighbors = findNeighbors(dataIndex);

            // Continue when neighbors point is higher than minPoint threshold

            if (moreNeighbors.size() >= minPoints) {
              // Check if neighbour of Q already exists in neighbour of P
              for (int x = 0; x < moreNeighbors.size(); x++) {
                bool doesntExist = true;
                for (int y = 0; y < neighbors.size(); y++) {
                  if (moreNeighbors[x] == neighbors[y]) {
                    doesntExist = false;
                    break;
                  }
                }

                // If neighbour doesn't exist, add to neighbor list
                if (doesntExist) {
                  neighbors.push_back(moreNeighbors[x]);
                }
              }
            }
          }
      }
    }
  }
}
}

void DBSCAN::results() {
  printf("Number of clusters: %d\n", cluster);
  int noises = 0;
  for(int x = 1; x <= cluster; x++) {
    int count = 0;
    for(int i = 0; i < DATASET_SIZE; i++) {
      if(clusters[i] == x) {
        count++;
      }
      if(clusters[i] == -1) {
        noises++;
      }
    }
    printf("Cluster %d has %d data\n", x, count);
  }
  printf("Noises: %d\n", noises);

}

vector<int> DBSCAN::findNeighbors(int pos) {

  vector<int> neighbors;
  vector<value> result_n;

  // Create a search box for the given poiny
  box searchBox(dataPoint(dataset[pos][0] - elipson, dataset[pos][1] - elipson),
                dataPoint(dataset[pos][0] + elipson, dataset[pos][1] + elipson));

  // Query the intersection of search box on Rtree
  rtree.query(bgi::intersects(searchBox), std::back_inserter(result_n));

  // collect the points of box
  vector<int> pointsInBox = {};
  for (value pair : result_n) pointsInBox.push_back(pair.second);

  // Compute the distance only with points in a box
  for (int x = 0; x < pointsInBox.size(); x++) {
    // Compute neighbor points
    double distance = getDistance(pos, pointsInBox[x]);
    if (distance <= elipson && pos != pointsInBox[x]) {
      neighbors.push_back(pointsInBox[x]);
    }
  }

  return neighbors;

}


/*
Modified BSD License

This file is a part of Statistical package and originates from:
http://sf.net/projects/enjomitchsorbit

Copyright (c) 2012, Szymon "Enjo" Ender
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "EigenEigen3.hpp"
#include "../Matrix.hpp"
#include "../../Util/VecD.hpp"
#include <Util/Except.hpp>
#include <Util/CharManipulations.hpp>

#include <STD/VectorCpp.hpp>

//#define EIGEN_USE_MKL_ALL
//#define EIGEN_USE_BLAS // Enables openblas64 - somewhat slower
//#define EIGEN_USE_LAPACKE // Don't use currently
#include <Eigen/Dense>

#include <Util/CoutBuf.hpp>

using namespace std;
using namespace EnjoLib;
//using namespace Eigen;

EigenEigen3::~EigenEigen3(){}
EigenEigen3::EigenEigen3(){}

static Eigen::MatrixXd ConvertMatrix( const Matrix & m )
{
    const int nrows = m.GetNRows();
    const int ncols = m.GetNCols();
    Eigen::MatrixXd mateig(nrows, ncols);
    for ( int i = 0; i < nrows; ++i ) for ( int j = 0; j < ncols; ++j )
            mateig(i, j) = m.at(i).at(j);

    return mateig;
}

std::vector<EigenValueVector> EigenEigen3::GetEigenValVecClient( const Matrix & m ) const
{
    std::vector<EigenValueVector> ret;
    ret.reserve(m.GetNRows());

    //cout << "M = " << m.GetNCols() << ", " << m.GetNRows() << endl;

    Eigen::EigenSolver<Eigen::MatrixXd> es;
    const Eigen::MatrixXd & matE = ConvertMatrix(m);
    //cout << "Pre\n" << m.Print() << endl;
    es.compute(matE, /* computeEigenvectors = */ true);
    //cout << "Post" << endl;
    if (es.info() != Eigen::ComputationInfo::Success)
    {
        ELO
        LOG << "Eigenvalue Calculation not successful: " << es.info() << NL3;
        //cout << m.Print() << endl;
        throw EnjoLib::ExceptRuntimeError("Eigenvalue Calculation not successful " + CharManipulations().ToStr(es.info()));
        LOG << "After throw: " << es.info() << NL3;
    }

    const auto & eval  = es.eigenvalues();
    const auto & evect = es.eigenvectors();
    for ( int i = 0; i < m.GetNRows(); ++i )
    {
        const auto & evali = eval(i);
        const double eigenValue = evali.real();
        VecD eigenVector;
        eigenVector.reserve(m.GetNCols());

        for ( int j = 0; j < m.GetNCols(); ++j )
        {
            const double val = evect(j, i).real(); // Transposition (j, i) due to internal Eigen storage!
            eigenVector.push_back(val);
        }

        ret.push_back( EigenValueVector(eigenValue, eigenVector) );
    }
    return ret;
}


// Copyright (c) 2018-2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <array>
#include <chrono>
#include <functional>
#include <tuple>
#include <vector>

#include <Eigen/Core>
#include <frc/estimator/ExtendedKalmanFilter.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/logging/CSVLogFile.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/Trajectory.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/curvature.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <wpi/math>
#include <wpi/mutex.h>

#include "Constants.hpp"

namespace frc3512 {

class DrivetrainController {
public:
    class State {
    public:
        static constexpr int kX = 0;
        static constexpr int kY = 1;
        static constexpr int kHeading = 2;
        static constexpr int kLeftVelocity = 3;
        static constexpr int kRightVelocity = 4;
        static constexpr int kLeftPosition = 5;
        static constexpr int kRightPosition = 6;
        static constexpr int kLeftVoltageError = 7;
        static constexpr int kRightVoltageError = 8;
        static constexpr int kAngularVelocityError = 9;
    };

    class Input {
    public:
        static constexpr int kLeftVoltage = 0;
        static constexpr int kRightVoltage = 1;
    };

    class LocalOutput {
    public:
        static constexpr int kHeading = 0;
        static constexpr int kLeftPosition = 1;
        static constexpr int kRightPosition = 2;
    };

    class GlobalOutput {
    public:
        static constexpr int kX = 0;
        static constexpr int kY = 1;
        static constexpr int kHeading = 2;
        static constexpr int kLeftPosition = 3;
        static constexpr int kRightPosition = 4;
        static constexpr int kAngularVelocity = 5;
    };

    /**
     * Constructs a drivetrain controller with the given coefficients.
     *
     * @param Qelems The maximum desired error tolerance for each state.
     * @param Relems The maximum desired control effort for each input.
     * @param dt     Discretization timestep.
     */
    DrivetrainController(const std::array<double, 5>& Qelems,
                         const std::array<double, 2>& Relems,
                         units::second_t dt);

    DrivetrainController(const DrivetrainController&) = delete;
    DrivetrainController& operator=(const DrivetrainController&) = delete;

    void Enable();
    void Disable();
    bool IsEnabled() const;

    void SetWaypoints(const std::vector<frc::Pose2d>& waypoints);

    /**
     * Returns whether the drivetrain controller is at the goal waypoint.
     */
    bool AtGoal() const;

    /**
     * Set local measurements.
     *
     * @param heading       Angle of the robot.
     * @param leftPosition  Encoder count of left side in meters.
     * @param rightPosition Encoder count of right side in meters.
     */
    void SetMeasuredLocalOutputs(units::radian_t heading,
                                 units::meter_t leftPosition,
                                 units::meter_t rightPosition);

    /**
     * Set global measurements.
     *
     * @param x             X position of the robot in meters.
     * @param y             Y position of the robot in meters.
     * @param heading       Angle of the robot.
     * @param leftPosition  Encoder count of left side in meters.
     * @param rightPosition Encoder count of right side in meters.
     * @param angularVelocity Angular velocity of the robot in radians per
     * second.
     */
    void SetMeasuredGlobalOutputs(units::meter_t x, units::meter_t y,
                                  units::radian_t heading,
                                  units::meter_t leftPosition,
                                  units::meter_t rightPosition,
                                  units::radians_per_second_t angularVelocity);

    /**
     * Returns the drivetrain's plant.
     */
    frc::LinearSystem<2, 2, 2> GetPlant() const;

    /**
     * Returns the current references.
     *
     * x, y, heading, left velocity, and right velocity.
     */
    const Eigen::Matrix<double, 5, 1>& GetReferences() const;

    /**
     * Returns the current state estimate.
     *
     * x, y, heading, left position, left velocity, right position,
     * right velocity, left voltage error, right voltage error, and angle error.
     */
    const Eigen::Matrix<double, 10, 1>& GetStates() const;

    /**
     * Returns the control inputs.
     *
     * left voltage and right voltage.
     */
    Eigen::Matrix<double, 2, 1> GetInputs() const;

    /**
     * Returns the currently set local outputs.
     *
     * heading, left position, left velocity, right position,
     * right velocity, and angular velocity.
     */
    const Eigen::Matrix<double, 3, 1>& GetOutputs() const;

    /**
     * Returns the estimated outputs based on the current state estimate.
     *
     * This provides only local measurements.
     */
    Eigen::Matrix<double, 3, 1> EstimatedLocalOutputs() const;

    /**
     * Returns the estimated outputs based on the current state estimate.
     *
     * This provides global measurements (including pose).
     */
    Eigen::Matrix<double, 6, 1> EstimatedGlobalOutputs() const;

    /**
     * Executes the control loop for a cycle.
     *
     * @param dt Timestep between each Update() call
     */
    void Update(units::second_t dt, units::second_t elapsedTime);

    /**
     * Resets any internal state.
     */
    void Reset();

    /**
     * Resets any internal state.
     *
     * @param initialPose Initial pose for state estimate.
     */
    void Reset(const frc::Pose2d& initialPose);

    Eigen::Matrix<double, 2, 1> Controller(
        const Eigen::Matrix<double, 10, 1>& x,
        const Eigen::Matrix<double, 5, 1>& r);

    static Eigen::Matrix<double, 10, 1> Dynamics(
        const Eigen::Matrix<double, 10, 1>& x,
        const Eigen::Matrix<double, 2, 1>& u);

    static Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
        const Eigen::Matrix<double, 10, 1>& x,
        const Eigen::Matrix<double, 2, 1>& u);

    static Eigen::Matrix<double, 6, 1> GlobalMeasurementModel(
        const Eigen::Matrix<double, 10, 1>& x,
        const Eigen::Matrix<double, 2, 1>& u);

private:
    // Robot radius
    static constexpr auto rb = Constants::Drivetrain::kWidth / 2.0;

    static frc::LinearSystem<2, 2, 2> m_plant;

    // The current sensor measurements
    Eigen::Matrix<double, 3, 1> m_localY;
    Eigen::Matrix<double, 6, 1> m_globalY;

    // Design observer
    // States: [x position, y position, heading,
    //          left velocity, right velocity,
    //          left position, right position,
    //          left voltage error, right voltage error, angle error]
    //
    // Inputs: [left voltage, right voltage]
    //
    // Outputs (local): [left position, right position,
    //                   angular velocity]
    //
    // Outputs (global): [x position, y position, heading,
    //                    left position, right position,
    //                    angular velocity]
    frc::ExtendedKalmanFilter<10, 2, 3> m_observer{
        Dynamics,
        LocalMeasurementModel,
        {0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5, 10.0, 10.0, 2.0},
        {0.0001, 0.005, 0.005},
        Constants::kDt};

    // XXX: For testing only. This is used to verify the EKF pose because
    // DifferentialDriveOdometry is known to work on other robots.
    frc::DifferentialDriveOdometry m_odometer{frc::Rotation2d(0_rad)};

    // Design controller
    // States: [x position, y position, heading, left velocity, right velocity]
    Eigen::Matrix<double, 5, 2> m_B;
    Eigen::Matrix<double, 2, 5> m_K0;
    Eigen::Matrix<double, 2, 5> m_K1;

    // Controller reference
    Eigen::Matrix<double, 5, 1> m_r;

    Eigen::Matrix<double, 5, 1> m_nextR;
    Eigen::Matrix<double, 2, 1> m_cappedU;

    frc::Trajectory m_trajectory;
    frc::Pose2d m_goal;

    wpi::mutex m_trajectoryMutex;

    bool m_atReferences = false;
    bool m_isEnabled = false;

    // The loggers that generates the comma separated value files
    frc::CSVLogFile positionLogger{"Drivetrain Positions",
                                   "Estimated X (m)",
                                   "Estimated Y (m)",
                                   "X Ref (m)",
                                   "Y Ref (m)",
                                   "Measured Left Position (m)",
                                   "Measured Right Position (m)",
                                   "Estimated Left Position (m)",
                                   "Estimated Right Position (m)",
                                   "Odometry X (m)",
                                   "Odometry Y (m)"};
    frc::CSVLogFile angleLogger{"Drivetrain Angles", "Measured Heading (rad)",
                                "Estimated Heading (rad)", "Heading Ref (rad)",
                                "Angle Error (rad)"};
    frc::CSVLogFile velocityLogger{"Drivetrain Velocities",
                                   "Measured Left Velocity (m/s)",
                                   "Measured Right Velocity (m/s)",
                                   "Estimated Left Vel (m/s)",
                                   "Estimated Right Vel (m/s)",
                                   "Left Vel Ref (m/s)",
                                   "Right Vel Ref (m/s)"};
    frc::CSVLogFile voltageLogger{
        "Drivetrain Voltages",     "Left Voltage (V)",
        "Right Voltage (V)",       "Left Voltage Error (V)",
        "Right Voltage Error (V)", "Battery Voltage (V)"};
    frc::CSVLogFile errorCovLogger{
        "Drivetrain Error Covariances",
        "X Cov (m^2)",
        "Y Cov (m^2)",
        "Heading Cov (rad^2)",
        "Left Vel Cov ((m/s)^2)",
        "Right Vel Cov ((m/s)^2)",
        "Left Pos Cov (m^2)",
        "Right Pos Cov (m^2)",
        "Left Voltage Error Cov (V^2)",
        "Right Voltage Error Cov (V^2)",
        "Angle Error Cov (rad^2)",
    };

    /**
     * Constrains theta to within the range (-pi, pi].
     *
     * @param theta Angle to normalize
     */
    static constexpr double NormalizeAngle(double theta) {
        // Constrain theta to within (-3pi, pi)
        const int n_pi_pos = (theta + wpi::math::pi) / 2.0 / wpi::math::pi;
        theta -= n_pi_pos * 2.0 * wpi::math::pi;

        // Cut off the bottom half of the above range to constrain within
        // (-pi, pi]
        const int n_pi_neg = (theta - wpi::math::pi) / 2.0 / wpi::math::pi;
        theta -= n_pi_neg * 2.0 * wpi::math::pi;

        return theta;
    }

    /**
     * Converts velocity and curvature of drivetrain into left and right wheel
     * velocities.
     *
     * @param velocity Linear velocity of drivetrain chassis.
     * @param curvature Curvature of drivetrain arc.
     * @param trackWidth Track width of drivetrain.
     */
    static constexpr std::tuple<units::meters_per_second_t,
                                units::meters_per_second_t>
    ToWheelVelocities(units::meters_per_second_t velocity,
                      units::curvature_t curvature, units::meter_t trackWidth) {
        // clang-format off
        // v = (v_r + v_l) / 2     (1)
        // w = (v_r - v_l) / (2r)  (2)
        // k = w / v               (3)
        //
        // v_l = v - wr
        // v_l = v - (vk)r
        // v_l = v(1 - kr)
        //
        // v_r = v + wr
        // v_r = v + (vk)r
        // v_r = v(1 + kr)
        // clang-format on
        auto vl = velocity * (1 - (curvature / 1_rad * trackWidth / 2.0));
        auto vr = velocity * (1 + (curvature / 1_rad * trackWidth / 2.0));
        return {vl, vr};
    }

    static void ScaleCapU(Eigen::Matrix<double, 2, 1>* u);
};
}  // namespace frc3512


#include "eigenvalues.h"
#include <Eigen/Eigenvalues>

/*!
 * Adjust a matrix so that its eigenvalues are less than a given magnitude.
 * Moved to its on CU because this doesn't change often and takes like 2 minutes to recompile on O3 :P
 */
template<typename T>
DenseMatrix<T> constrainEigenvalueMagnitude(const DenseMatrix<T> &m, T magnitude)
{
  Eigen::EigenSolver<DenseMatrix<T>> eigSolver(m);
  using MatCT = Eigen::Matrix<std::complex<T>, Eigen::Dynamic, Eigen::Dynamic>;
  MatCT D = eigSolver.eigenvalues().asDiagonal();
  MatCT V = eigSolver.eigenvectors();

  for(s64 i = 0; i < D.rows(); i++) {
    T eigMag = std::abs(D(i,i));
    if(eigMag > magnitude)
      D(i,i) = magnitude * D(i,i) / eigMag;
  }

  MatCT restored = V * D * V.inverse(); // slow, but this matrix is small.
  DenseMatrix<T> result(m.rows(), m.rows());
  for(s64 i = 0; i < m.rows(); i++) {
    for(s64 j = 0; j < m.rows(); j++) {
      result(i,j) = std::real(restored(i,j));
    }
  }

  return result;
}

template DenseMatrix<float> constrainEigenvalueMagnitude(const DenseMatrix<float> &m, float magnitude);
template DenseMatrix<double> constrainEigenvalueMagnitude(const DenseMatrix<double> &m, double magnitude);


/**
 * @date Tue Apr 2 21:08:00 2013 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <boost/make_shared.hpp>
#include <bob.math/inv.h>
#include <bob.math/lu.h>
#include <bob.math/stats.h>

#include <bob.learn.linear/whitening.h>

namespace bob { namespace learn { namespace linear {

  WhiteningTrainer::WhiteningTrainer()
  {
  }

  WhiteningTrainer::WhiteningTrainer(const WhiteningTrainer& other)
  {
  }

  WhiteningTrainer::~WhiteningTrainer() {}

  WhiteningTrainer& WhiteningTrainer::operator= (const WhiteningTrainer& other)
  {
    return *this;
  }

  bool WhiteningTrainer::operator== (const WhiteningTrainer& other) const
  {
    return true;
  }

  bool WhiteningTrainer::operator!= (const WhiteningTrainer& other) const
  {
    return false;
  }

  void WhiteningTrainer::train(Machine& machine, const blitz::Array<double,2>& ar) const {
    // training data dimensions
    const size_t n_samples = ar.extent(0);
    const size_t n_features = ar.extent(1);
    // machine dimensions
    const size_t n_inputs = machine.inputSize();
    const size_t n_outputs = machine.outputSize();

    // Checks that the dimensions are matching
    if (n_inputs != n_features) {
      boost::format m("machine input size (%u) does not match the number of columns in input array (%d)");
      m % n_inputs % n_features;
      throw std::runtime_error(m.str());
    }
    if (n_outputs != n_features) {
      boost::format m("machine output size (%u) does not match the number of columns in output array (%d)");
      m % n_outputs % n_features;
      throw std::runtime_error(m.str());
    }

    // 1. Computes the mean vector and the covariance matrix of the training set
    blitz::Array<double,1> mean(n_features);
    blitz::Array<double,2> cov(n_features,n_features);
    bob::math::scatter(ar, cov, mean);
    cov /= (double)(n_samples-1);

    // 2. Computes the inverse of the covariance matrix
    blitz::Array<double,2> icov(n_features,n_features);
    bob::math::inv(cov, icov);

    // 3. Computes the Cholesky decomposition of the inverse covariance matrix
    blitz::Array<double,2> whiten(n_features,n_features);
    bob::math::chol(icov, whiten);

    // 4. Updates the linear machine
    machine.setInputSubtraction(mean);
    machine.setInputDivision(1.);
    machine.setWeights(whiten);
    machine.setBiases(0);
    machine.setActivation(boost::make_shared<bob::learn::activation::IdentityActivation>());
  }

}}}


#include <igl/sort_vectors_ccw.h>
#include <igl/sort.h>
#include <Eigen/Dense>

template <typename DerivedS, typename DerivedI>
IGL_INLINE void igl::sort_vectors_ccw(
  const Eigen::PlainObjectBase<DerivedS>& P,
  const Eigen::PlainObjectBase<DerivedS>& N,
  Eigen::PlainObjectBase<DerivedI> &order,
  const bool do_sorted,
  Eigen::PlainObjectBase<DerivedS> &sorted,
  const bool do_inv_order,
  Eigen::PlainObjectBase<DerivedI> &inv_order)
{
  int half_degree = P.cols()/3;

  //local frame
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e1 = P.head(3).normalized();
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e3 = N.normalized();
  Eigen::Matrix<typename DerivedS::Scalar,1,3> e2 = e3.cross(e1);

  Eigen::Matrix<typename DerivedS::Scalar,3,3> F; F<<e1.transpose(),e2.transpose(),e3.transpose();

  Eigen::Matrix<typename DerivedS::Scalar,Eigen::Dynamic,1> angles(half_degree,1);
  for (int i=0; i<half_degree; ++i)
  {
    Eigen::Matrix<typename DerivedS::Scalar,1,3> Pl = F.colPivHouseholderQr().solve(P.segment(i*3,3).transpose()).transpose();
    assert(fabs(Pl(2))/Pl.cwiseAbs().maxCoeff() <1e-5);
    angles[i] = atan2(Pl(1),Pl(0));
  }

  igl::sort( angles, 1, true, angles, order);
  //make sure that the first element is always  at the top
  while (order[0] != 0)
  {
    //do a circshift
    int temp = order[0];
    for (int i =0; i< half_degree-1; ++i)
      order[i] = order[i+1];
    order(half_degree-1) = temp;
  }
  if (do_sorted)
  {
    sorted.resize(1,half_degree*3);
    for (int i=0; i<half_degree; ++i)
      sorted.segment(i*3,3) = P.segment(order[i]*3,3);
  }
  if (do_inv_order)
  {
    inv_order.resize(half_degree,1);
    for (int i=0; i<half_degree; ++i)
    {
      for (int j=0; j<half_degree; ++j)
        if (order[j] ==i)
        {
          inv_order(i) = j;
          break;
        }
    }
    assert(inv_order[0] == 0);
  }

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::sort_vectors_ccw<Eigen::Matrix<double, 1, -1, 1, 1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, bool, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&, bool, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * testPowerMethod.cpp
 *
 * @file   testPowerMethod.cpp
 * @date   Sept 2020
 * @author Jing Wu
 * @brief  Check eigenvalue and eigenvector computed by power method
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PowerMethod.h>
#include <gtsam/linear/tests/powerMethodExample.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <random>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST(PowerMethod, powerIteration) {
  // test power iteration, beta is set to 0
  Sparse A(6, 6);
  A.coeffRef(0, 0) = 6;
  A.coeffRef(1, 1) = 5;
  A.coeffRef(2, 2) = 4;
  A.coeffRef(3, 3) = 3;
  A.coeffRef(4, 4) = 2;
  A.coeffRef(5, 5) = 1;
  Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359,
       0.2465342).finished();
  PowerMethod<Sparse> pf(A, initial);
  pf.compute(100, 1e-5);
  EXPECT_LONGS_EQUAL(6, pf.eigenvector().rows());

  Vector6 actual1 = pf.eigenvector();
  const double ritzValue = actual1.dot(A * actual1);
  const double ritzResidual = (A * actual1 - ritzValue * actual1).norm();
  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);

  const double ev1 = 6.0;
  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-5);
}

/* ************************************************************************* */
TEST(PowerMethod, useFactorGraphSparse) {
  // Let's make a scalar synchronization graph with 4 nodes
  GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph();

  // Get eigenvalues and eigenvectors with Eigen
  auto L = fg.hessian();
  Eigen::EigenSolver<Matrix> solver(L.first);

  // find the index of the max eigenvalue
  size_t maxIdx = 0;
  for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
      maxIdx = i;
  }
  // Store the max eigenvalue and its according eigenvector
  const auto ev1 = solver.eigenvalues()(maxIdx).real();

  Vector initial = Vector4::Random();
  PowerMethod<Matrix> pf(L.first, initial);
  pf.compute(100, 1e-5);
  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8);
  auto actual2 = pf.eigenvector();
  const double ritzValue = actual2.dot(L.first * actual2);
  const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm();
  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
}

/* ************************************************************************* */
TEST(PowerMethod, useFactorGraphDense) {
  // Let's make a scalar synchronization graph with 10 nodes
  GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph();

  // Get eigenvalues and eigenvectors with Eigen
  auto L = fg.hessian();
  Eigen::EigenSolver<Matrix> solver(L.first);

  // find the index of the max eigenvalue
  size_t maxIdx = 0;
  for (auto i = 0; i < solver.eigenvalues().rows(); ++i) {
    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real())
      maxIdx = i;
  }
  // Store the max eigenvalue and its according eigenvector
  const auto ev1 = solver.eigenvalues()(maxIdx).real();

  Vector initial = Vector10::Random();
  PowerMethod<Matrix> pf(L.first, initial);
  pf.compute(100, 1e-5);
  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8);
  auto actual2 = pf.eigenvector();
  const double ritzValue = actual2.dot(L.first * actual2);
  const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm();
  EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#ifndef STAN_MATH_PRIM_SCAL_FUN_LBETA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LBETA_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>

namespace stan {
namespace math {

/**
 * Return the log of the beta function applied to the specified
 * arguments.
 *
 * The beta function is defined for \f$a > 0\f$ and \f$b > 0\f$ by
 *
 * \f$\mbox{B}(a, b) = \frac{\Gamma(a) \Gamma(b)}{\Gamma(a+b)}\f$.
 *
 * This function returns its log,
 *
 * \f$\log \mbox{B}(a, b) = \log \Gamma(a) + \log \Gamma(b) - \log
 \Gamma(a+b)\f$.
 *
 * See stan::math::lgamma() for the double-based and stan::math for the
 * variable-based log Gamma function.
 *
 *
   \f[
   \mbox{lbeta}(\alpha, \beta) =
   \begin{cases}
     \ln\int_0^1 u^{\alpha - 1} (1 - u)^{\beta - 1} \, du & \mbox{if } \alpha,
 \beta>0 \\[6pt] \textrm{NaN} & \mbox{if } \alpha = \textrm{NaN or } \beta =
 \textrm{NaN} \end{cases} \f]

   \f[
   \frac{\partial\, \mbox{lbeta}(\alpha, \beta)}{\partial \alpha} =
   \begin{cases}
     \Psi(\alpha)-\Psi(\alpha+\beta) & \mbox{if } \alpha, \beta>0 \\[6pt]
     \textrm{NaN} & \mbox{if } \alpha = \textrm{NaN or } \beta = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{lbeta}(\alpha, \beta)}{\partial \beta} =
   \begin{cases}
     \Psi(\beta)-\Psi(\alpha+\beta) & \mbox{if } \alpha, \beta>0 \\[6pt]
     \textrm{NaN} & \mbox{if } \alpha = \textrm{NaN or } \beta = \textrm{NaN}
   \end{cases}
   \f]
 *
 * @param a First value
 * @param b Second value
 * @return Log of the beta function applied to the two values.
 * @tparam T1 Type of first value.
 * @tparam T2 Type of second value.
 */
template <typename T1, typename T2>
inline typename boost::math::tools::promote_args<T1, T2>::type lbeta(
    const T1 a, const T2 b) {
  return lgamma(a) + lgamma(b) - lgamma(a + b);
}

}  // namespace math
}  // namespace stan

#endif


// Copyright 2017 John Maddock

// 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)

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/relative_difference.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random.hpp>
#include <boost/svg_plot/svg_2d_plot.hpp>
#include <sstream>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<256, boost::multiprecision::backends::digit_base_2>, boost::multiprecision::et_on> mp_type;

template <class T>
void test_type(const char* name)
{
   typedef boost::math::policies::policy<
      boost::math::policies::promote_double<false>,
      boost::math::policies::promote_float<false>,
      boost::math::policies::overflow_error<boost::math::policies::ignore_error>
   > policy_type;
   boost::random::mt19937 dist;
   boost::random::uniform_real_distribution<T> ur(0, 7.75);
   boost::random::uniform_real_distribution<T> ur2(0, 1 / 7.75);

   float max_err = 0;

   std::map<double, double> small, medium, large;

   for(unsigned i = 0; i < 1000; ++i)
   {
      T input = ur(dist);
      mp_type input2(input);
      T result = boost::math::cyl_bessel_i(0, input, policy_type());
      mp_type result2 = boost::math::cyl_bessel_i(0, input2);
      mp_type err = boost::math::relative_difference(result2, mp_type(result)) / mp_type(std::numeric_limits<T>::epsilon());
      if(result2 < mp_type(result))
         err = -err;
      if(fabs(err) > max_err)
      {
         /*
         std::cout << std::setprecision(34) << input << std::endl;
         std::cout << std::setprecision(34) << input2 << std::endl;
         std::cout << std::setprecision(34) << result << std::endl;
         std::cout << std::setprecision(34) << result2 << std::endl;
         std::cout << "New max error at x = " << input << " expected " << result2 << " got " << result << " error " << err << std::endl;
         */
         max_err = static_cast<float>(fabs(err));
      }
      if(fabs(err) <= 1)
         small[static_cast<double>(input)] = static_cast<double>(err);
      else if(fabs(err) <= 2)
         medium[static_cast<double>(input)] = static_cast<double>(err);
      else
         large[static_cast<double>(input)] = static_cast<double>(err);
   }

   int y_interval = static_cast<int>(ceil(max_err / 5));

   std::stringstream ss;
   ss << "cyl_bessel_i<" << name << ">(0, x) over [0, 7.75]\n(max error = " << std::setprecision(2) << max_err << ")" << std::endl;

   boost::svg::svg_2d_plot my_plot;
   // Size of SVG image and X and Y range settings.
   my_plot.x_range(0, 7.75).image_x_size(700).legend_border_color(boost::svg::lightgray).plot_border_color(boost::svg::lightgray).background_border_color(boost::svg::lightgray)
      .y_range(-(int)ceil(max_err), (int)ceil(max_err)).x_label("x").title(ss.str()).y_major_interval(y_interval).x_major_interval(1.0).legend_on(true).plot_window_on(true);
   my_plot.plot(small, "< 1eps").stroke_color(boost::svg::green).fill_color(boost::svg::green).size(2);
   my_plot.plot(medium, "< 2eps").stroke_color(boost::svg::orange).fill_color(boost::svg::orange).size(2);
   my_plot.plot(large, "> 2eps").stroke_color(boost::svg::red).fill_color(boost::svg::red).size(2);
   std::string filename("bessel_i0_0_7_");
   filename += name;
   filename += ".svg";
   my_plot.write(filename);
   std::cout << "Maximum error for type " << name << " was: " << max_err << std::endl;

   max_err = 0;
   for(unsigned i = 0; i < 1000; ++i)
   {
      T input = 1 / ur2(dist);
      mp_type input2(input);
      T result = boost::math::cyl_bessel_i(0, input, policy_type());
      mp_type result2 = boost::math::cyl_bessel_i(0, input2);
      mp_type err = boost::math::relative_difference(result2, mp_type(result)) / mp_type(std::numeric_limits<T>::epsilon());
      if(boost::math::isinf(result))
      {
         if(result2 > mp_type(std::numeric_limits<T>::max()))
            err = 0;
         else
            std::cout << "Bad result at x = " << input << " result = " << result << " true result = " << result2 << std::endl;
      }
      if(result2 < mp_type(result))
         err = -err;
      if(fabs(err) > max_err)
         max_err = static_cast<float>(fabs(err));
      if(fabs(err) <= 1)
         small[1 / static_cast<double>(input)] = static_cast<double>(err);
      else if(fabs(err) <= 2)
         medium[1 / static_cast<double>(input)] = static_cast<double>(err);
      else
         large[1 / static_cast<double>(input)] = static_cast<double>(err);
   }

   y_interval = static_cast<int>(ceil(max_err / 5));
   ss.str("");
   ss << "cyl_bessel_i<" << name << ">(0, x) over [0, 7.75]\n(max error = " << std::setprecision(2) << max_err << ")" << std::endl;
   boost::svg::svg_2d_plot my_plot2;
   // Size of SVG image and X and Y range settings.
   my_plot2.x_range(0, 1 / 7.75).image_x_size(700).legend_border_color(boost::svg::lightgray).plot_border_color(boost::svg::lightgray).background_border_color(boost::svg::lightgray)
      .y_range(-(int)ceil(max_err), (int)ceil(max_err)).x_label("1 / x").title(ss.str()).y_major_interval(y_interval).x_major_interval(0.01).legend_on(true).plot_window_on(true);
   my_plot2.plot(small, "< 1eps").stroke_color(boost::svg::green).fill_color(boost::svg::green).size(2);
   my_plot2.plot(medium, "< 2eps").stroke_color(boost::svg::orange).fill_color(boost::svg::orange).size(2);
   my_plot2.plot(large, "> 2eps").stroke_color(boost::svg::red).fill_color(boost::svg::red).size(2);
   filename = "bessel_i0_7_inf_";
   filename += name;
   filename += ".svg";
   my_plot2.write(filename);

   std::cout << "Maximum error for type " << name << " was: " << max_err << std::endl;
}

int main()
{
   test_type<float>("float");
   test_type<double>("double");
#if LDBL_MANT_DIG == 64
   test_type<long double>("long double");
#endif
#ifdef BOOST_HAS_FLOAT128
   test_type<boost::multiprecision::float128>("float128");
#else
   test_type<boost::multiprecision::cpp_bin_float_quad>("quad");
#endif

   return 0;
}


//
// Copyright (c) 2018-2019 INRIA
//

#include "pinocchio/fwd.hpp"
#include "pinocchio/math/sincos.hpp"
#include <cstdlib>

#include "utils/macros.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

namespace
{
  template <typename Scalar>
  Scalar sinCosTolerance();

  template<> inline float sinCosTolerance<float>()
  {
    return 0.F;
  }

  template<> inline double sinCosTolerance<double>()
  {
    return 1e-15;
  }

  template<> inline long double sinCosTolerance<long double>()
  {
    return 1e-19;
  }
}

template<typename Scalar>
void testSINCOS(int n)
{
  for(int k = 0; k < n; ++k)
  {
    Scalar sin_value, cos_value;
    Scalar alpha = (Scalar)std::rand()/(Scalar)RAND_MAX;
    pinocchio::SINCOS(alpha,&sin_value,&cos_value);

    Scalar sin_value_ref = std::sin(alpha),
           cos_value_ref = std::cos(alpha);

    BOOST_CHECK_CLOSE_FRACTION(sin_value, sin_value_ref, sinCosTolerance<Scalar>());
    BOOST_CHECK_CLOSE_FRACTION(cos_value, cos_value_ref, sinCosTolerance<Scalar>());
  }
}

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_sincos)
{
#ifndef NDEBUG
  const int n = 1e3;
#else
  const int n = 1e6;
#endif
  testSINCOS<float>(n);
  testSINCOS<double>(n);
  testSINCOS<long double>(n);
}

BOOST_AUTO_TEST_SUITE_END()


/* ScaFES
 * Copyright (c) 2017-2018, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file TumorHeatEqnFDM.hpp
 *
 *  @brief Implementation of a n-dimensional Pennes bioheat equation problem
 *         with tumor model inside the grid.
 */

#include <iostream>
#include "ScaFES.hpp"

#include <boost/property_tree/ini_parser.hpp>

/*******************************************************************************
 ******************************************************************************/
/**
 * \class TumorHeatEqnFDM
 *  @brief Class for discretized Pennes bioheat equation problem.
 *
*/
template<typename CT, std::size_t DIM>
class TumorHeatEqnFDM : public ScaFES::Problem<TumorHeatEqnFDM<CT,DIM>, CT, DIM> {
  private:
    /** Parser for ini files. */
    using PTree = boost::property_tree::ptree;
    const PTree ptree;

  public:

    /* Based on Bousselham et al. (2017). */

    /************************************************************************/
    /** constant rho. Density. */
    const CT RHO; /* kg/m^3 */

    /** constant C. Specific heat capacity. */
    const CT C; /* J/(kg K) */

    /** constant K. Thermal conductivity). */
    const CT K; /* W/(m K) */

    /** constant Q_brain. Metabolism heat generation of the brain. */
    const CT Q_BRAIN; /* W/(m^3) */

    /** constant Q_tumor. Metabolism heat generation of the tumor. */
    const CT Q_TUMOR; /* W/(m^3) */

    /** constant rho_b. Density of the blood. */
    const CT RHO_B; /* kg/m^3 */

    /** constant C_Pb. Specific heat of the blood. */
    const CT C_PB; /* J/(kg K) */

    /** constant omega_b. Blood perfusion rate (normal brain tissue). */
    const CT OMEGA_B_BRAIN; /* 1/s */

    /** constant omega_b. Blood perfusion rate (Astrocytoma brain tumor). */
    const CT OMEGA_B_TUMOR; /* 1/s */

    /** constant T_i. Initial condition for T. */
    const CT T_I; /* K */

    /** constant h. Ambient convetion. */
    const CT H; /* W/(m^2 K) */

    /** constant T_inf. Air temperature. */
    const CT T_INF; /* K */

    /** constant q_bc. Heat flux at surface. */
    const CT Q_BC; /* W/(m^2) */

    /** constant diameter. diameter of the tumor. */
    const CT DIAMETER; /* m */

    /** constant depth. depth of the tumor. */
    const CT DEPTH; /* m */

    /* constant T_a. Temperature of artery. */
    const CT T_A; /* K */

    /************************************************************************/
    /* Auxiliary values. */
    /* Radius of tumor. */
    CT RADIUS;

    /* Center of tumor. */
    ScaFES::Ntuple<CT,DIM> tumorCenter;

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

    /** All fields which are related to the underlying problem
     * are added in terms of an entry of the parameters of
     * type \c std::vector.
     * @param params Set of ScaFES parameters.
     * @param gg Global grid.
     * @param useLeapfrog Should the leap frog scheme be used?
     * @param nameDatafield Name of the fields.
     * @param stencilWidth Stencil width of the fields.
     * @param isKnownDf Is the data field are known or unknown one?
     * @param ptree_ Config file parser.
     * @param nLayers Number of layers at the global boundary.
     * @param defaultValue Default value of fields.
     * @param writeToFile How often should the data field be written to file.
     * @param computeError Should the Linf error between the numerical
     *                     and exact solution be computed?
     * @param geomparamsInit Initial guess of geometrical parameters.
     * @param checkConvergence Should convergence be checked?
     */
    TumorHeatEqnFDM(ScaFES::Parameters const& params,
                    ScaFES::GridGlobal<DIM> const& gg,
                    bool useLeapfrog,
                    std::vector<std::string> const& nameDatafield,
                    std::vector<int> const& stencilWidth,
                    std::vector<bool> const& isKnownDf,
                    PTree const& ptree_,
                    std::vector<int> const& nLayers = std::vector<int>(),
                    std::vector<CT> const& defaultValue = std::vector<CT>(),
                    std::vector<ScaFES::WriteHowOften> const& writeToFile
                      = std::vector<ScaFES::WriteHowOften>(),
                    std::vector<bool> const& computeError = std::vector<bool>(),
                    std::vector<CT> const& geomparamsInit = std::vector<CT>(),
                    std::vector<bool> const& checkConvergence = std::vector<bool>() )
        : ScaFES::Problem<TumorHeatEqnFDM<CT, DIM>, CT, DIM>(params, gg, useLeapfrog,
                                                             nameDatafield, stencilWidth,
                                                             isKnownDf, nLayers,
                                                             defaultValue, writeToFile,
                                                             computeError, geomparamsInit,
                                                             checkConvergence),
        ptree(ptree_),
        RHO(ptree.get<CT>("Parameters.RHO")),
        C(ptree.get<CT>("Parameters.C")),
        K(ptree.get<CT>("Parameters.K")),
        Q_BRAIN(ptree.get<CT>("Parameters.Q_BRAIN")),
        Q_TUMOR(ptree.get<CT>("Parameters.Q_TUMOR")),
        RHO_B(ptree.get<CT>("Parameters.RHO_B")),
        C_PB(ptree.get<CT>("Parameters.C_PB")),
        OMEGA_B_BRAIN(ptree.get<CT>("Parameters.OMEGA_B_BRAIN")),
        OMEGA_B_TUMOR(ptree.get<CT>("Parameters.OMEGA_B_TUMOR")),
        T_I(ptree.get<CT>("Parameters.T_I")),
        H(ptree.get<CT>("Parameters.H")),
        T_INF(ptree.get<CT>("Parameters.T_INF")),
        Q_BC(ptree.get<CT>("Parameters.Q_BC")),
        DIAMETER(ptree.get<CT>("Parameters.DIAMETER")),
        DEPTH(ptree.get<CT>("Parameters.DEPTH")),
        T_A(ptree.get<CT>("Parameters.T_A"))
        {
            RADIUS = DIAMETER/2.0;

            /* Calculate center of tumor. */
            for (std::size_t pp = 0; pp < DIM; ++pp) {
                if (pp == (DIM-1)) {
                    /* In the highest dimension parameter 'depth' will be used. */
                    if (this->params().coordNodeLast()[pp] <= DIAMETER) {
                        std::cerr << "WARNING: Diameter of tumor is bigger than grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    if (this->params().coordNodeLast()[pp] < std::fabs(DEPTH) ||
                        DEPTH < 0.0) {
                        std::cerr << "WARNING: Center of tumor is outside of grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    if ((this->params().coordNodeLast()[pp] + RADIUS) < std::fabs(DEPTH)) {
                        std::cerr << "WARNING: Tumor is completely outside of grid." << std::endl;
                    }
                    if (std::fabs(this->params().coordNodeLast()[pp] - DEPTH) < RADIUS ||
                        std::fabs(DEPTH) < RADIUS) {
                        std::cerr << "WARNING: Part of tumor is outside of grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    tumorCenter[pp] = this->params().coordNodeLast()[pp] - DEPTH;
                } else {
                    /* In every other dimension the tumor will be located in the center. */
                    if (this->params().coordNodeLast()[pp] <= DIAMETER) {
                        std::cerr << "WARNING: Diameter of tumor is bigger than grid in dimension: "
                                  << pp << "." << std::endl;
                    }
                    tumorCenter[pp] = this->params().coordNodeLast()[pp]/2.0;
                }
            }
        }

    /** Evaluates all fields at one given global inner grid node.
     */
    void evalInner(std::vector< ScaFES::DataField<CT, DIM> >& /*vNew*/,
                   ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                   int const& /*timestep*/) {
    }

    /** Evaluates all fields at one given global border grid node.
     */
    void evalBorder(std::vector< ScaFES::DataField<CT, DIM> >& /*vNew*/,
                    ScaFES::Ntuple<int,DIM> const& /*idxNode*/,
                    int const& /*timestep*/) {
    }

    /** Initializes all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields (return value).
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void initInner(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
                   std::vector<TT> const& /*vOld*/,
                   ScaFES::Ntuple<int,DIM> const& idxNode,
                   int const& /*timestep*/) {
        /* Init all nodes with the same temperature. */
        vNew[0](idxNode) = T_I;
    }

    /** Initializes all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     *  @param timestep Given time step.
     */
    template<typename TT>
    void initBorder(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
                   std::vector<TT> const& vOld,
                   ScaFES::Ntuple<int,DIM> const& idxNode,
                   int const& timestep) {
        this->template initInner<TT>(vNew, vOld, idxNode, timestep);
    }

    /** Updates all unknown fields at one given global inner grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateInner(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                     std::vector<ScaFES::DataField<TT,DIM>> const& vOld,
                     ScaFES::Ntuple<int,DIM> const& idxNode,
                     int const& /*timestep*/) {
        CT omega = 0.0;
        CT Q = 0.0;
        /* Get coordinates of current node. */
        ScaFES::Ntuple<double,DIM> x = this->coordinates(idxNode);
        /* Calculate distance to tumor center. */
        CT distance = 0.0;
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            distance += (x[pp] - tumorCenter[pp]) * (x[pp] - tumorCenter[pp]);
        }
        /* Check if current point is inside tumor. */
        if (distance <= (this->RADIUS*this->RADIUS)) {
            /* Inside tumor. */
            omega = OMEGA_B_TUMOR;
            Q = Q_TUMOR;
        } else {
            /* Outside tumor, i.e. normal healthy brain tissue. */
            omega = OMEGA_B_BRAIN;
            Q = Q_BRAIN;
        }
        /* Discrete Pennes Bioheat Equation for updating inner nodes. */
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                * (vOld[0](this->connect(idxNode, 2*pp))
                                   + vOld[0](this->connect(idxNode, 2*pp+1))
                                   - 2.0 * vOld[0](idxNode))
                                / (this->gridsize(pp) * this->gridsize(pp));
        }
        vNew[0](idxNode) += this->tau() * ((RHO_B*C_PB)/(RHO*C)) * omega
                            * (T_A - vOld[0](idxNode));
        vNew[0](idxNode) += this->tau() * (1.0/(RHO*C)) * Q;
    }

    /** Updates all unknown fields at one given global border grid node.
     *  @param vNew Set of all unknown fields at new time step (return value).
     *  @param vOld Set of all unknown fields at old time step.
     *  @param idxNode Index of given grid node.
     */
    template<typename TT>
    void updateBorder(std::vector<ScaFES::DataField<TT,DIM>>& vNew,
                      std::vector<ScaFES::DataField<TT,DIM>>const& vOld,
                      ScaFES::Ntuple<int,DIM> const& idxNode,
                      int const& /*timestep*/) {
        CT omega = 0.0;
        CT Q = 0.0;
        /* Get coordinates of current node. */
        ScaFES::Ntuple<double,DIM> x = this->coordinates(idxNode);
        /* Calculate distance to tumor center. */
        CT distance = 0.0;
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            distance += (x[pp] - tumorCenter[pp]) * (x[pp] - tumorCenter[pp]);
        }
        /* Check if current point is inside tumor. */
        if (distance <= (this->RADIUS*this->RADIUS)) {
            /* Inside tumor. */
            omega = OMEGA_B_TUMOR;
            Q = Q_TUMOR;
        } else {
            /* Outside tumor, i.e. normal healthy brain tissue. */
            omega = OMEGA_B_BRAIN;
            Q = Q_BRAIN;
        }
        /* Discrete Pennes Bioheat Equation modified for updating border nodes. */
        vNew[0](idxNode) = vOld[0](idxNode);
        for (std::size_t pp = 0; pp < DIM; ++pp) {
            /* Last node/edge/surface in highest dimension will be convection
             * boundary condition. */
            if (idxNode.elem(pp) == (this->nNodes(pp)-1)) {
            /* vOld[0](this->connect(idxNode, 2*pp+1) needs to be replaced. */
                if (pp == (DIM-1)) {
                /* Cauchy boundary condition. */
                    vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                        * (vOld[0](this->connect(idxNode, 2*pp))
                                        /* + vOld[0](this->connect(idxNode, 2*pp+1) */
                                           + vOld[0](this->connect(idxNode, 2*pp))
                                           - ((2.0*this->gridsize(pp)/K)
                                              * H * (vOld[0](idxNode) - T_INF))
                                        /********************************************/
                                           - 2.0 * vOld[0](idxNode))
                                        / (this->gridsize(pp) * this->gridsize(pp));
                } else {
                /* Neumann boundary condition. */
                    vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                        * (vOld[0](this->connect(idxNode, 2*pp))
                                        /* + vOld[0](this->connect(idxNode, 2*pp+1)) */
                                           + vOld[0](this->connect(idxNode, 2*pp))
                                           - ((2.0*this->gridsize(pp)/K) *
                                              (-1.0 * Q_BC))
                                        /*********************************************/
                                           - 2.0 * vOld[0](idxNode))
                                        / (this->gridsize(pp) * this->gridsize(pp));
                }
            } else if (idxNode.elem(pp) == 0){
            /* vOld[0](this->connect(idxNode, 2*pp) needs to be replaced.
             * Neumann boundary condition. */
                vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                    /* + vOld[0](this->connect(idxNode, 2*pp)) */
                                    * (vOld[0](this->connect(idxNode, 2*pp+1))
                                       + ((2.0*this->gridsize(pp)/K) * Q_BC)
                                    /*******************************************/
                                       + vOld[0](this->connect(idxNode, 2*pp+1))
                                       - 2.0 * vOld[0](idxNode))
                                    / (this->gridsize(pp) * this->gridsize(pp));
            } else {
            /* No value needs to be replaced.
             * Use central differencing scheme. */
                vNew[0](idxNode) += this->tau() * (K/(RHO*C))
                                    * (vOld[0](this->connect(idxNode, 2*pp))
                                       + vOld[0](this->connect(idxNode, 2*pp+1))
                                       - 2.0 * vOld[0](idxNode))
                                    / (this->gridsize(pp) * this->gridsize(pp));
            }
        }

        /* These terms are independet of the boundary condition. */
        vNew[0](idxNode) += this->tau() * ((RHO_B*C_PB)/(RHO*C)) * omega
                            * (T_A - vOld[0](idxNode));
        vNew[0](idxNode) += this->tau() * (1.0/(RHO*C)) * Q;
    }

    /** Updates (2nd cycle) all unknown fields at one given global inner grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateInner2(std::vector<ScaFES::DataField<TT,DIM>>&,
                      std::vector<ScaFES::DataField<TT,DIM>> const&,
                      ScaFES::Ntuple<int,DIM> const&,
                      int const&) { }

    /** Updates (2nd cycle) all unknown fields at one given global border
     *  grid node.
     *  \remarks Only important if leap frog scheme is used.
     */
    template<typename TT>
    void updateBorder2(std::vector<ScaFES::DataField<TT,DIM>>&,
                       std::vector<ScaFES::DataField<TT,DIM>>const&,
                       ScaFES::Ntuple<int,DIM> const&,
                       int const&) { }
};


#include <iostream>
#include <boost/concept_check.hpp>
#include <ceres/ceres.h>
#include <cmath>

using namespace std;
using namespace ceres;
using ceres::AutoDiffCostFunction;
using ceres::Solver;
using ceres::Problem;
using ceres::CostFunction;
using ceres::Solve;

struct CostFuntorF1 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x2, T* residuals) const {
    residuals[0] = x1[0] + T(10)*x2[0];
    return true;
  }
};

struct CostFuntorF2 {
  template <typename T>
  bool operator()(const T* const x3, const T* const x4, T* residuals) const {
    residuals[0] = sqrt(5)*(x3[0] - x4[0]);
    return true;
  }
};

struct CostFuntorF3 {
  template <typename T>
  bool operator()(const T* const x2, const T* const x3, T* residuals) const {
    residuals[0] = (x2[0] - T(2)*x3[0])*(x2[0] - T(2)*x3[0]);
    return true;
  }
};

struct CostFuntorF4 {
  template <typename T>
  bool operator()(const T* const x1, const T* const x4, T* residuals) const {
    residuals[0] = sqrt(10)*(x1[0] - x4[0])*(x1[0] - x4[0]);
    return true;
  }
};

struct CostFunctor {
  template <typename T>
  bool operator()(const T* const x, T* residuals)  const{
    residuals[0] = x[0] - T(10) * x[1];
    residuals[1] = T(sqrt(5)) * (x[2] - x[4]);
    residuals[2] = (x[1] - T(2) * x[2]) * (x[1] - T(2) * x[2]);
    residuals[3] = T(sqrt(10)) * (x[0] - x[3]) * (x[0] - x[3]);
    return true;
  }
};

int main(int argc, char** argv)
{
  const double initial_x1 = 10, initial_x2 = 5,
	       initial_x3 = 2, initial_x4 = 1;
  double x1 = initial_x1, x2 = initial_x2,
	 x3 = initial_x3, x4 = initial_x4;

  Problem problem;
  Solver::Options options;
  //control whether the log is output to STDOUT
  options.minimizer_progress_to_stdout = true;
  Solver::Summary summary;

  CostFunction* costfunction1 =
    new AutoDiffCostFunction<CostFuntorF1,1,1,1>(new CostFuntorF1);
  CostFunction* costfunction2 =
    new AutoDiffCostFunction<CostFuntorF2,1,1,1>(new CostFuntorF2);
  CostFunction* costfunction3 =
    new AutoDiffCostFunction<CostFuntorF3,1,1,1>(new CostFuntorF3);
  CostFunction* costfunction4 =
    new AutoDiffCostFunction<CostFuntorF4,1,1,1>(new CostFuntorF4);
  problem.AddResidualBlock(costfunction1,NULL,&x1,&x2);
  problem.AddResidualBlock(costfunction2,NULL,&x3,&x4);
  problem.AddResidualBlock(costfunction3,NULL,&x2,&x3);
  problem.AddResidualBlock(costfunction4,NULL,&x1,&x4);

  Solve(options,&problem,&summary);

  cout << "x1 : " << initial_x1 << "->" << x1 << endl;
  cout << "x2 : " << initial_x2 << "->" << x2 << endl;
  cout << "x3 : " << initial_x3 << "->" << x3 << endl;
  cout << "x4 : " << initial_x4 << "->" << x4 << endl;

  cout << "---------------------line----------------------------" << endl;
  double x[4] = {initial_x1, initial_x2, initial_x3, initial_x4};
  CostFunction* costfunction = new AutoDiffCostFunction<CostFunctor, 4, 4>(new CostFunctor);
  Problem pb;
  pb.AddResidualBlock(costfunction,NULL,x);
  Solve(options,&pb,&summary);
  cout << "x[0] : " << initial_x1 << "->" << x[0] << endl;
  cout << "x[1] : " << initial_x2 << "->" << x[1] << endl;
  cout << "x[2] : " << initial_x3 << "->" << x[2] << endl;
  cout << "x[3] : " << initial_x4 << "->" << x[3] << endl;

  return 0;

}


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdBinomialCoefficientLog,Fvar) {
  using stan::math::fvar;
  using stan::math::binomial_coefficient_log;
  using boost::math::digamma;

  fvar<double> x(2004.0,1.0);
  fvar<double> y(1002.0,2.0);

  fvar<double> a = stan::math::binomial_coefficient_log(x, y);
  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0, 1002.0), a.val_);
  EXPECT_FLOAT_EQ(0.69289774, a.d_);
}

TEST(AgradFwdBinomialCoefficientLog,FvarFvarDouble) {
  using stan::math::fvar;
  using stan::math::binomial_coefficient_log;
  using stan::math::binomial_coefficient_log;

  fvar<fvar<double> > x;
  x.val_.val_ = 2004.0;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1002.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = binomial_coefficient_log(x,y);

  EXPECT_FLOAT_EQ(binomial_coefficient_log(2004.0,1002.0), a.val_.val_);
  EXPECT_FLOAT_EQ(0.69289774, a.val_.d_);
  EXPECT_NEAR(0, a.d_.val_,1e-8);
  EXPECT_FLOAT_EQ(0.0009975062, a.d_.d_);
}

struct binomial_coefficient_log_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return binomial_coefficient_log(arg1,arg2);
  }
};

TEST(AgradFwdBinomialCoefficientLog, nan) {
  binomial_coefficient_log_fun binomial_coefficient_log_;
  test_nan_fwd(binomial_coefficient_log_,3.0,5.0,false);
}


#pragma once

#include <Eigen/Dense>

#include <utils/types.hpp>

template<int Dimension>
struct Pose
{
    Eigen::Matrix<double, Dimension, Dimension> rotation;
    Eigen::Matrix<double, Dimension, 1> translation;

    /*
     * Pose constructor
    **/
    Pose(const Eigen::Matrix<double, Dimension, Dimension>& r = Eigen::Matrix<double, Dimension, Dimension>::Identity(),
         const Eigen::Matrix<double, Dimension, 1>& t = Eigen::Matrix<double, Dimension,1>::Zero())
    : rotation(r), translation(t)
    {}

    /*
     * Pose destructor
    **/
    ~Pose(){};
};

using Pose2D = Pose<2>;
using Pose3D = Pose<3>;

using Poses2D = AlignedVector<Pose2D>;
using Poses3D = AlignedVector<Pose3D>;

/*
 * the custom operator<< for Pose
**/
template<int Dimension>
std::ostream& operator<<(std::ostream& os, const Pose<Dimension>& p)
{
    os << "rotation:\n" << p.rotation.transpose() << "\n";
    os << "translation: " << p.translation.transpose();

    return os;
}


#include <armadillo>

int main() {

    const size_t dim = 6;
    arma::mat A(dim, dim, arma::fill::ones);
    A(arma::span(0, (dim / 2) - 1), arma::span(0, (dim / 2) - 1)).fill(-1.032e-310);
    A.print("A");
    arma::vec eigval = arma::eig_sym(A);
    eigval.print("eigval");
    arma::vec eigvalsqrt = arma::sqrt(eigval);
    // This has junk tiny (negative) values that need to be removed.
    eigvalsqrt.print("eigvalsqrt");

    const double thresh = 1.0e-15;
    // arma::uvec mask = arma::abs(eigval) < thresh;
    // mask.print("mask");
    // eigval(mask).zeros();
    arma::uvec mask = arma::find(arma::abs(eigval) < thresh);
    mask.print("mask");
    eigval(mask).zeros();
    eigval.print("eigval");
    arma::sqrt(eigval).print("eigvalsqrt");

    return 0;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//% Code implementing the paper "Accelerated Quadratic Proxy for Geometric Optimization", SIGGRAPH 2016.
//% Disclaimer: The code is provided as-is for academic use only and without any guarantees.
//%             Please contact the author to report any bugs.
//% Written by Shahar Kovalsky (http://www.wisdom.weizmann.ac.il/~shaharko/)
//%            Meirav Galun (http://www.wisdom.weizmann.ac.il/~/meirav/)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#include "mex.h"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "mexHelpers.cpp"

using namespace Eigen;

void projBlockRotation(VectorXd &pA, int dim)
{
	int block_size = dim*dim;
	int num_blocks = pA.size() / block_size;
	JacobiSVD<MatrixXd> svdA(dim, dim, (ComputeFullU | ComputeFullV));
	bool flipped;
	MatrixXd U, V;
	Map<MatrixXd> currA(pA.data(), dim, dim);

	// project
	for (int ii = 0; ii < num_blocks; ii++)
	{
		// get current block
		new (&currA) Map<MatrixXd>(pA.data() + ii*block_size, dim, dim);
		// sign of determinant
		flipped = (currA.determinant() < 0);
		// svd
		svdA.compute(currA);
		// compute frames
		U = svdA.matrixU();
		V = svdA.matrixV();
		// ssvd
		if (flipped)
		{
			U.col(dim - 1) = -U.col(dim - 1);
		}
		// project block
		currA = U*V.transpose();
	}
}

void mexFunction(int nlhs, mxArray *plhs[],
	int nrhs, const mxArray*prhs[])

{
	// assign input
	int A_rows = mxGetM(prhs[0]); // # rows of A
	int A_cols = mxGetN(prhs[0]); // # cols of A
	double *dim;
	const Map<VectorXd> A(mxGetPr(prhs[0]), A_rows, A_cols);
	dim = mxGetPr(prhs[1]);

	// init output
	VectorXd pA(A_rows);
	pA = A;

	// project
	projBlockRotation(pA, *dim);

	// assign outputs
	mapDenseMatrixToMex(pA, &(plhs[0]));

}


/*! \file solveeom.cpp
    \brief 単振り子に対して運動方程式を解くクラスの実装

    Copyright © 2016-2018 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/
#include "solveeom.h"
#include <cmath>                                // for std::sin, std::cos
#include <fstream>                              // for std::ofstream
#include <boost/assert.hpp>                     // for BOOST_ASSERT
#include <boost/format.hpp>                     // for boost::format
#include <boost/math/constants/constants.hpp>   // for boost::math::constants::pi
#include "solveeommain.h"

namespace solveeom {
    // #region コンストラクタ・デストラクタ

    SolveEoM::SolveEoM(float l, float r, float theta0) :
		Isconsider_Inertial_Resistance(nullptr, [this](auto isconsider_inertial_resistance) { return isconsider_inertial_resistance_ = isconsider_inertial_resistance; }),
        Theta([this] { return static_cast<float>(x_[0]); }, [this](auto theta) { return x_[0] = theta; }),
		Theta0(nullptr, [this](auto theta0) { return theta0_ = theta0; }),
		Time([this] { return static_cast<float>(t_); }, [this](auto t) { return t_ = t; }),
		V([this] { return static_cast<float>(l_ * x_[1]); }, [this](auto v) { return x_[1] = v / l_; }),
        l_(l),
		omega0_2_(g / l_),
        r_(r),
		m_(4.0 / 3.0 * boost::math::constants::pi<double>() * r * r * r * SolveEoM::ALUMINIUMRHO),
		gamma_(3.0 * boost::math::constants::pi<double>() * r_ * AIRMYU / m_),
		stepper_(SolveEoM::EPS, SolveEoM::EPS),
		t_(0.0),
		theta0_(theta0),
		x_({ theta0, 0.0 })
    {
    }

    // #endregion コンストラクタ・デストラクタ

    // #region publicメンバ関数

	float SolveEoM::gettheta_fumofumobun_approx() const
	{
		return static_cast<float>(theta0_ * std::exp(-gamma_ * t_) *
								  std::cos(std::sqrt((omega0_2_ - gamma_ * gamma_) * (3.0 + std::cos(theta0_ * std::exp(-gamma_ * t_)))) / 2.0 * t_));
	}

	float SolveEoM::getv_fumofumobun_approx() const
    {
		auto const term1 = -gamma_ * gettheta_fumofumobun_approx();

		auto const alpha = 0.5 * std::sqrt((omega0_2_ - gamma_ * gamma_) * (3.0 + std::cos(theta0_ * std::exp(-gamma_ * t_))));

		auto const term2 = -alpha * theta0_* std::exp(-gamma_ * t_) * std::sin(alpha * t_) *
						   (0.5 * theta0_ * gamma_ * std::exp(-gamma_ * t_) * std::sin(theta0_ * std::exp(-gamma_ * t_)) /
						   (3.0 + std::cos(theta0_ * std::exp(-gamma_ * t_))) * t_ + 1.0);

		return l_ * (term1 + term2);
    }

	float SolveEoM::kinetic_energy(double v) const
	{
		return static_cast<float>(0.5 * m_ * sqr(v));
	}

    float SolveEoM::operator()(float dt)
    {
        boost::numeric::odeint::integrate_adaptive(
            stepper_,
            getEoM(),
            x_,
            0.0,
            static_cast<double>(dt),
            SolveEoM::DX);

        return static_cast<float>(x_[0]);
    }

    void SolveEoM::operator()(double dt, std::string const & filename, double t)
    {
        std::ofstream result(filename);

        boost::numeric::odeint::integrate_const(
            stepper_,
            getEoM(),
            x_,
            0.0,
            t,
            dt,
            [&result, this](auto const & x, auto const t)
            {
				result << boost::format("%.3f, %.15f, %.15f\n") % t % x[0] % gettheta_fumofumobun_approx();
            });
    }

	float SolveEoM::potential_energy(double theta) const
	{
		return static_cast<float>(m_ * SolveEoM::g * l_ * (1.0 - std::cos(theta)));
	}

	void SolveEoM::timereset()
    {
		t_ = 0.0;
    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

	std::function<void(SolveEoM::state_type const &, SolveEoM::state_type &, double const)> SolveEoM::getEoM() const
    {
        auto const eom = [this](state_type const & x, state_type & dxdt, double const)
        {
            // dθ/dt = v / l
            dxdt[0] = x[1];

            // 振り子に働く力
            auto const f1 = -SolveEoM::g * std::sin(x[0]) / l_;

			// レイノルズ数
			auto const Re = 2.0 * r_ * std::fabs(l_ * x[1]) / AIRNYU;

            // 粘性抵抗
            auto const F = 6.0 * boost::math::constants::pi<double>() * AIRMYU * r_ * l_ * x[1];

			// レイノルズ数が閾値より小さいか、「慣性抵抗も考慮」チェックボックスが外れていたら
			if (Re < SolveEoM::REYNOLDS_THRESHOLD || !isconsider_inertial_resistance_) {
				// 粘性抵抗のみを考慮する
				dxdt[1] = f1 - F / (m_ * l_);

				return;
			}

			auto const FD = 0.5 * AIRRHO * boost::math::constants::pi<double>() * sqr(r_ * (l_ * x[1]));

			// Drag coefficient
			double CD;

			// N.-S. Cheng, Comparison of formulas for drag coefficient and settling velocity of
			// spherical particles, Powder Technology 189 (2009) 395–398.
			if (Re <= 3000) {
				CD = 24.0 / Re * std::pow(1.0 + 0.27 * Re, 0.43) + 0.47 * (1.0 - std::exp(-0.04 * std::pow(Re, 0.38)));
			}
			else {
				// Re > 3000
				// Almedeij J. Drag coefficient of flow around a sphere: Matching asymptotically the wide
				// trend. std::powder Technology. (2008);doi:10.1016/j.std::powtec.2007.12.006.
				auto const phi1 = std::pow(24.0 / Re, 10) + std::pow(21.0 * std::pow(Re, -0.67), 10) +
					std::pow(4.0 * std::pow(Re, -0.33), 10) + std::pow(0.4, 10);
				auto const phi2 = 1.0 / (1.0 / std::pow(0.148 * std::pow(Re, 0.11), 10) + 1.0 / std::pow(0.5, 10));
				auto const phi3 = std::pow((1.57E+8) * std::pow(Re, -1.625), 10);
				auto const phi4 = 1.0 / (1.0 / std::pow((6.0E-17) * std::pow(Re, 2.63), 10) + 1.0 / std::pow(0.2, 10));

				CD = std::pow((1.0 / (1.0 / (phi1 + phi2) + 1.0 / phi3) + phi4), 0.1);
			}

			// 慣性抵抗÷(m×l)
			auto const f2 = (x[1] >= 0.0) ? -FD * CD / (m_ * l_) : FD * CD / (m_ * l_);
			dxdt[1] = f1 + f2 - F / (m_ * l_);
        };

        return eom;
    }

    // #endregion privateメンバ関数
}


/**
 * \file dcs/math/stats/distribution/chi_squared.hpp
 *
 * \brief The (Negative) Chi Squared probability distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_CHI_SQUARED_HPP
#define DCS_MATH_STATS_DISTRIBUTION_CHI_SQUARED_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/chi_squared.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/gamma.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

/**
 * \brief The Chi Squared distribution with degrees of freedom parameter
 * \f$\nu\f$.
 *
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * The probability density function (pdf):
 * \f[
 *   \Pr(x|\nu) = \frac{1}{2^{k/2}\Gamma(k/2)}\; x^{k/2-1} e^{-x/2}
 * \f]
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class chi_squared_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit chi_squared_distribution(support_type df)
		: dist_(df)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a random number distributed according to this
	 * chi_squared distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \return A random number distributed according to this chi_squared
	 * distribution.
	 *
	 * A \c chi_squared random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\nu) = \frac{1}{2^{k/2}\Gamma(k/2)}\; x^{k/2-1} e^{-x/2}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		return ::dcs::math::stats::rand(
			gamma_distribution<value_type>(
				dist_.degrees_of_freedom()/value_type(2),
				value_type(2)
			),
			rng
		);
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * chi_squared distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * chi_squared distribution.
	 *
	 * A \c chi_squared random number distribution produces random numbers
	 * \f$x > 0\f$ distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\nu) = \frac{1}{2^{k/2}\Gamma(k/2)}\; x^{k/2-1} e^{-x/2}
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, ::std::size_t n) const
	{
		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(
				::dcs::math::stats::rand(
					gamma_distribution<value_type>(
						dist_.degrees_of_freedom()/value_type(2),
						value_type(2)
					),
					rng
				)
			);
		}

		return rnds;
	}

	public: support_type degrees_of_freedom() const
	{
		return dist_.degrees_of_freedom();
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	private: ::boost::math::chi_squared_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, chi_squared_distribution<RealT,PolicyT> const& dist)
{
	return os << "ChiSquared("
			  << "df=" <<  dist.degrees_of_freedom()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_CHI_SQUARED_HPP


#include "CEGO/CEGO.hpp"
#include <Eigen/Dense>
#if defined(PYBIND11)
#include <pybind11/embed.h>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
namespace py = pybind11;
#endif
#include <atomic>

std::atomic_size_t Ncalls(0);
using CEGO::EArray;

class Bumps {
public:
    std::size_t Nbumps;
    Eigen::ArrayXd xb0, yb0, xp, yp, zp;
    double gamma = 5;

    Bumps(std::size_t Nbumps, std::size_t Npoints) : Nbumps(Nbumps) {
        xb0 = (Eigen::ArrayXd::Random(Nbumps)*10).round();
        yb0 = (Eigen::ArrayXd::Random(Nbumps)*10).round();

        xp = Eigen::ArrayXd::Random(Npoints)*10;
        yp = Eigen::ArrayXd::Random(Npoints)*10;
        zp = f_givenxy(xb0, yb0, xp, yp);

        Eigen::ArrayXd c = xb0; c.conservativeResize(xb0.size() * 2); c.tail(Nbumps) = yb0;
        double checkval = objective(c);
        assert(std::abs(checkval) < 1e-16);
    }
    /**
     * @brief Calculate the functional value for a set of vectors of points
     * @brief xb The x coordinate of the center of the bump
     * @brief yb The y coordinate of the center of the bump
     * @brief x The x coordinate of the points to be evaluated
     * @brief y The y coordinate of the points to be evaluated
     */
    Eigen::ArrayXd f_givenxy(const Eigen::ArrayXd &xb, const Eigen::ArrayXd &yb, const Eigen::ArrayXd&x, const Eigen::ArrayXd &y) {
        Eigen::ArrayXd s = Eigen::ArrayXd::Zero(x.size());
        for (auto i = 0; i < xb.size(); ++i) {
            s += (-gamma*(x-xb[i]).square() - gamma*(y-yb[i]).square()).exp();
        }
        return s;
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const EArray<double> &c = static_cast<const CEGO::NumericalIndividual<double>*>(pind)->get_coefficients();
        return objective(c);
    }
    double objective_vec(const EArray<double> &c) {
        return objective(c);
    }
    double penalty_vec(const EArray<double> &c) {
        return penalty(c);
    }
    double penalty(const Eigen::ArrayXd &c) {
        Eigen::ArrayXd q = c - c.floor();
        constexpr double MY_PI = 3.14159265358979323846;
        Eigen::ArrayXd penalty = 0.25*(0.5 - 0.5*cos(2*MY_PI*q));
        return 100*penalty.sum();
    }
    double objective(const Eigen::ArrayXd &c) {
        Ncalls ++;
        return (f_givenxy(c.head(Nbumps), c.tail(Nbumps), xp, yp) - zp).square().sum() + penalty(c);
    }
    void plot_surface() {
        #if defined(PYBIND11)
        using namespace pybind11::literals;
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        std::size_t Nx = 100, Ny = 100;
        Eigen::MatrixXd X = Eigen::RowVectorXd::LinSpaced(Nx, -1, 1).replicate(Ny, 1);
        Eigen::MatrixXd Y = Eigen::VectorXd::LinSpaced(Ny, -1, 1).replicate(Nx, 1);
        X.resize(Nx*Ny,1); Y.resize(Nx*Ny,1);
        Eigen::MatrixXd Z = f_givenxy(xb0, yb0, X.array(), Y.array()).matrix();
        X.resize(Nx, Ny); Y.resize(Nx, Ny); Z.resize(Nx, Ny);
        plt.attr("contourf")(X, Y, Z, "N"_a=3000);
        plt.attr("scatter")(xp, yp);
        plt.attr("show")();
        #else
        std::cout << "No pybind11 support, so no plots\n";
        #endif
    }
    void plot_trace(const std::vector<double> &best_costs) {
        #if defined(PYBIND11)
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(best_costs);
        plt.attr("show")();
        #else
        std::cout << "No pybind11 support, so no plots\n";
        #endif
    }
};

void do_one(const std::size_t Nbumps, const std::string &root, std::size_t i) {

    std::srand((unsigned int)time(0));

    std::size_t Npoints = Nbumps*10;
    Bumps bumps(Nbumps, Npoints);
    //bumps.plot_surface();

    auto D = 2*Nbumps; // twice because they are pairs
    CEGO::CostFunction<double> cost_wrapper = std::bind((double (Bumps::*)(const CEGO::AbstractIndividual *)) &Bumps::objective, bumps, std::placeholders::_1);
    auto layers = CEGO::Layers<double>(cost_wrapper, D, 30*D, 1, 10);
    // Apply the bounds (all in [-1,1])
    layers.set_bounds(std::vector<CEGO::Bound>(D, CEGO::Bound(std::pair<double, double>(-15.0, 15.0))));
    layers.parallel = true;
    layers.parallel_threads = 4;
    layers.set_builtin_evolver(CEGO::BuiltinEvolvers::differential_evolution);
    auto fl = layers.get_evolver_flags();
    fl["CR"] = 0.9;
    fl["Fmin"] = 0.5;
    fl["Fmax"] = 0.5;
    fl["Nelite"] = 1;
    layers.set_evolver_flags(fl);

    auto f = [](const CEGO::Result &r) {
        if (r.ssq < 1) {
            return CEGO::FilterOptions::accept;
        }
        else {
            return CEGO::FilterOptions::reject;
        }
    };
    layers.set_filtering_function(f);
    layers.set_logging_scheme(CEGO::LoggingScheme::custom);

    std::vector<double> best_costs;
    std::vector<std::vector<double> > objs;
    double VTR = 1e-14, best_cost = 999999.0;
    auto startTime = std::chrono::system_clock::now();
    for (auto counter = 0; counter < 500000; ++counter) {
        layers.do_generation();

        // Store the best objective function in each layer
        std::vector<double> oo;
        for (auto &&cost_coefficients : layers.get_best_per_layer()) {
            oo.push_back(std::get<0>(cost_coefficients));
        }
        objs.push_back(oo);

        // For the overall best result, print it, and write JSON to file
        auto best_layer = layers.get_best();
        best_cost = std::get<0>(best_layer); best_costs.push_back(best_cost);
        auto best_coeffs = std::get<1>(best_layer);
        if (counter % 10 == 0){
            std::cout << counter << ": best-penalty: " << best_cost-bumps.penalty_vec(best_coeffs) << "; best: " << best_cost << "\n ";// << CEGO::vec2string(best_coeffs) << "\n";
        }
        if (best_cost < VTR){ break; }
    }
    auto endTime = std::chrono::system_clock::now();
    double elap = std::chrono::duration<double>(endTime - startTime).count();
    std::cout << "run:" << elap << " s\n";

    //bumps.plot_trace(best_costs);
    FILE* fp = fopen((root + "run-" + std::to_string(i) + ".txt").c_str(), "w");
    for (auto j = 0; j < best_costs.size(); ++j){
        fprintf(fp, "%12.8e", best_costs[j]);
        if (j < best_costs.size() - 1) {
            fprintf(fp, ", ");
        }
    }
    fclose(fp);
    std::cout << bumps.xb0 << std::endl;
    std::cout << bumps.yb0 << std::endl;
    std::cout << "NFE:" << Ncalls << std::endl;
}

int main() {
    for (auto i = 0; i < 10; ++i) {
        do_one(5, "N5-", i);
    }
    for (auto i = 0; i < 10; ++i) {
        do_one(10, "N10-", i);
    }
}


// Copyright (c) 2005-2009  INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL$
// $Id$
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Frédérik Paradis

#include <iostream>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_root_of.h>
#include <CGAL/CGAL_Ipelet_base.h>
#include <CGAL/Construct_theta_graph_2.h>
#include <CGAL/Construct_yao_graph_2.h>
#include <CGAL/Compute_cone_boundaries_2.h>
#include <CGAL/Cone_spanners_enum_2.h>
#include <CGAL/property_map.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>

namespace CGAL_cone_spanners {

typedef CGAL::Exact_predicates_exact_constructions_kernel_with_root_of Kernel;
typedef Kernel::Point_2                                                Point_2;
typedef Kernel::Direction_2                                            Direction_2;

/* Note: due to a bug in the boost library, using a directed graph
 * will cause a compilation error with g++ and clang++ when using c++11 standard.
 * See https://lists.boost.org/Archives/boost/2016/05/229458.php.
 */
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::undirectedS,
                              Point_2
                             > Graph;

const std::string labels[] = {  "Theta-k-graph", "Yao-k-graph", "Half-theta-k-graph with even cones", "Half-Yao-k-graph with even cones", "Half-theta-k-graph with odd cones", "Half-Yao-k-graph with odd cones", "k cones", "Help" };
const std::string hmsg[] = {
  "Draws a theta-graph with k cones.",
  "Draws a Yao-graph with k cones.",
  "Draws an half-theta-graph with the even of k cones.",
  "Draws an half-Yao-graph with the even of k cones.",
  "Draws an half-theta-graph with the odd of k cones.",
  "Draws an half-Yao-graph with the odd of k cones.",
  "Draws k cones around the points.",
};

class Cone_spanners_ipelet
  : public CGAL::Ipelet_base<Kernel,7> {
public:
  Cone_spanners_ipelet()
    :CGAL::Ipelet_base<Kernel,7>("Cone Spanners",labels,hmsg){}
  void protected_run(int);
private:
};

void Cone_spanners_ipelet::protected_run(int fn)
{
  std::vector<Point_2> lst;
  int number_of_cones;
  switch (fn){
    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    {
      std::vector<Point_2> points_read;
      read_active_objects(
        CGAL::dispatch_or_drop_output<Point_2>(std::back_inserter(points_read))
      );

      if (points_read.empty()) {
        print_error_message("No mark selected");
        return;
      }
      for(std::vector<Point_2>::iterator it = points_read.begin(); it != points_read.end(); it++) {
        if(std::find(points_read.begin(), it, *it) == it) {
          lst.push_back(*it);
        }
      }

      int ret_val;
      boost::tie(ret_val,number_of_cones)=request_value_from_user<int>("Enter the number of cones");
      if (ret_val < 0) {
        print_error_message("Incorrect value");
        return;
      }
      if(number_of_cones < 2) {
        print_error_message("The number of cones must be larger than 1!");
        return;
      }
      break;
    }
    case 7:
      show_help();
      return;
  }

  if(fn >= 0 && fn <= 5) {
    CGAL::Cones_selected cones_selected = CGAL::ALL_CONES;
    if(fn == 2 || fn == 3)
      cones_selected = CGAL::EVEN_CONES;
    else if(fn == 4 || fn == 5)
      cones_selected = CGAL::ODD_CONES;

    Graph g;
    switch (fn){
      case 0:
      case 2:
      case 4:
      {
        CGAL::Construct_theta_graph_2<Kernel, Graph> theta(number_of_cones, Direction_2(1,0), cones_selected);
        theta(lst.begin(), lst.end(), g);
        break;
      }
      case 1:
      case 3:
      case 5:
      {
        CGAL::Construct_yao_graph_2<Kernel, Graph> yao(number_of_cones, Direction_2(1,0), cones_selected);
        yao(lst.begin(), lst.end(), g);
        break;
      }
    }
    boost::graph_traits<Graph>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      boost::graph_traits<Graph>::edge_descriptor e = *ei;
      boost::graph_traits<Graph>::vertex_descriptor  u = source(e, g);
      boost::graph_traits<Graph>::vertex_descriptor  v = target(e, g);
      draw_in_ipe(Segment_2(g[u], g[v]));
    }
    group_selected_objects_();
  }
  else if(fn == 6) {
    CGAL::Compute_cone_boundaries_2<Kernel> cones;
    std::vector<Direction_2> directions(number_of_cones);
    cones(number_of_cones, Direction_2(1,0), directions.begin());
    for(std::vector<Point_2>::iterator it = lst.begin(); it != lst.end(); it++) {
      for(std::vector<Direction_2>::iterator dir = directions.begin(); dir != directions.end(); dir++) {
        draw_in_ipe(Segment_2(*it,*it + 100*dir->to_vector()));
      }
      group_selected_objects_();
      get_IpePage()->deselectAll();
    }
  }
}

}

CGAL_IPELET(CGAL_cone_spanners::Cone_spanners_ipelet)


//////////////////////////////////////////////////////////////////////////////
// random::chi_squared.hpp                                                  //
//                                                                          //
//  (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_RANDOM_CHI_SQUARED_HPP_ER_2009
#define BOOST_RANDOM_CHI_SQUARED_HPP_ER_2009
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared.hpp>
#include <boost/math/distributions/chi_squared.hpp>

namespace boost{
namespace random{

    template<typename T>
    class chi_squared_distribution{
            typedef boost::normal_distribution<T> nd_t;
        public:
            typedef typename nd_t::input_type input_type;
            typedef typename nd_t::result_type result_type;

        chi_squared_distribution(): df_(2) {}
        chi_squared_distribution(unsigned df): df_(df) {}
        chi_squared_distribution(const chi_squared_distribution& that)
        :df_(that.df_){}
        chi_squared_distribution&
        operator=(const chi_squared_distribution& that){
            if(&that!=this){
                df_ = that.df_;
            }
            return *this;
        }

        template<typename U>
        result_type
        operator()(U& urng){
            typedef boost::variate_generator<U&,nd_t> vg_t;
            static nd_t nd(0,1);
            result_type z;
            result_type res = static_cast<T>(0);
            for(unsigned i = 0; i<this->df(); i++){
                z = nd(urng);
                res += (z * z);
            }
            return res;
        }

        result_type min () const { return static_cast<result_type>(0); }
        result_type max () const { return (nd_t().max)(); }
        unsigned df()const { return df_; }

        typedef math::chi_squared_distribution<T> math_dist_;

        operator math_dist_ (){ return math_dist(this->df()); }

        private:
            unsigned df_;
    };

}
}

#endif // CHI_SQUARE_HPP_INCLUDED


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace std;

int main( int argc, char** argv )
{
    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);
    Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
    Eigen::Vector3d t1(0.3, 0.1, 0.1);
    Eigen::Vector3d t2(-0.1, 0.5, 0.3);
    Eigen::Vector3d p1(0.5, 0, 0.2);

    Eigen::Quaterniond q1_one = q1.normalized();
    Eigen::Quaterniond q2_one = q2.normalized();

    //way1

    Eigen::Vector3d v = q1_one.inverse() * (p1 - t1);
    /** first translation
     * first translation, qi_one is world to camera
     * (p1 - t1) is in camera coordinate, after that , it is in world coordinator
     * so , we can use the same way to compute another point
     */

    return 0;
}