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

#include "eptlib/finite_difference.h"

#include <complex>
#include <iostream>

#include <Eigen/Dense>

using namespace eptlib;

// FDSavitzkyGolayFilter constructor
FDSavitzkyGolayFilter::
FDSavitzkyGolayFilter(const Shape &shape) :
    shape_(shape),
    m_vox_(std::accumulate(shape.GetSize().begin(),shape.GetSize().end(),1,std::multiplies<int>())) {
    double toll = 1e-10;
    std::array<int,NDIM> mm = shape_.GetSize();
    // check to have odd shape size
    assert(m_vox_%2);
    // get the central voxel address
    int idx0 = m_vox_/2;
    std::array<int,NDIM> ii0;
    for (int d = 0; d<NDIM; ++d) {
        ii0[d] = mm[d]/2;
    }
    // initialise the design matrix
    int n_row = shape_.GetVolume();
    int n_col2 = 1 + NDIM;
    int n_col1 = (NDIM*(NDIM+1))/2;
    Eigen::MatrixXd F2(n_row,n_col2);
    Eigen::MatrixXd F1(n_row,n_col1);
    // fill the design matrix
    std::array<int,NDIM> ii;
    std::array<double,NDIM> di;
    int r = 0;
    for (ii[2] = 0; ii[2]<mm[2]; ++ii[2]) {
        for (ii[1] = 0; ii[1]<mm[1]; ++ii[1]) {
            for (ii[0] = 0; ii[0]<mm[0]; ++ii[0]) {
                if (shape_[ii]) {
                    for (int d = 0; d<NDIM; ++d) {
                        di[d] = ii[d]-ii0[d];
                    }
                    // design matrix for even quantities
                    F2(r,0) = 1.0;
                    for (int d = 0; d<NDIM; ++d) {
                        F2(r,1+d) = di[d]*di[d];
                    }
                    // design matrix for odd quantities
                    int c = 0;
                    for (int d = 0; d<NDIM; ++d) {
                        F1(r,d) = di[d];
                        for (int d2 = d+1; d2<NDIM; ++d2) {
                            F1(r,NDIM+c) = di[d]*di[d2];
                            ++c;
                        }
                    }
                    ++r;
                }
            }
        }
    }
    // check that all lines are filled
    Eigen::VectorXd v = F2.cwiseAbs().colwise().sum();
    double ref = v.maxCoeff();
    for (int d = 0; d<NDIM; ++d) {
        if (v[d]<toll*ref) {
            throw std::runtime_error("Impossible to set-up the Savitzky-Golay filter with the provided shape: no points along direction "+std::to_string(d)+".");
        }
    }
    v = F1.cwiseAbs().colwise().sum();
    ref = v.maxCoeff();
    for (int d = 0; d<NDIM; ++d) {
        if (v[d]<toll*ref) {
            throw std::runtime_error("Impossible to set-up the Savitzky-Golay filter with the provided shape: no points along direction "+std::to_string(d)+".");
        }
    }
    for (int c = NDIM; c<n_col1; ++c) {
        if (v[c]<toll*ref) {
            --n_col1;
            F1.col(c).swap(F1.col(n_col1));
            --c;
        }
    }
    F1.conservativeResize(Eigen::NoChange, n_col1);
    // solve the normal equations
    Eigen::MatrixXd A;
    if (shape_.IsSymmetric()) {
        A = (F2.transpose()*F2).inverse()*F2.transpose();
    } else {
        Eigen::MatrixXd F(n_row,n_col2+n_col1);
        F << F2,F1;
        A = (F.transpose()*F).inverse()*F.transpose();
    }
    // Compute the kernel for the laplacian
    for (int d = 0; d<NDIM; ++d) {
        lapl_kernel_[d].resize(m_vox_,0.0);
        r = 0;
        for (int idx = 0; idx<m_vox_; ++idx) {
            if (shape_[idx]) {
                lapl_kernel_[d][idx] = 2.0*A(1+d,r);
                ++r;
            }
        }
    }
    // Compute the kernel for the gradient
    int c_base = 1+NDIM;
    if (shape_.IsSymmetric()) {
        A = (F1.transpose()*F1).inverse()*F1.transpose();
        c_base = 0;
    }
    for (int d = 0; d<NDIM; ++d) {
        grad_kernel_[d].resize(m_vox_,0.0);
        r = 0;
        for (int idx = 0; idx<m_vox_; ++idx) {
            if (shape_[idx]) {
                grad_kernel_[d][idx] = A(c_base+d,r);
                ++r;
            }
        }
    }
    return;
}

// FDSavitzkyGolayFilter apply
template <typename NumType>
EPTlibError FDSavitzkyGolayFilter::
Apply(const DifferentialOperator diff_op, NumType *dst, const NumType *src, const std::array<int,NDIM> &nn, const std::array<double,NDIM> &dd) const {
    const int n_vox = std::accumulate(nn.begin(),nn.end(),1,std::multiplies<int>());
    std::array<int,NDIM> ii;
    std::array<int,NDIM> rr;
    std::array<int,NDIM> inc;
    for (int d = 0; d<NDIM; ++d) {
        rr[d] = shape_.GetSize()[d]/2;
    }
    inc[0] = 1;
    inc[1] = nn[0]-shape_.GetSize()[0];
    inc[2] = nn[0]*(nn[1]-shape_.GetSize()[1]);
    // loop over field voxels
    for (ii[2] = rr[2]; ii[2]<nn[2]-rr[2]; ++ii[2]) {
        for (ii[1] = rr[1]; ii[1]<nn[1]-rr[1]; ++ii[1]) {
            for (ii[0] = rr[0]; ii[0]<nn[0]-rr[0]; ++ii[0]) {
                std::array<int,NDIM> ii_l;
                std::vector<NumType> field_crop(m_vox_,0.0);
                // inner loop over kernel voxels
                int idx_l = 0;
                int idx_g = ii[0]-rr[0] + nn[0]*(ii[1]-rr[1] + nn[1]*(ii[2]-rr[2]));
                for (ii_l[2] = -rr[2]; ii_l[2]<=rr[2]; ++ii_l[2]) {
                    for (ii_l[1] = -rr[1]; ii_l[1]<=rr[1]; ++ii_l[1]) {
                        for (ii_l[0] = -rr[0]; ii_l[0]<=rr[0]; ++ii_l[0]) {
                            if (shape_[idx_l]) {
                                // set the field_crop to the field value
                                field_crop[idx_l] = src[idx_g];
                            }
                            ++idx_l;
                            idx_g += inc[0];
                        }
                        idx_g += inc[1];
                    }
                    idx_g += inc[2];
                }
                // compute the derivative
                int idx = ii[0] + nn[0]*(ii[1] + nn[1]*ii[2]);
                if (diff_op==DifferentialOperator::GradientX||
                    diff_op==DifferentialOperator::GradientY||
                    diff_op==DifferentialOperator::GradientZ) {
                    int d = static_cast<int>(diff_op) -
                        static_cast<int>(DifferentialOperator::GradientX);
                    dst[idx] = FirstOrder(d,field_crop,dd);
                } else if (diff_op==DifferentialOperator::GradientXX||
                    diff_op==DifferentialOperator::GradientYY||
                    diff_op==DifferentialOperator::GradientZZ) {
                    int d = static_cast<int>(diff_op) -
                        static_cast<int>(DifferentialOperator::GradientXX);
                    dst[idx] = SecondOrder(d,field_crop,dd);
                } else if (diff_op==DifferentialOperator::Laplacian) {
                    dst[idx] = Laplacian(field_crop,dd);
                }
            }
        }
    }
    return EPTlibError::Success;
}

// FDSavitzkyGolayFilter apply kernel first order derivative
template <typename NumType>
NumType FDSavitzkyGolayFilter::
FirstOrder(const int d, const std::vector<NumType> &field_crop, const std::array<double,NDIM> &dd) const {
    NumType dst = std::inner_product(grad_kernel_[d].begin(),grad_kernel_[d].end(),field_crop.begin(),static_cast<NumType>(0.0))/dd[d];
    return dst;
}
// FDSavitzkyGolayFilter apply kernel second order derivative
template <typename NumType>
NumType FDSavitzkyGolayFilter::
SecondOrder(const int d, const std::vector<NumType> &field_crop, const std::array<double,NDIM> &dd) const {
    NumType dst = std::inner_product(lapl_kernel_[d].begin(),lapl_kernel_[d].end(),field_crop.begin(),static_cast<NumType>(0.0))/dd[d]/dd[d];
    return dst;
}
// FDSavitzkyGolayFilter apply kernel laplacian
template <typename NumType>
NumType FDSavitzkyGolayFilter::
Laplacian(const std::vector<NumType> &field_crop, const std::array<double,NDIM> &dd) const {
    NumType dst = 0.0;
    for (int d = 0; d<NDIM; ++d) {
        dst += std::inner_product(lapl_kernel_[d].begin(),lapl_kernel_[d].end(),field_crop.begin(),static_cast<NumType>(0.0))/dd[d]/dd[d];
    }
    return dst;
}

// FDSavitzkyGolayFilter getters
const Shape& FDSavitzkyGolayFilter::
GetShape() const {
    return shape_;
}
const std::array<std::vector<double>,NDIM>& FDSavitzkyGolayFilter::
GetLaplKernel() const {
    return lapl_kernel_;
}
const std::array<std::vector<double>,NDIM>& FDSavitzkyGolayFilter::
GetGradKernel() const {
    return grad_kernel_;
}

// FDSavitzkyGolayFilter specialisations
template EPTlibError FDSavitzkyGolayFilter::Apply<double>(const DifferentialOperator diff_op,double *dst, const double *src, const std::array<int,NDIM> &nn, const std::array<double,NDIM> &dd) const;
template EPTlibError FDSavitzkyGolayFilter::Apply<std::complex<double> >(const DifferentialOperator diff_op,std::complex<double> *dst, const std::complex<double> *src, const std::array<int,NDIM> &nn, const std::array<double,NDIM> &dd) const;
template double FDSavitzkyGolayFilter::FirstOrder<double>(const int d,const std::vector<double> &field_crop,const std::array<double,NDIM> &dd) const;
template std::complex<double> FDSavitzkyGolayFilter::FirstOrder<std::complex<double> >(const int d,const std::vector<std::complex<double> > &field_crop,const std::array<double,NDIM> &dd) const;
template double FDSavitzkyGolayFilter::SecondOrder<double>(const int d,const std::vector<double> &field_crop,const std::array<double,NDIM> &dd) const;
template std::complex<double> FDSavitzkyGolayFilter::SecondOrder<std::complex<double> >(const int d,const std::vector<std::complex<double> > &field_crop,const std::array<double,NDIM> &dd) const;
template double FDSavitzkyGolayFilter::Laplacian<double>(const std::vector<double> &field_crop,const std::array<double,NDIM> &dd) const;
template std::complex<double> FDSavitzkyGolayFilter::Laplacian<std::complex<double> >(const std::vector<std::complex<double> > &field_crop,const std::array<double,NDIM> &dd) const;

// Apply the FD filter to an wrapped phase input field.
EPTlibError eptlib::WrappedPhaseDerivative(const DifferentialOperator diff_op,
    double *dst, const double *src, const std::array<int,NDIM> &nn,
    const std::array<double,NDIM> &dd, const FDSavitzkyGolayFilter &fd_filter) {
    int n_vox = std::accumulate(nn.begin(),nn.end(),1,std::multiplies<int>());
    // initialize the exponential map
    std::vector<std::complex<double> > emap(n_vox);
    for (int idx = 0; idx<n_vox; ++idx) {
        emap[idx] = std::exp(std::complex<double>(0,src[idx]));
    }
    // compute the derivative...
    if (diff_op==DifferentialOperator::GradientX||
        diff_op==DifferentialOperator::GradientY||
        diff_op==DifferentialOperator::GradientZ) {
        // ...first order
        std::vector<std::complex<double> > d_emap(n_vox);
        fd_filter.Apply(diff_op,d_emap.data(),emap.data(),nn,dd);
        for (int idx = 0; idx<n_vox; ++idx) {
            dst[idx] = std::imag(d_emap[idx]/emap[idx]);
        }
    } else if (diff_op==DifferentialOperator::GradientXX||
        diff_op==DifferentialOperator::GradientYY||
        diff_op==DifferentialOperator::GradientZZ) {
        // ...second order
        DifferentialOperator diff_op1 = static_cast<DifferentialOperator>(static_cast<int>(diff_op)-static_cast<int>(DifferentialOperator::GradientXX));
        std::vector<std::complex<double> > d_emap(n_vox);
        std::vector<std::complex<double> > dd_emap(n_vox);
        fd_filter.Apply(diff_op1,d_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(diff_op,dd_emap.data(),emap.data(),nn,dd);
        for (int idx = 0; idx<n_vox; ++idx) {
            dst[idx] = std::imag(d_emap[idx]*d_emap[idx]/emap[idx]/emap[idx] + dd_emap[idx]/emap[idx]);
        }
    } else if (diff_op==DifferentialOperator::Laplacian) {
        // ...laplacian
        std::vector<std::complex<double> > dx_emap(n_vox);
        std::vector<std::complex<double> > dy_emap(n_vox);
        std::vector<std::complex<double> > dz_emap(n_vox);
        std::vector<std::complex<double> > dd_emap(n_vox);
        fd_filter.Apply(DifferentialOperator::GradientX,dx_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(DifferentialOperator::GradientY,dy_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(DifferentialOperator::GradientZ,dz_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(DifferentialOperator::Laplacian,dd_emap.data(),emap.data(),nn,dd);
        for (int idx = 0; idx<n_vox; ++idx) {
            dst[idx] = std::imag((dx_emap[idx]*dx_emap[idx]+dy_emap[idx]*dy_emap[idx]+dz_emap[idx]*dz_emap[idx])/emap[idx]/emap[idx] + dd_emap[idx]/emap[idx]);
        }
    }
    return EPTlibError::Success;
}


/*
Copyright (c) 2020 Inverse Palindrome
ProceduralX - ECS/SteeringBehaviors.cpp
https://inversepalindrome.com/
*/

#include "ECS/SteeringBehaviors.hpp"
#include "App/Random.hpp"
#include "App/Constants.hpp"

#include <boost/math/constants/constants.hpp>

#include <cmath>

b2Vec2 ECS::SteeringBehaviors::move(const b2Vec2& bodyPosition, Direction direction, const b2Vec2& bodyVelocity, float acceleration, float maxSpeed, float mass)
{
    switch (direction)
    {
    case Direction::Up:
        return { 0.f, mass * (b2Min(bodyVelocity.y + acceleration, maxSpeed) - bodyVelocity.y) };
    case Direction::Down:
        return { 0.f, mass * (b2Max(bodyVelocity.y - acceleration, -maxSpeed) - bodyVelocity.y) };
    case Direction::Right:
        return { mass * (b2Min(bodyVelocity.x + acceleration, maxSpeed) - bodyVelocity.x), 0.f };
    case Direction::Left:
        return { mass * (b2Max(bodyVelocity.x - acceleration, -maxSpeed) - bodyVelocity.x), 0.f };
    default:
        return { 0.f, 0.f };
    }
}

b2Vec2 ECS::SteeringBehaviors::seek(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, float maxSpeed)
{
    return desiredVelocity(bodyPosition, targetPosition, maxSpeed) - bodyVelocity;
}

b2Vec2 ECS::SteeringBehaviors::flee(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, float maxSpeed)
{
    return desiredVelocity(targetPosition, bodyPosition, maxSpeed) - bodyVelocity;
}

b2Vec2 ECS::SteeringBehaviors::pursue(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, const b2Vec2& targetVelocity, float maxSpeed)
{
    auto predictionFrames = (targetPosition - bodyPosition).Length() / maxSpeed;

    return seek(bodyPosition, targetPosition + predictionFrames * targetVelocity, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::evade(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, const b2Vec2& targetVelocity, float maxSpeed)
{
    auto predictionFrames = (targetPosition - bodyPosition).Length() / maxSpeed;

    return flee(bodyPosition, targetPosition + predictionFrames * targetVelocity, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::arrive(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, float slowRadius, float maxSpeed)
{
    if (auto radius = (targetPosition - bodyPosition).Length(); radius < slowRadius)
    {
        return seek(bodyPosition, targetPosition, bodyVelocity, maxSpeed * radius / slowRadius);
    }

    return seek(bodyPosition, targetPosition, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::wander(const b2Vec2& bodyPosition, const b2Vec2& bodyVelocity, float wanderDistance, float wanderRadius, float wanderRate, float& wanderAngle, float maxSpeed)
{
    auto wanderCenter = bodyVelocity;
    wanderCenter.Normalize();
    wanderCenter *= wanderDistance;
    wanderCenter += { wanderRadius* std::cos(wanderAngle), wanderRadius* std::sin(wanderAngle) };

    wanderAngle += App::Random::get(-1.f, 1.f) * wanderRate;

    return seek(bodyPosition, bodyPosition + wanderCenter, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::orbit(const b2Vec2& satellitePosition, const b2Vec2& primaryPosition, const b2Vec2& bodyVelocity, float maxSpeed)
{
    auto radius = primaryPosition - satellitePosition;

    auto steeringForce = radius.Skew();
    steeringForce.Normalize();
    steeringForce *= maxSpeed;

    return steeringForce - bodyVelocity;
}

b2Vec2 ECS::SteeringBehaviors::alignForce(const b2Vec2& agentPosition, const std::vector<b2Vec2>& neighborVelocities, float alignmentForce)
{
    b2Vec2 steeringForce(0.f, 0.f);

    for (const auto& neighborVelocity : neighborVelocities)
    {
        steeringForce += neighborVelocity;
    }

    if (!neighborVelocities.empty())
    {
        steeringForce *= 1.f / neighborVelocities.size();
        steeringForce.Normalize();
        steeringForce *= alignmentForce;
    }

    return steeringForce;
}

b2Vec2 ECS::SteeringBehaviors::cohesionForce(const b2Vec2& agentPosition, const std::vector<b2Vec2>& neighborPositions, float cohesionForce)
{
    b2Vec2 steeringForce(0.f, 0.f);

    for (const auto& neighborPosition : neighborPositions)
    {
        steeringForce += neighborPosition;
    }

    if (!neighborPositions.empty())
    {
        steeringForce *= 1.f / neighborPositions.size();
        steeringForce -= agentPosition;
        steeringForce.Normalize();
        steeringForce *= cohesionForce;
    }

    return steeringForce;
}

b2Vec2 ECS::SteeringBehaviors::separateForce(const b2Vec2& agentPosition, const std::vector<b2Vec2>& neighborPositions, float separationForce)
{
    b2Vec2 steeringForce(0.f, 0.f);

    for (const auto& neighborPosition : neighborPositions)
    {
        steeringForce += neighborPosition - agentPosition;
    }

    if (!neighborPositions.empty())
    {
        steeringForce *= -1.f / neighborPositions.size();
        steeringForce.Normalize();
        steeringForce *= separationForce;
    }

    return steeringForce;
}

b2Vec2 ECS::SteeringBehaviors::desiredVelocity(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, float maxSpeed)
{
    auto desiredVelocity = targetPosition - bodyPosition;
    desiredVelocity.Normalize();
    desiredVelocity *= maxSpeed;

    return desiredVelocity;
}

float ECS::SteeringBehaviors::face(float desiredAngle, float bodyAngle, float bodyAngularVelocity, float bodyInertia)
{
    auto nextAngle = bodyAngle + bodyAngularVelocity / App::FRAMES_PER_SECOND;
    auto totalRotation = std::remainderf(desiredAngle - nextAngle, 2 * boost::math::constants::pi<float>());

    return bodyInertia * totalRotation * App::FRAMES_PER_SECOND;
}

float ECS::SteeringBehaviors::face(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, float bodyAngle, float bodyAngularVelocity, float bodyInertia)
{
    auto desiredAngle = std::atan2f(targetPosition.y - bodyPosition.y, targetPosition.x - bodyPosition.x);

    return face(desiredAngle, bodyAngle, bodyAngularVelocity, bodyInertia);
}


#include <random>
#include "Eigen/Dense"
#define _USE_MATH_DEFINES
#include <math.h>

#include "scalar-typedef.hpp"

#include "shared-probability-distributions.hpp"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

using namespace Eigen;

// Explicit template instantiation(s)
template double lpdf_imulti_normal(const Eigen::Matrix<double,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<double,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<double,Eigen::Dynamic,1> &sdVec);

template double lpdf_imulti_lognormal(const Eigen::Matrix<double,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<double,Eigen::Dynamic,1> &meanVec,
                                      const Eigen::Matrix<double,Eigen::Dynamic,1> &sdVec,
                                      unsigned int & ifail);

#ifdef USE_DCO_TYPES
template gt1s_scalar lpdf_imulti_normal(const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &sdVec);
template ga1s_scalar lpdf_imulti_normal(const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &sdVec);
template gt2s_ga1s_scalar lpdf_imulti_normal(const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &sdVec);

template gt1s_scalar lpdf_imulti_lognormal(const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &x,
                                           const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &meanVec,
                                           const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &sdVec,
                                           unsigned int & ifail);
template ga1s_scalar lpdf_imulti_lognormal(const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                                           const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &sdVec,
                                           unsigned int & ifail);
template gt2s_ga1s_scalar lpdf_imulti_lognormal(const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &x,
                                                const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                                                const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &sdVec,
                                                unsigned int & ifail);
#endif

template<typename SCALAR_T>
SCALAR_T lpdf_univariate_normal(const SCALAR_T & x, const SCALAR_T & mu, const SCALAR_T & sd)
{
  const SCALAR_T logSqrt2Pi = 0.5*std::log(2*M_PI);
  const SCALAR_T z = x - mu;
  return - logSqrt2Pi - log(sd) - 0.5 * (z * z) / (sd * sd);
}

template<typename SCALAR_T>
SCALAR_T lpdf_univariate_lognormal(const SCALAR_T & x, const SCALAR_T & mu, const SCALAR_T & sd)
{
  const SCALAR_T logSqrt2Pi = 0.5*std::log(2*M_PI);
  const SCALAR_T z = log(x) - mu;
  return -log(x) - logSqrt2Pi - log(sd) - 0.5 * (z * z) / (sd * sd);
}

template<typename SCALAR_T>
SCALAR_T lpdf_imulti_lognormal(const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &x,
                               const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &meanVec,
                               const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &sdVec,
                               unsigned int & ifail)
{
  SCALAR_T lp = 0;
  for (int i=0; i < x.size(); ++i){
    if (x[i] < 0){
      ifail = 7;
      return - std::numeric_limits<SCALAR_T>::infinity();;
    }
    SCALAR_T mu = log( meanVec[i] / sqrt(1 + pow( sdVec[i] / meanVec[i] , 2.0) ) );
    SCALAR_T log_sd = sqrt( log( 1 + pow( sdVec[i] / meanVec[i] , 2.0)  ) );
    lp += lpdf_univariate_lognormal(x[i], mu, log_sd);
  }
  return lp;
}

template<typename SCALAR_T>
SCALAR_T lpdf_imulti_normal(const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &sdVec)
{
  SCALAR_T lp = 0;
  for (int i=0; i < x.size(); ++i){
    lp += lpdf_univariate_normal(x[i], meanVec[i], sdVec[i]);
  }
  return lp;
}

// downloaded from https://stackoverflow.com/questions/41538095/evaluate-multivariate-normal-gaussian-density-in-c
double lpdf_multi_normal(const Eigen::VectorXd &x, const Eigen::VectorXd &meanVec,
                         const Eigen::MatrixXd &covMat)
{
    // avoid magic numbers in your code. Compilers will be able to compute this at compile time:
    const double logSqrt2Pi = 0.5*std::log(2*M_PI);
    typedef Eigen::LLT<Eigen::MatrixXd> Chol;
    Chol chol(covMat);
    // Handle non positive definite covariance somehow:
    if(chol.info()!=Eigen::Success) throw "decomposition failed!";
    const Chol::Traits::MatrixL& L = chol.matrixL();
    Eigen::VectorXd centredVec = (x - meanVec);
    // solve  L y = (x - mu) for y
    double quadform = (L.solve(centredVec)).squaredNorm();
    // log(|Sigma|^{-1/2} = - 0.5 log( |Sigma| )
    return -x.rows()*logSqrt2Pi - 0.5*quadform - 0.5 * covMat.colPivHouseholderQr().logAbsDeterminant() ;
}

// Note - this is not an optimal implementation if generating many samples from dist with the same covariance matrix
Matrix<double,Dynamic,1> rand_multi_normal(const Matrix<double,Dynamic,1> mean,
                                           Matrix<double,Dynamic,Dynamic> cov,
                                           std::mt19937 & gen)
{
  std::normal_distribution<> std_normal_rng;
  Eigen::LLT<Eigen::MatrixXd> llt_of_cov = cov.llt();
  Eigen::VectorXd z(cov.cols());
  for (int i = 0; i < cov.cols(); ++i){
    z(i) = std_normal_rng(gen);
  }
  MatrixXd L = llt_of_cov.matrixL();
  return mean + L  * z;
}

// Adapted from https://stackoverflow.com/questions/41538095/evaluate-multivariate-normal-gaussian-density-in-c
#undef MYPREC_VERSION
#ifdef MYPREC_VERSION
double lpdf_multi_normal(const Eigen::VectorXd &x, const Eigen::VectorXd &meanVec, const Eigen::MatrixXd &precMat)
{
    // avoid magic numbers in your code. Compilers will be able to compute this at compile time:
    // const double logSqrt2Pi = 0.5*std::log(2*M_PI);
    typedef Eigen::LLT<Eigen::MatrixXd> llt_of_prec;
    llt_of_prec chol(precMat);
    // Handle non positive definite covariance somehow:
    if(chol.info()!=Eigen::Success) throw "decomposition failed!";
    const llt_of_prec::Traits::MatrixL& L = chol.matrixL();
    double quadform = (L * (x - meanVec)).squaredNorm();
    // return std::exp(-x.rows()*logSqrt2Pi - 0.5*quadform) / L.determinant();
    return - 0.5*quadform +  0.5 * precMat.colPivHouseholderQr().logAbsDeterminant();
}
#endif

#undef STACKOVERFLOW_VERSION
#ifdef STACKOVERFLOW_VERSION
// From https://stackoverflow.com/questions/6142576/sample-from-multivariate-normal-gaussian-distribution-in-c
// October 2019
struct normal_random_variable
{
    normal_random_variable(Eigen::MatrixXd const& covar)
        : normal_random_variable(Eigen::VectorXd::Zero(covar.rows()), covar)
    {}

    normal_random_variable(Eigen::VectorXd const& mean, Eigen::MatrixXd const& covar)
        : mean(mean)
    {
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
        transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
    }

    Eigen::VectorXd mean;
    Eigen::MatrixXd transform;

    Eigen::VectorXd operator()() const
    {
        static std::mt19937 gen{ std::random_device{}() };
        static std::normal_distribution<> dist;

        return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](auto x) { return dist(gen); });
    }
};
#endif

#undef MY_COV_VERSION
#ifdef MY_COV_VERSION
// Note - this is less efficient than stackoverflow version if generating many samples from dist with the same covariance matrix
Matrix<double,Dynamic,1> rand_multi_normal(const Matrix<double,Dynamic,1> mean,
                                           Matrix<double,Dynamic,Dynamic> covar,
                                           std::mt19937 & gen,
                                           std::normal_distribution<> & dist)
{
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
  Matrix<double,Dynamic,Dynamic> transform m= eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
  return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](double x) { return dist(gen); });
}
#endif

#undef STAN_PREC_VERSION
#ifdef STAN_PREC_VERSION
// From https://mc-stan.org/math/d4/d28/multi__normal__prec__rng_8hpp_source.html
// October 2019

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/err/check_pos_definite.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
  namespace math {

    template <typename T_loc, class RNG>
    inline typename StdVectorBuilder<true, Eigen::VectorXd, T_loc>::type
    multi_normal_prec_rng(const T_loc &mu, const Eigen::MatrixXd &S, RNG &rng) {
      using boost::normal_distribution;
      using boost::variate_generator;

      static const char *function = "multi_normal_prec_rng";

      check_positive(function, "Precision matrix rows", S.rows());
      check_finite(function, "Precision matrix", S);
      check_symmetric(function, "Precision matrix", S);

      Eigen::LLT<Eigen::MatrixXd> llt_of_S = S.llt();
      check_pos_definite(function, "precision matrix argument", llt_of_S);

      vector_seq_view<T_loc> mu_vec(mu);
      check_positive(function, "number of location parameter vectors",
                     mu_vec.size());
      size_t size_mu = mu_vec[0].size();

      size_t N = mu_vec.size();

      for (size_t i = 1; i < N; i++) {
        int size_mu_new = mu_vec[i].size();
        check_size_match(function,
                         "Size of one of the vectors of "
                         "the location variable",
                         size_mu_new,
                         "Size of another vector of the "
                         "location variable",
                         size_mu);
      }

      for (size_t i = 0; i < N; i++) {
        check_finite(function, "Location parameter", mu_vec[i]);
      }

      check_size_match(function, "Rows of location parameter", size_mu, "Rows of S",
                       S.rows());

      StdVectorBuilder<true, Eigen::VectorXd, T_loc> output(N);

      variate_generator<RNG &, normal_distribution<>> std_normal_rng(
                                                                     rng, normal_distribution<>(0, 1));

      for (size_t n = 0; n < N; ++n) {
        Eigen::VectorXd z(S.cols());
        for (int i = 0; i < S.cols(); i++)
          z(i) = std_normal_rng();

        output[n] = Eigen::VectorXd(mu_vec[n]) + llt_of_S.matrixU().solve(z);
      }

      return output.data();
    }

  }  // namespace math
}  // namespace stan
#endif


/*
 * Tsai algorithm 1989, initially ported to Eigen from VISP and then manually modified for efficiency, clearness and more Eigenability
 *
 * Copyright Scuola Superiore Sant'Anna (2016)
 * Emanuele Ruffaldi, e.ruffaldi@sssup.it
 *
 * TODO: exclude collinear pairs
 * TODO: compute residuals for providing a measure of the error
 *
 * Structure optimization (double the memory but faster)
 *    A1,A2,b1,b2,tmp
 *    loop all pairs
 *      exclude bad pairs
 *      write A1 and A2
 *      write b1 and partial b2 the part that doesn't require Rcg
 *    solve Rcg using A1,b1
 *    update b2 using eRcg*cijo.matrix().template block<3,1>(0,3) (so we need to store cijo translation part into tmp)
 *    solve translation for A2,b2
 */
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "tsai.hpp"

/// extracts the translation part
template <class T>
static Vector3<T> gettx(const Matrix4<T> & x)
{
  return x.template block<3,1>(0,3);
}

/// extracts the translation part
template <class T>
static Vector3<T> gettx(const Eigen::Transform<T,3,Eigen::Affine> & x)
{
  return x.matrix().template block<3,1>(0,3);
}

/// extracts rotation
template <class T>
static Matrix3<T> getrot(const Eigen::Transform<T,3,Eigen::Affine> & x)
{
  return x.matrix().template block<3,3>(0,0);
}

/// extracts rotation
template <class T>
static Matrix3<T> getrot(const Matrix4<T> & x)
{
  return x.template block<3,3>(0,0);
}

/// extracts quat and transform from matrix 4x4
template <class T>
static void extract(const Matrix4<T> & x,Eigen::Quaternion<T> & q, Vector3<T> & t)
{
  q = x.template block<3,3>(0,0);
  t = x.template block<3,1>(0,3);
}

/// extract rotation
template <class T>
static void extract(const Matrix4<T> & x,Eigen::Quaternion<T> & q)
{
  q = x.template block<3,3>(0,0);
}

/// makes affine3f from matrix using extraction
template <class T>
static void extract(const Matrix4<T> & x,Eigen::Transform<T,3,Eigen::Affine> & a)
{
  auto q = x.template block<3,3>(0,0);
  auto t = x.template block<3,1>(0,3);
  a.fromPositionOrientationScale(t,q,Vector3<T>(1,1,1));
}

template <class T>
static inline T sinc(T sx,T x)
{
  return fabs(x) < 1e-8 ? 1 : sx/x;
}

template <class T>
static inline T sinc(T x)
{
  return fabs(x) < 1e-8 ? 1 : sin(x)/x;
}

// modified tsai is: 2 sin(theta/2) axis
// BUT quaternion real part is:  sin(theta/2) * axis
// equation 9
// THIS IS FROM VISP
template <class T>
Vector3<T> quat2paratsai(const Eigen::Quaternion<T> & q)
{
  //Eigen::AngleAxisf aa(q);
  //return 2*sin(aa.angle()/2)*aa.axis();
  //return 2*Vector3<T>(q.x(),q.y(),q.z());
  const double minimum = 0.0001;
  Matrix3<T> R(q);
  Vector3<T> r;

  double s = (R(1,0)-R(0,1))*(R(1,0)-R(0,1))
    + (R(2,0)-R(0,2))*(R(2,0)-R(0,2))
    + (R(2,1)-R(1,2))*(R(2,1)-R(1,2));
   s = sqrt(s)/2.0;
   double c = (R(0,0)+R(1,1)+R(2,2)-1.0)/2.0;
   double theta=atan2(s,c);  /* theta in [0, PI] since s > 0 */

  // General case when theta != pi. If theta=pi, c=-1
  if ( (1+c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
  {
    double si = sinc(s,theta);

    r[0] = (R(2,1)-R(1,2))/(2*si);
    r[1] = (R(0,2)-R(2,0))/(2*si);
    r[2] = (R(1,0)-R(0,1))/(2*si);
  }
  else /* theta near PI */
  {
    if ( (R(0,0)-c) < std::numeric_limits<double>::epsilon() )
      r[0] = 0.;
    else
      r[0] = theta*(sqrt((R(0,0)-c)/(1-c)));
    if ((R(2,1)-R(1,2)) < 0) r[0] = -r[0];

    if ( (R(1,1)-c) < std::numeric_limits<double>::epsilon() )
      r[1] = 0.;
    else
      r[1] = theta*(sqrt((R(1,1)-c)/(1-c)));

    if ((R(0,2)-R(2,0)) < 0) r[1] = -r[1];

    if ( (R(2,2)-c) < std::numeric_limits<double>::epsilon() )
      r[2] = 0.;
    else
      r[2] = theta*(sqrt((R(2,2)-c)/(1-c)));

    if ((R(1,0)-R(0,1)) < 0) r[2] = -r[2];
  }
  return r;

}

template <class T>
T paratsaiprime2theta(const Vector3<T> & x)
{
  return 2*atan(x.norm()); // eq. 13
}

// port and improve: https://github.com/thomas-moulard/visp-deb/blob/master/src/camera/calibration/vpCalibrationTools.cpp
template<class T>
T calibrationTsaiT(const std::vector<Matrix4<T> > & cMo,const std::vector<Matrix4<T> > & rMe, Matrix4<T> &eMc, bool computeResidual)
{
  assert(cMo.size() == rMe.size() && !cMo.empty() && "not empty calibration matrices");
  const unsigned int nbPose = cMo.size();
  Vector3<T> x; // tsaipara result
  Eigen::Matrix<T,Eigen::Dynamic,3> A((nbPose*(nbPose-1)/2)*3,3); // reused between the two loops over all pairs
  Eigen::Matrix<T,Eigen::Dynamic,1> B(A.rows(),1);
  {
    unsigned int k = 0 ;
    // for all couples ij
    for (unsigned int i=0 ; i < nbPose ; i++)
    {
      Eigen::Quaternion<T> eiR,ioR;
      extract(rMe[i],eiR);
      extract(cMo[i],ioR);

      for (unsigned int j=i+1; j < nbPose ; j++, k+= 3)
      {
          Eigen::Quaternion<T> ejR,joR;
          extract(rMe[j],ejR);
          extract(cMo[j],joR);

          Eigen::Quaternion<T> rRgij = ejR.conjugate() * eiR;
          Eigen::Quaternion<T>  cRijo = joR * ioR.conjugate();

          // CODE: theta = sqrt(|rotationalpart of rRgij|) then multiply rotation part by sinc(theta/2)
          // going from norm theta to scaled by sinc

          // rotation axis (eq 11a,11b,11c)
          Vector3<T>  rPgij = quat2paratsai<T>(rRgij);
          Vector3<T>  cPijo = quat2paratsai<T>(cRijo);

          // TODO: in paper we can remove pair if not good

          // skewtsai(Pgij+Pcij) Pcg' = Pcij - Pgij  equation 12
          A.template block<3,3>(k,0) = skewtsai(Vector3<T>(rPgij + cPijo)) ;
          B.template block<3,1>(k,0) = cPijo - rPgij;

#if 0
          if(k == 0)
          {
            std::cout << "rPeij0 R\n" << Matrix3<T>(rRgij) << std::endl;
            std::cout << "rPeij0\n" << rPgij.transpose() << std::endl;
            std::cout << "rPeij0\n" << Eigen::AngleAxisf(rRgij).angle() << "|" << Eigen::AngleAxisf(rRgij).axis() << std::endl;
            std::cout << "cijPo\n" << cPijo.transpose() << std::endl;

            std::cout << "As0\n " << A.template block<3,3>(0,0) << " \n b0" << B.template block<3,1>(0,0).transpose() << std::  endl;
          }
#endif
      }
    }

    // the output is in the MODIFIED form
    x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

    // convert output to PARAMETRIC eq. 14
    // Pcg' = 1/(4-|Pcg|^2) Pcg
    // Pcg  = 2 Pcg' /(1+|Pcg|^2)
    x = paratsaiprime2paratsai(x);

    // verification of the quality of the vector
    const double theta = paratsai2theta(x);

    if (std::fabs(theta) < std::numeric_limits<double>::epsilon())
      x.setZero();
    // x *= theta/(2*sin(theta/2));     // paratsai => para NOT NEEDED we work in paratsai
  }

  // Building of the rotation matrix eRc using eq. 10
  Matrix3<T>  eRcg = paratsai2rot(x);

  {
    // Building of the system for the translation estimation
    // for all couples ij
    unsigned int k = 0 ;
    for (unsigned int i = 0 ; i < nbPose ; i++)
    {
      /*
      Eigen::Affine3f eiA,ioA;
      extract(rMe[i],eiA);
      extract(cMo[i],ioA);
      */

      Vector3<T>  eiT = gettx(rMe[i]);
      Vector3<T>  ioT = gettx(cMo[i]);
      Matrix3<T>  eiR = getrot(rMe[i]);

      for (unsigned int j = i+1 ; j < nbPose ; j++, k+= 3)
      {
          /*
          Eigen::Affine3f ejA,joA;
          extract(rMe[j],rejA);
          extract(cMo[j],joA);
          Eigen::Affine3f jiA = ejA.inverse() * eiA; // gij of paper
          */
          Vector3<T>  ejT = gettx(rMe[j]);
          Vector3<T> joT = gettx(cMo[j]);
          Matrix3<T> ejR = getrot(rMe[j]);
          Matrix3<T> jiR = ejR.transpose()*eiR;

          // TODO: in paper we can remove pair if not good COLLINEARITY ISSUE

          // equation 15: (Rgij-I) T = Rcg Tcij - Tgij
          A.template block<3,3>(k,0) = jiR-Matrix3<T>::Identity();
          B.template block<3,1>(k,0) = eRcg*joT - jiR*eRcg*ioT + ejR.transpose()*(ejT-eiT);
      }
    }

    eMc.setIdentity();
    eMc.template block<3,3>(0,0) = eRcg;
    eMc.template block<3,1>(0,3) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

    // A (n*3,3) x(3,1) - B(n*3,1)
    return computeResidual ? (A*eMc.template block<3,1>(0,3)-B).squaredNorm()/A.rows() : 0; // this is the variance of result
 }
}

float calibrationTsai(const std::vector<Eigen::Matrix4f> & cMo,
                      const std::vector<Eigen::Matrix4f> & rMe,
                      Eigen::Matrix4f &eMc,
                      bool computeResidual)
{
    return calibrationTsaiT<float>(cMo,rMe,eMc,computeResidual);
}

double calibrationTsai(const std::vector<Eigen::Matrix4d> & cMo,
                       const std::vector<Eigen::Matrix4d> & rMe,
                       Eigen::Matrix4d &eMc,bool computeResidual)
{
    return calibrationTsaiT<double>(cMo,rMe,eMc,computeResidual);
}


#ifndef PLAN_BY_CAPTURE_POINT
#define PLAN_BY_CAPTURE_POINT
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <deque>
#include <Eigen/Geometry>

#ifdef PLANNER_DEBUG
static constexpr bool planner_debug = true;
#else
static constexpr bool planner_debug = false;
#endif

template <class T>
static void debugPrint(T &&s, const std::string &label)
{
    if (planner_debug)
        std::cout << "Debug planner:" << label << "::" << std::forward<T>(s) << std::endl;
}

static constexpr double dt = 4.0 / 1000.0;   // sampling period (s)
static constexpr double zh = 0.2;            // height of CoM (m)
static constexpr double g = 9.8;             // gravity (m/s^2)
static constexpr double MAX_X_STRIDE = 0.30; //(m)
static constexpr double MAX_Y_STRIDE = 0.08; //(m)
static constexpr double MIN_Y_STRIDE = 0.03; //(m)

struct FootPrint
{
    double time_;
    double x_;                  //(m)ロボットのローカル座標での足配置位置
    double y_;                  //(m)ロボットのローカル座標での足配置位置
    bool support_foot_is_right; //支持脚がどちらか
};

/**
 * @brief capture pointによる歩行パターン生成方法
 *
 */
void footStepPlannerCapturePoint()
{
    double x = 0, y = 0, xinit = 0, yinit = 0;     //(m) CoMのワールド座標 {CoM = Center of Mass}
    double xd = 0, yd = 0, xdinit = 0, ydinit = 0; // CoMの速度 v(m/s) xdot(t)
    double px = 0.0, py = 0.0;                     //(m)　着地位置のワールド座標 これは実用的にはローカルの方が良いのでは？？
    constexpr double Tc = std::sqrt(zh / g);       //微分方程式の時定数
    constexpr double w = std::sqrt(g / zh);
    const double Tsup = 0.32;
    int_fast64_t step_n = 0;
    const int32_t steps_ = 7;
    const double stride_x = 0.3;
    const double stride_y = 0.1;
    double cp_x_target = 0.0, cp_y_target = 0.0;
    double cp_x_now = 0.0, cp_y_now = 0.0;
    double cp_x_old = 0.0, cp_y_old = 0.0;
    debugPrint(Tsup, "Tsup");
    std::deque<std::deque<double>> result;
    std::ofstream velofs("velo.dat");
    std::vector<FootPrint> footprint_list(100);
    for (double t = 0.0; t < (Tsup * static_cast<double>(steps_ * 2 + 1)); ++step_n)
    {
        //決められた次の一歩を着く地点までの遊脚の移動を行っている時のシミュレーション---------------
        for (double t_tmp = 0.0; t_tmp < Tsup; t_tmp += dt, t += dt)
        {
            // double C = std::cosh(t_tmp / Tc);
            // double S = std::sinh(t_tmp / Tc);
            double ewt = std::exp(w * t_tmp);
            double dT = Tsup - t_tmp; //次のCPまでの時間
            // x = (xinit - px) * C + Tc * xdinit * S + px; // x,xdともにn歩目開始時の状態
            // y = (yinit - py) * C + Tc * ydinit * S + py;
            // xd = (xinit - px) / Tc * S + xdinit * C;
            // yd = (yinit - py) / Tc * S + ydinit * C;
            // cp_x_now = ewt * cp_x_old + (1.0 - ewt)*px; //ここでのcp_targetは1ループ前の話。
            // cp_y_now = ewt * cp_y_old + (1.0 - ewt)*py;
            cp_x_now = ewt * cp_x_old + (1.0 - ewt) * px; // CPの軌道(x)
            cp_y_now = ewt * cp_y_old + (1.0 - ewt) * py; // CPの軌道(y)
            xdinit = xd;
            ydinit = yd;
            xd = -w * (x - cp_x_now);
            yd = -w * (y - cp_y_now);
            x += dt * xdinit;
            y += dt * ydinit;
            // result.push_back({x, y, cp_x_now, cp_y_now});
            result.push_back({x, y, cp_x_now, cp_y_now});
            velofs << t << " " << xd << " " << yd << std::endl;
        }
        xinit = x;
        yinit = y;
        xdinit = xd;
        ydinit = yd;
        //次の一歩の目標位置を計算------------------
        double b = std::exp(w * Tsup);
        std::cout << b << std::endl;
        double sx = 0.0, sy = 0.0;
        if (steps_ < step_n)
        {
            sx = 0.0;
            sy = 0.0;
        }
        else
        {
            sx = stride_x / 2;
            sy = ((step_n % 2) ? -1 : 1) * stride_y ;
        }
        cp_x_old = cp_x_now; //次の一歩の最初のCPの位置
        cp_y_old = cp_y_now;
        cp_x_target = cp_x_old + sx;
        cp_y_target = cp_y_old + sy;
        if (step_n != 0)
        {
            px = 1.0 / (1.0 - b) * cp_x_target - b / (1.0 - b) * cp_x_now;
            py = 1.0 / (1.0 - b) * cp_y_target - b / (1.0 - b) * cp_y_now;
        }
        result.back().push_back(px);
        result.back().push_back(py);
    }
    std::ofstream ofs("position.dat");
    for (auto &itr : result)
    {
        for (auto &item : itr)
        {
            ofs << item << " ";
        }
        ofs << std::endl;
    }
    // return footprint_list;
}

#endif // !PLAN_BY_CAPTURE_POINT


#include "encode_decode.h"
#include <queue>
#include <string>
#include <unordered_map>
#include <vector>
#include <boost/dynamic_bitset.hpp>
#include "char_node.h"
#include "encoding.pb.h"
#include "freq_node.h"
#include "inner_node.h"

namespace encode_decode {

	class Comparator {
	public:
	    bool operator()(const std::shared_ptr<FreqNode>& a,
	    				const std::shared_ptr<FreqNode>& b) {
	        return (a->get_freq() > b->get_freq());
	    }
	};

	std::unordered_map<char, int> count_freq(const std::string& s) {
		std::unordered_map<char, int> counter;
		for (auto it = s.begin(); it != s.end(); ++it) {
			std::unordered_map<char,int>::iterator res = counter.find(*it);
			if (res == counter.end()) {
				counter.emplace(*it, 1);
			} else {
				res->second = res->second + 1;
			}
		}
		return counter;
	}

	std::shared_ptr<FreqNode> create_optimal_hufftree(const std::string& s) {

		std::unordered_map<char, int>&& counter = count_freq(s);

		// We use shared_ptr because the priority queue only provides
		// access to the top element as a const reference.
		// Thus, it is impossible to move the ownership of the node or its
		// child nodes.
		// Alternatively, we could copy the node out of the queue,
		// but as we combine nodes into a tree throughout the algorithm,
		// this would entail copying the entire tree,
		// making the algorithm O(n^2) rather than O(n lg n).
		std::priority_queue<std::shared_ptr<FreqNode>,
							std::vector<std::shared_ptr<FreqNode>>,
							Comparator> minheap;
		for (auto it = counter.begin(); it != counter.end(); ++it) {
			// Allocate each character node on the heap
			minheap.push(std::make_shared<CharNode>(
				CharNode(it->first, it->second)));
		}

		int numchars = minheap.size();
		for (int i = 1; i < numchars; ++i) {
			std::shared_ptr<FreqNode> left(minheap.top());
			minheap.pop();
			std::shared_ptr<FreqNode> right(minheap.top());
			minheap.pop();
			minheap.push(std::shared_ptr<InnerNode>{
				new InnerNode(left, right)});
		}
		std::shared_ptr<FreqNode> root(minheap.top());
		minheap.pop();
		return root;
	}

	std::unordered_map<char, boost::dynamic_bitset<>> encode_tree(const FreqNode& root) {
		std::unordered_map<char, boost::dynamic_bitset<>> encodings;
		boost::dynamic_bitset<> bits;
		root.encode_node(bits, encodings);
		return encodings;
	}

	void concat_bitsets(boost::dynamic_bitset<>& bits1,
						const boost::dynamic_bitset<>& bits2) {
		size_t orig_size = bits1.size();
		bits1.resize(orig_size + bits2.size());
		for (size_t i = 0; i < bits2.size(); i++) {
			bits1[orig_size + i] = bits2[i];
		}
	}

	Status encode_string(const std::unordered_map<char, boost::dynamic_bitset<>>& encodings,
					   	 const std::string& s,
					   	 boost::dynamic_bitset<>& bits) {
		for (auto it = s.begin(); it != s.end(); ++it) {
			std::unordered_map<char,boost::dynamic_bitset<>>::const_iterator res = encodings.find(*it);
			if (res == encodings.end()) {
				return Status::kInvalid;
			}
			concat_bitsets(bits, res->second);
		}
		return Status::kOk;
	}

	Status from_proto(const FreqNodeProto& proto_node,
	 			      std::shared_ptr<FreqNode>& node) {
		if (proto_node.type() == FreqNodeProto_Type_CHAR) {
			// TODO: convert to char
			google::protobuf::int32 int_c = proto_node.c();
			char c = static_cast<char>(int_c);
			int freq = proto_node.freq();
			node = std::make_shared<CharNode>(CharNode(c, freq));
			return Status::kOk;
		} else if (proto_node.type() == FreqNodeProto_Type_INNER) {
			std::shared_ptr<FreqNode> lc;
			std::shared_ptr<FreqNode> rc;
			if (from_proto(proto_node.lc(), lc) == Status::kOk &&
				from_proto(proto_node.rc(), rc) == Status::kOk) {
				node = std::make_shared<InnerNode>(InnerNode(lc, rc));
				return Status::kOk;
			} else {
				return Status::kInvalid;
			}
		} else {
			return Status::kInvalid;
		}
	}

	Status decode_bits(const FreqNode& root,
					   const boost::dynamic_bitset<>& encoded_bits,
					   std::string& decoding) {
		size_t bit_index(0);
		while (bit_index < encoded_bits.size()) {
			char c;
			if (root.find_next_char(encoded_bits, bit_index, c) == Status::kOk) {
				decoding.push_back(c);
			} else {
				// An invalid status occurs if there are not enough bits
				// in the bitset to reach a char node.
				return Status::kInvalid;
			}
		}
		return Status::kOk;
	}
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Utils/Optimizer/GradientBased/GradientBasedCheck.h"
#include "Utils/Settings.h"
#include <Eigen/Core>

namespace Scine {
namespace Utils {

bool GradientBasedCheck::checkConvergence(const Eigen::VectorXd& parameter, double value, const Eigen::VectorXd& gradient) {
  if (_oldParams.size() != parameter.size()) {
    _oldParams = Eigen::VectorXd::Zero(parameter.size());
  }
  // Generate temporaries
  Eigen::VectorXd deltaParam = (parameter - _oldParams).eval();
  double deltaV = value - _oldValue;
  // Rotate stored old data
  _oldParams = parameter;
  _oldValue = value;
  // Check
  unsigned int converged = 0;
  if (gradient.cwiseAbs().maxCoeff() < gradMaxCoeff) {
    converged++;
  }
  if (deltaParam.cwiseAbs().maxCoeff() < stepMaxCoeff) {
    converged++;
  }
  if (sqrt(gradient.squaredNorm() / gradient.size()) < gradRMS) {
    converged++;
  }
  if (sqrt(deltaParam.squaredNorm() / deltaParam.size()) < stepRMS) {
    converged++;
  }
  return ((fabs(deltaV) < deltaValue) && (converged >= requirement));
}

bool GradientBasedCheck::checkMaxIterations(unsigned int currentIteration) const {
  return currentIteration >= maxIter;
}

void GradientBasedCheck::setParametersAndValue(const Eigen::VectorXd& parameter, double value) {
  _oldParams = parameter;
  _oldValue = value;
}

void GradientBasedCheck::addSettingsDescriptors(UniversalSettings::DescriptorCollection& collection) const {
  UniversalSettings::DoubleDescriptor step_max_coeff(
      "Convergence threshold for step vector's maximum absolute element.");
  step_max_coeff.setMinimum(0.0);
  step_max_coeff.setDefaultValue(stepMaxCoeff);
  collection.push_back(GradientBasedCheck::gconvStepMaxCoeffKey, step_max_coeff);

  UniversalSettings::DoubleDescriptor step_RMS("Convergence threshold for step vector's RMS.");
  step_RMS.setMinimum(0.0);
  step_RMS.setDefaultValue(stepRMS);
  collection.push_back(GradientBasedCheck::gconvStepRMSKey, step_RMS);

  UniversalSettings::DoubleDescriptor grad_max_coeff(
      "Convergence threshold for gradient vector's maximum absolute element.");
  grad_max_coeff.setMinimum(0.0);
  grad_max_coeff.setDefaultValue(gradMaxCoeff);
  collection.push_back(GradientBasedCheck::gconvGradMaxCoeffKey, grad_max_coeff);

  UniversalSettings::DoubleDescriptor grad_RMS("Convergence threshold for gradient vector's RMS.");
  grad_RMS.setMinimum(0.0);
  grad_RMS.setDefaultValue(gradRMS);
  collection.push_back(GradientBasedCheck::gconvGradRMSKey, grad_RMS);

  UniversalSettings::DoubleDescriptor delta_value(
      "Convergence threshold for the absolute difference in the value between the current and the last step.");
  delta_value.setMinimum(0.0);
  delta_value.setDefaultValue(deltaValue);
  collection.push_back(GradientBasedCheck::gconvDeltaValueKey, delta_value);

  UniversalSettings::IntDescriptor max_iter("The maximum number of iterations.");
  max_iter.setMinimum(0.0);
  max_iter.setDefaultValue(maxIter);
  collection.push_back(GradientBasedCheck::gconvMaxIterKey, max_iter);

  UniversalSettings::IntDescriptor requirements(
      "The number of threasholds besides the value one that need to converge for overall convergence.");
  requirements.setDefaultValue(requirement);
  requirements.setMaximum(4);
  requirements.setMinimum(0);
  collection.push_back(GradientBasedCheck::gconvRequirementKey, requirements);
}

void GradientBasedCheck::applySettings(const Settings& settings) {
  stepMaxCoeff = settings.getDouble(GradientBasedCheck::gconvStepMaxCoeffKey);
  stepRMS = settings.getDouble(GradientBasedCheck::gconvStepRMSKey);
  gradMaxCoeff = settings.getDouble(GradientBasedCheck::gconvGradMaxCoeffKey);
  gradRMS = settings.getDouble(GradientBasedCheck::gconvGradRMSKey);
  deltaValue = settings.getDouble(GradientBasedCheck::gconvDeltaValueKey);
  maxIter = settings.getInt(GradientBasedCheck::gconvMaxIterKey);
  requirement = settings.getInt(GradientBasedCheck::gconvRequirementKey);
}

} // namespace Utils
} // namespace Scine


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * 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 copyright holder 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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.
 *
 * ****************************************************************************
 * Projection matrices to be used with OpenGL.
 * ****************************************************************************
 */

#ifndef CAMERA_PROJECTION_MATRICES_HPP
#define CAMERA_PROJECTION_MATRICES_HPP

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

// http://www.songho.ca/opengl/gl_projectionmatrix.html

namespace camera
{

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> getOrtographicProjection(T l, T r, T b, T t, T n, T f)
{
    Eigen::Matrix<T,4,4> m;

    m << T(2.0)/(r-l) , T(0.0)       , T(0.0)        , -(r+l)/(r-l),
         T(0.0)       , T(2.0)/(t-b) , T(0.0)        , -(t+b)/(t-b),
         T(0.0)       , T(0.0)       , T(-2.0)/(f-n) , -(f+n)/(f-n),
         T(0.0)       , T(0.0)       , T(0.0)        , T(1.0);

    return m;
}

// Camera Axis:
//   X - Right, Y - Up, Z - Back
// Image Origin:
//   Bottom Left
// Caution: Principal point defined with respect to image origin (0,0) at
//          top left of top-left pixel (not center, and in different frame
//          of reference to projection function image)
template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjectionRUBBottomLeft(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    typedef typename CameraModel::Scalar Scalar;

    const Scalar L = +(cam.u0()) * N / -cam.fx();
    const Scalar T = +(cam.v0()) * N / cam.fy();
    const Scalar R = -(cam.width() - cam.u0()) * N / -cam.fx();
    const Scalar B = -(cam.height() - cam.v0()) * N / cam.fy();

    Eigen::Matrix<typename CameraModel::Scalar,4,4> ret;

    ret <<
    Scalar(2.0)*N/(R-L) , Scalar(0.0)         , (R+L)/(R-L) , Scalar(0.0),
    Scalar(0.0)         , Scalar(2.0)*N/(T-B) , (T+B)/(T-B) , Scalar(0.0),
    Scalar(0.0)         , Scalar(0.0)         ,-(F+N)/(F-N) ,-(Scalar(2.0)*F*N)/(F-N),
    Scalar(0.0)         , Scalar(0.0)         ,-Scalar(1.0) , Scalar(0.0);

    return ret;
}

// Camera Axis:
//   X - Right, Y - Down, Z - Forward
// Image Origin:
//   Top Left
// Pricipal point specified with image origin (0,0) at top left of top-left pixel (not center)
template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjectionRDFTopLeft(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    typedef typename CameraModel::Scalar Scalar;

    const Scalar L = -(cam.u0()) * N / cam.fx();
    const Scalar T = -(cam.v0()) * N / cam.fy();
    const Scalar R = +(cam.width() - cam.u0()) * N / cam.fx();
    const Scalar B = +(cam.height() - cam.v0()) * N / cam.fy();

    Eigen::Matrix<typename CameraModel::Scalar,4,4> ret;

    ret <<
    Scalar(2.0)*N/(R-L) , Scalar(0.0)         , (R+L)/(L-R) , Scalar(0.0),
    Scalar(0.0)         , Scalar(2.0)*N/(T-B) , (T+B)/(B-T) , Scalar(0.0),
    Scalar(0.0)         , Scalar(0.0)         , (F+N)/(F-N) ,(Scalar(2.0)*F*N)/(N-F),
    Scalar(0.0)         , Scalar(0.0)         , Scalar(1.0) , Scalar(0.0);

    return ret;
}

// Camera Axis:
//   X - Right, Y - Down, Z - Forward
// Image Origin:
//   Bottom Left
// Pricipal point specified with image origin (0,0) at top left of top-left pixel (not center)
template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjectionRDFBottomLeft(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    typedef typename CameraModel::Scalar Scalar;

    const Scalar L = -(cam.u0()) * N / cam.fx();
    const Scalar T = +(cam.height() - cam.v0()) * N / cam.fy();
    const Scalar R = +(cam.width() - cam.u0()) * N / cam.fx();
    const Scalar B = -(cam.v0()) * N / cam.fy();

    Eigen::Matrix<typename CameraModel::Scalar,4,4> ret;

    ret <<
    Scalar(2.0)*N/(R-L) , Scalar(0.0)         , (R+L)/(L-R) , Scalar(0.0),
    Scalar(0.0)         , Scalar(2.0)*N/(T-B) , (T+B)/(B-T) , Scalar(0.0),
    Scalar(0.0)         , Scalar(0.0)         , (F+N)/(F-N) ,(Scalar(2.0)*F*N)/(N-F),
    Scalar(0.0)         , Scalar(0.0)         , Scalar(1.0) , Scalar(0.0);

    return ret;
}

template<typename CameraModel>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<typename CameraModel::Scalar,4,4> getPerspectiveProjection(const CameraModel& cam, typename CameraModel::Scalar N, typename CameraModel::Scalar F)
{
    return getPerspectiveProjectionRDFTopLeft(cam,N,F);
}

// NOTE element (2,3) may be wrong

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> lookAtRUB(const Eigen::Matrix<T,3,1>& eye, const Eigen::Matrix<T,3,1>& center, const Eigen::Matrix<T,3,1>& up)
{
    Eigen::Matrix<T,4,4> view_matrix = Eigen::Matrix<T,4,4>::Zero();
    Eigen::Matrix<T,3,3> R;
    R.col(2) = (eye-center).normalized();
    R.col(0) = up.cross(R.col(2)).normalized();
    R.col(1) = R.col(2).cross(R.col(0));
    view_matrix.template topLeftCorner<3,3>() = R.transpose();
    view_matrix.template topRightCorner<3,1>() = -R.transpose() * center;
    view_matrix.row(3) << T(0.0), T(0.0), T(0.0), T(1.0);
    return view_matrix;
}

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> lookAtRDF(const Eigen::Matrix<T,3,1>& eye, const Eigen::Matrix<T,3,1>& center, const Eigen::Matrix<T,3,1>& up)
{
    Eigen::Matrix<T,4,4> view_matrix = Eigen::Matrix<T,4,4>::Zero();
    Eigen::Matrix<T,3,3> R;
    R.col(2) = (center-eye).normalized();
    R.col(0) = R.col(2).cross(up).normalized();
    R.col(1) = R.col(2).cross(R.col(0));
    view_matrix.template topLeftCorner<3,3>() = R.transpose();
    view_matrix.template topRightCorner<3,1>() = -R.transpose() * center;
    view_matrix.row(3) << T(0.0), T(0.0), T(0.0), T(1.0);
    return view_matrix;
}

template<typename T>
EIGEN_DEVICE_FUNC inline Eigen::Matrix<T,4,4> lookAt(const Eigen::Matrix<T,3,1>& eye, const Eigen::Matrix<T,3,1>& center, const Eigen::Matrix<T,3,1>& up)
{
    return lookAtRDF(eye, center, up);
}

}

#endif // CAMERA_PROJECTION_MATRICES_HPP


/**
 * @file Mesh.hpp
 * @brief
 */
#ifndef MI4_MESH_HPP
#define MI4_MESH_HPP 1
#include <iostream>
#include <vector>
#include <utility>
#include <deque>
#include <map>
#include <cmath>
#include <Eigen/Dense>
namespace mi4
{
        class Mesh
        {
        private:
                std::string _name;
                std::vector<Eigen::Vector3d> _vertex; /// vertex position
                std::vector<size_t> _index; /// triangles
        public:
                void clone ( const Mesh& mesh )
                {
                        this->init();

                        for ( int i = 0 ; i < mesh.getNumVertices() ; ++i ) {
                                this->addPoint ( mesh.getPosition ( i ) );
                        }

                        for ( int i = 0 ; i < mesh.getNumFaces() ; ++i ) {

                                this->addFace ( mesh.getFaceIndices ( i ) );
                        }
                }

                size_t addPoint ( const Eigen::Vector3d& p ) const
                {
                        const_cast<Mesh*> ( this )->_vertex.push_back ( p );
                        return this->_vertex.size() - 1;
                }

                size_t addFace ( const std::vector<size_t>& fidx )
                {
                        if ( fidx.size() != 3 ) {
                                std::cerr << "only triangle is supported." << std::endl;
                                return 0;
                        }

                        this->_index.insert ( this->_index.end(), fidx.begin(), fidx.end() );
                        return this->_index.size() / 3 ; // ID
                }

                void addName ( const std::string name = std::string ( "mesh" ) )
                {
                        this->_name = name;
                        return;
                }

                inline bool isValidFaceId ( const size_t faceid ) const
                {
                        return ( faceid < this->getNumFaces() );
                }

                inline bool isValidVertexId ( const size_t vertexid ) const
                {
                        return ( vertexid < this->getNumVertices() );
                }

                inline std::vector<size_t> getFaceIndices ( const size_t faceid ) const
                {
                        std::vector<size_t> idx;

                        if ( this->isValidFaceId ( faceid ) ) {
                                for ( int i = 0 ; i < 3 ; i++ ) {
                                        idx.push_back ( this->_index.at ( faceid * 3 + i ) );
                                }
                        }

                        return idx;
                }

                inline Eigen::Vector3d getPosition ( const size_t vertexid ) const
                {
                        if ( this->isValidVertexId ( vertexid ) ) {
                                return this->_vertex.at ( vertexid );
                        }

                        return Eigen::Vector3d();
                }

                Eigen::Vector3d getNormal ( const size_t faceid, bool normalize = true ) const
                {
                        if ( ! this->isValidFaceId ( faceid ) ) {
                                return Eigen::Vector3d();
                        }

                        auto fidx = this->getFaceIndices ( faceid );
                        Eigen::Vector3d v0 = this->getPosition ( fidx[0] );
                        Eigen::Vector3d v1 = this->getPosition ( fidx[1] ) - v0;
                        Eigen::Vector3d v2 = this->getPosition ( fidx[2] ) - v0;
                        Eigen::Vector3d n = v1.cross ( v2 );

                        if ( normalize ) {
                                n.normalize();
                        }

                        return n;
                }

                inline std::string
                getName ( void ) const
                {
                        return this->_name;
                }

                void setPosition ( const size_t vertexid, const Eigen::Vector3d& pos )
                {
                        if ( this->isValidVertexId ( vertexid ) ) {
                                this->_vertex.at ( vertexid ) = pos;
                        }

                        return;
                };
                void init ( void )
                {
                        this->_vertex.clear();
                        this->_index.clear();
                        return;
                }

                inline size_t getNumVertices ( void ) const
                {
                        return this->_vertex.size();
                }

                inline size_t getNumFaces ( void ) const
                {
                        return this->_index.size() / 3;
                }

                void
                getBoundingBox ( Eigen::Vector3d& bmin, Eigen::Vector3d& bmax )
                {
                        Eigen::AlignedBox3d bbox;
                        const size_t numv = this->getNumVertices();

                        for ( size_t i = 1 ; i < numv ; ++i ) {
                                bbox.extend ( this->getPosition ( i ) );
                        }

                        bmin = bbox.min();
                        bmax = bbox.max();
                        return ;
                }

                void negateOrientation()
                {
                        for ( size_t i = 0 ; i < this->getNumFaces() ; ++i ) {
                                size_t f0 = this->_index [ i * 3 + 0 ];
                                this->_index[ i * 3 + 0 ] = this->_index [ i * 3 + 1];
                                this->_index[ i * 3 + 1 ] = f0;

                        }

                }

                double getArea ( const int faceId ) const
                {
                        std::vector<size_t> index = this->getFaceIndices ( faceId );
                        const Eigen::Vector3d p0 = this->getPosition ( index[0] );
                        const Eigen::Vector3d p1 = this->getPosition ( index[1] );
                        const Eigen::Vector3d p2 = this->getPosition ( index[2] );
                        const Eigen::Vector3d v0 = p1 - p0;
                        const Eigen::Vector3d v1 = p2 - p0;
                        return v0.cross ( v1 ).norm() * 0.5; //|v0^v1|/2
                }
        };
}//namespace mi4
#endif


#pragma once

// std c++ headers
#include <cmath>

// mtl headers
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/reduction_functors.hpp>
#include <boost/numeric/mtl/operation/dot.hpp>

// AMDiS headers
#include "operations/assign.hpp"
#include "operations/functors.hpp"

namespace AMDiS
{
  namespace functors
  {
    // unary reduction functors: import from mtl
    using MTL_VEC::infinity_norm_functor;
    using MTL_VEC::sum_functor;
    using MTL_VEC::product_functor;
    using MTL_VEC::max_functor;
    using MTL_VEC::min_functor;

    /// Reduction functor to calculate the ||v||_1
    /** Defines \ref init, \ref update and \ref post_reduction
     *  so that ||v||_1 := |v_0| + |v_1| + ...:
     *  init: result = 0,
     *  update: result += |v_i|,
     *  post_reduction: result = result
     **/
    template <class A>
    struct one_norm_functor
    {
      using result_type = A;

      template <class Value>
      static void init(Value& value)
      {
        using ::math::zero;
        value= zero(value);
      }

      template <class Value, class Element>
      static void update(Value& value, const Element& x)
      {
        using std::abs;
        value+= abs(x);
      }

      template <class Value>
      static void finish(Value& value, const Value& value2)
      {
        value+= value2;
      }

      template <class Value>
      static constexpr Value post_reduction(Value const& value)
      {
        return value;
      }
    };

    /// Reduction functor to calculate the |v|_2
    /** Defines \ref init, \ref update and \ref post_reduction
     *  so that ||v||_2 := sqrt(|v_0|^2 + |v_1|^2 + ...):
     *  init: result = 0,
     *  update: result += |v_i|^2,
     *  post_reduction: result = sqrt(result)
     **/
    template <class A>
    struct two_norm_functor
    {
      using result_type = A;

      template <class Value>
      static void init(Value& value)
      {
        using ::math::zero;
        value= zero(value);
      }

      template <class Value, class Element>
      static void update(Value& value, Element const& x)
      {
        using mtl::squared_abs;
        value+= squared_abs(x);
      }

      template <class Value>
      static void finish(Value& value, Value const& value2)
      {
        value+= value2;
      }

      // After reduction compute square root
      template <class Value>
      static Value post_reduction(Value const& value)
      {
        using std::sqrt;
        return sqrt(value);
      }
    };

    /// Same as \ref two_norm_functor without the root at the end:
    /**
     *  post_reduction: result = result
     **/
    template <class A>
    struct unary_dot_functor
      : two_norm_functor<A>
    {
      template <typename Value>
      static Value post_reduction(Value const& value)
      {
        return value;
      }
    };

    /// \cond HIDDEN_SYMBOLS
    template <class A, class B, class ConjOp>
    struct dot_functor_aux
    {
      using result_type = decltype( std::declval<ConjOp>()(std::declval<A>()) * std::declval<B>() );

      template <class Value>
      static void init(Value& value)
      {
        using ::math::zero;
        value= zero(value);
      }

      template <class Value, class Element1, class Element2>
      static void update(Value& value, Element1 const& x, Element2 const& y)
      {
        value+= ConjOp()(x) * y;
      }

      template <class Value>
      static void finish(Value& value, Value const& value2, Value const& value3)
      {
        value+= ConjOp()(value2) * value3;
      }

      template <class Value>
      static constexpr Value post_reduction(Value const& value)
      {
        return value;
      }
    };
    /// \endcond

    /// Binary reduction functor (scalar product)
    /** Same as reduction functors, but \ref update has two arguments:
     *  init: result = 0,
     *  update: result += v_i^H * w_i,
     *  post_reduction: result =result
     **/
    template <class A, class B>
    using dot_functor
      = dot_functor_aux<A,B, MTL_VEC::detail::with_conj>;

    /// Binary reduction functor (scalar product)
    /** Same as reduction functors, but \ref update has two arguments:
     *  init: result = 0,
     *  update: result += v_i^T * w_i,
     *  post_reduction: result =result
     **/
    template <class A, class B>
    using dot_real_functor
      = dot_functor_aux<A,B, MTL_VEC::detail::without_conj>;

    template <class ResultType, class InitAssign, class UpdateAssign,
              class PostOp = identity<ResultType>, class FinishAssign = UpdateAssign>
    struct general_unary_reduction_functor
    {
      using result_type = ResultType;

      template <class Value>
      static void init(Value& value)
      {
        InitAssign()(value);
      }

      template <class Value, class Element>
      static void update(Value& value, Element const& x)
      {
        UpdateAssign()(value, x);
      }

      template <class Value>
      static void finish(Value& value, Value const& value2)
      {
        FinishAssign()(value, value2);
      }

      // After reduction compute square root
      template <class Value>
      static constexpr Value post_reduction(Value const& value)
      {
        return PostOp()(value);
      }
    };

    // max(v0, v1, v2, v3,...)
    template <class T>
    using max_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::min_value<T>, AMDiS::assign::max<T>>;

    // max(|v0|,|v1|,|v2|,...)
    template <class T>
    using abs_max_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::ct_value<T, int, 0>,
        AMDiS::assign::compose<AMDiS::assign::max<T>, 2, abs<T>>>;

    // min(v0, v1, v2, v3, ...)
    template <class T>
    using min_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::max_value<T>, AMDiS::assign::min<T>>;

    // min(|v0|,|v1|,|v2|,...)
    template <class T>
    using abs_min_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::max_value<T>,
        AMDiS::assign::compose<AMDiS::assign::min<T>, 2, abs<T>>>;

    // v0+v1+v2+v3+...
    template <class T>
    using sum_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::ct_value<T, int, 0>, AMDiS::assign::plus<T>>;

    // v0*v1*v2*v3*...
    template <class T>
    using prod_reduction_functor
      = general_unary_reduction_functor<T,
        AMDiS::assign::ct_value<T, int, 1>, AMDiS::assign::multiplies<T>>;

  } // end namespace functors
} // end namespace AMDiS


#ifndef ILP_TASK_HPP
#define ILP_TASK_HPP

#include <eigen3/Eigen/Core>
#include <boost/container_hash/hash.hpp>

#include <utility>

namespace ilp
{
    using index_t = Eigen::Index;

    template <typename T>
    using matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

    template <typename T>
    using rvector = Eigen::Matrix<T, 1, Eigen::Dynamic>;

    template <typename T>
    using cvector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

    namespace detail
    {
        template <typename T>
        struct VectorHash
        {
            std::size_t operator () (const rvector<T>& row) const
            {
                std::size_t seed = 0;
                for (index_t i = 0; i != row.cols(); ++i)
                {
                    boost::hash_combine(seed, row(i, 0));
                }

                return seed;
            }

            std::size_t operator () (const cvector<T>& column) const
            {
                std::size_t seed = 0;
                for (index_t i = 0; i != column.rows(); ++i)
                {
                    boost::hash_combine(seed, column(i, 0));
                }

                return seed;
            }
        };

    } // namespace detail

    class ilp_task
    {
    public:
        ilp_task(matrix<int> A, cvector<int> b, rvector<int> c);
        ~ilp_task() = default;

        [[nodiscard]] std::size_t size_m() const;
        [[nodiscard]] std::size_t size_n() const;

    public:
        matrix<int> A;
        cvector<int> b;
        rvector<int> c;
        cvector<int> x;

        std::size_t m;
        std::size_t n;
    };

    struct ilp_solution
    {
        bool is_feasible = false;
        bool is_bounded = false;
        cvector<int> x;
        int c_result = 0;
        double time = 0;
    };

} // namespace ilp

template <typename T>
std::ostream& operator << (std::ostream& os, const ilp::rvector<T>& vec)
{
    bool flag{false};

    os << "(";
    for (ilp::index_t col = 0; col < vec.cols(); ++col)
    {
        if (flag)
        {
            os << ", ";
        }
        flag = true;
        os << vec(col);
    }
    os << ")";
    return os;
}

template <typename T>
std::ostream& operator << (std::ostream& os, const ilp::cvector<T>& vec)
{
    bool flag{false};

    os << "(";
    for (ilp::index_t row = 0; row < vec.rows(); ++row)
    {
        if (flag)
        {
            os << ", ";
        }
        flag = true;
        os << vec(row);
    }
    os << ")^T";
    return os;
}

template <typename T>
std::ostream& operator << (std::ostream& os, const ilp::matrix<T>& A)
{
    for (int row = 0; row < A.rows(); ++row)
    {
        for (int col = 0; col < A.cols(); ++col)
        {
            os << "   " << A(row, col) << " ";
        }

        if (row != A.rows() - 1)
        {
            os <<  "\n";
        }
    }

    return os;
}

#endif // ILP_TASK_HPP


#include "tkdCmdParser.h"

#include "nrFourier.h"

#include "vnl/vnl_matrix.h"
#include "vnl/vnl_vector.h"
#include "vnl/algo/vnl_determinant.h"
#include "vnl/algo/vnl_svd.h"
#include "vnl/algo/vnl_fft_1d.h"
#include "vnl/algo/vnl_cholesky.h"

#include "itkNumericTraits.h"

#include <stdlib.h>
#include <math.h>
#include <fstream>

#include <boost/math/constants/constants.hpp>

#include "Array.h"
#include "fftw++.h"

using namespace Array;
using namespace fftwpp;

/**
 * Option list.
 */
struct parameters
{
	std::string inputFileName;
	std::string separation;
	bool verbose;
};

/**
 * General Linear Model (GLM) with autocorrelation prewhitening.
 *
 * Refs:
 * 1. Book: 'Functional MRI an introduction to methods',
 * 		P. Jezzard, P.M. Matthews, S.M. Smith, Oxford (2005).
 *
 * 2. Article: 'Temporal autocorrelation in Univariate Modeling of FMRI Data',
 * 		M.W. Woolrich, B.D. Ripley, M. Brady and S.M. Smith, Neuroimage 14, 1370-1386 (2001).
 */
namespace general_linear_model
{
	typedef double PixelType;
	typedef vnl_matrix< PixelType > MatrixType;
	typedef vnl_vector< PixelType > VectorType;

	class GLM
	{
	public:

		/**
		 * Run glm.
		 */
		void Run( parameters& list )
		{
			// data Y
			VectorType Y( 8, 0 );
			Y( 0 ) = 0.1;
			Y( 1 ) = 3.5;
			Y( 2 ) = 6.7;
			Y( 3 ) = 6.4;
			Y( 4 ) = 3.5;
			Y( 5 ) = 3.7;
			Y( 6 ) = 3.1;
			Y( 7 ) = 2.1;

			// design matrix X
			MatrixType X( 8, 3 );
			X( 0, 0 ) = 1;
			X( 1, 0 ) = 1;
			X( 2, 0 ) = 1;
			X( 3, 0 ) = 1;
			X( 4, 0 ) = 1;
			X( 5, 0 ) = 1;
			X( 6, 0 ) = 1;
			X( 7, 0 ) = 1;

			X( 0, 1 ) = 0;
			X( 1, 1 ) = 0;
			X( 2, 1 ) = 1;
			X( 3, 1 ) = 1;
			X( 4, 1 ) = 0;
			X( 5, 1 ) = 0;
			X( 6, 1 ) = 0;
			X( 7, 1 ) = 0;

			X( 0, 2 ) = 0;
			X( 1, 2 ) = 0;
			X( 2, 2 ) = 0;
			X( 3, 2 ) = 0;
			X( 4, 2 ) = 1;
			X( 5, 2 ) = 1;
			X( 6, 2 ) = 0;
			X( 7, 2 ) = 0;

			// Toeplitz matrix S
			MatrixType S( Y.size(), Y.size() );
			S.set_identity();

			// Adjust for correlation using Cochrane–Orcutt estimation
			VectorType B = OLS( Y, X, S );

			std::cout << "Betas: " << B << std::endl;

			// Residuals
			VectorType r = Residuals( Y, X, B );

			std::cout << "Residuals: " << r << std::endl;

			// Raw autocorrelation (all lags)
			VectorType ac = AutoCorrelation( r );

			std::cout << "Raw autocorrelation: " << ac << std::endl;

			// non-parametric autocorrelation smoothing (M = 2sqrt(N))
			unsigned int M = 2. * std::sqrt( Y.size() );

			VectorType sac = TaperedCosineWindow( ac, M );

			std::cout << "Smoothed autocorrelation (" << M << ")" << ": " << sac << std::endl;

			// construct V
			MatrixType V = GetV( sac, true );

			vnl_svd< PixelType > svd( V );
			//svd.solve();
			MatrixType W = svd.W();
			MatrixType Vprime = W * W.transpose();

			for( unsigned int r = 0; r < V.rows(); r++ )
			{
				for( unsigned int c = 0; c < V.cols(); c++ )
				{
					std::cout << V( r, c ) << " ";
				}
				std::cout << std::endl;
			}

			std::cout << "UU'" << std::endl;

			for( unsigned int r = 0; r < Vprime.rows(); r++ )
			{
				for( unsigned int c = 0; c < Vprime.cols(); c++ )
				{
					std::cout << Vprime( r, c ) << " ";
				}
				std::cout << std::endl;
			}

			exit( 0 );

			// refit
			//B = OLS( Y, X, S );
		}

	protected:

		/**
		 * Return inverted Cholesky V=KK'
		 */
		MatrixType GetInvertedK( const MatrixType& V )
		{
			vnl_svd< PixelType > svd( V );
			MatrixType pinvV = svd.inverse();

		}

		/**
		 * Return V.
		 */
		MatrixType GetV( const VectorType& v, bool circular )
		{
			MatrixType result( v.size(), v.size(), 0.0 );

			for( unsigned int r = 0; r < result.rows(); r++ )
				for( unsigned int c = r; c < result.cols(); c++ )
					result( r, c ) = v( r );

			result += result.transpose();

			result.fill_diagonal( 1 );

			return result;
		}

		/**
		 * Tukey windowing.
		 *
		 * http://en.wikipedia.org/wiki/Window_function#Tukey_window
		 */
		VectorType TaperedCosineWindow( const VectorType& ar, unsigned int M )
		{
			VectorType rho( ar.size(), 0 );

			for ( unsigned int i = 0; i < ar.size(); i++ )
			{
				if ( i < M )
				{
					PixelType PI = boost::math::constants::pi< PixelType >();
					rho( i ) = 0.5 * ( 1 + std::cos( ( PI * i ) / M ) ) * ar( i );
				} else
				{
					rho( i ) = 0;
				}
			}

			return rho;
		}

		/**
		 * Cross correlation for all lags
		 * @param a Vector 1 (of length n)
		 * @param b Vector 2 (of length n)
		 * @param output Output vector of length n
		 *
		 * Normalize the sequence so the autocorrelations at zero lag are identically 1.0.
		 */
		VectorType AutoCorrelation( const VectorType& a )
		{
			const int n = a.size();

			// added zeropadding!
			int zeropad = (int) pow( 2, ceil( log( a.size() ) / log( 2 ) ) );
			VectorType output( zeropad, 0 );

			for ( int i = 0; i < n; ++i )
				output[i] = a[i];

			array1< PixelType > finput( a.size(), sizeof( Complex ) );
			array1< Complex > foutput( a.size(), sizeof( Complex ) );
			array1< Complex > ftemp( a.size(), sizeof( Complex ) );

			for( unsigned int i = 0; i < a.size(); i++ )
				finput[ i ] = a( i );

			rcfft1d Forward( a.size(), finput, foutput );

			Forward.fft( finput, foutput ); // fill foutput ...
			Forward.fft( finput, ftemp ); // fill ftemp ...

			const PixelType no2 = static_cast< PixelType > ( n >> 1 );

			for ( int i = 2; i < n; i += 2 )
			{
				Complex ftmp = foutput[i];
				foutput[i] = ( foutput[i] * ftemp[i] + foutput[i + 1] * ftemp[i + 1] ) / no2;
				foutput[i + 1] = ( foutput[i + 1] * ftemp[i] - ftmp * ftemp[i + 1] ) / no2;
			}

			foutput[0] = foutput[0] * ftemp[0] / no2;
			foutput[1] = foutput[1] * ftemp[1] / no2;

			crfft1d Backward( a.size(), foutput, finput );
			Backward.fftNormalized( foutput, finput );

			VectorType finalOutput( n );

			// recrop
			for ( int i = 0; i < n; ++i )
				finalOutput( i ) = std::abs( foutput( i ) / foutput( 0 ) ); // normalize

			return finalOutput;
		}

		/**
		 * Return standard deviation.
		 */
		template< class T >
		T GetSD( const vnl_vector< T >& distances )
		{
			int size = distances.size();

			if ( size < 2 )
			{
				return 0;
			}

			T mean = distances.mean();

			T sd = 0;
			for ( int i = 0; i < size; i++ )
			{
				sd += pow( distances[i] - mean, 2 );
			}

			return vcl_sqrt( sd / size );
		}

		/**
		 * Return residuals of fit.
		 */
		VectorType Residuals( const VectorType& Y, const MatrixType& X, const VectorType& B )
		{
			return Y - X * B;

			// TODO
			//VectorType r( B.size(), 0 );
			//return r;
		}

		/**
		 * Ordinary Least Squares ( OLS ) estimation, by finding the
		 * maximum likelihood estimates of a linear regression model.
		 *
		 * Ref: http://en.wikipedia.org/wiki/Ordinary_least_squares
		 */
		VectorType OLS( const VectorType& Y, const MatrixType& X, const MatrixType& S )
		{
			if ( Y.size() != X.rows() )
			{
				std::cerr << "*** ERROR ***: The data vector Y and rows of the "
					"design matrix X must have the same length." << std::endl;
				exit( EXIT_FAILURE );
			}
			if ( X.rows() != S.rows() )
			{
				std::cerr << "*** ERROR ***: The Toeplitz matrix S and "
					"design matrix X must have the same number of rows." << std::endl;
				exit( EXIT_FAILURE );
			}

			// SY = SXB + mu, where mu ~ N( 0, sigma^2 SVS') => Bhat = pinv(SX)SY,
			// where pinv(SX) is the Moore-Penrose pseudoinverse: ((SX)'SX)^-1(SX)'
			vnl_svd< PixelType > svd( ( S * X ).transpose() * S * X );
			MatrixType pinvSX = svd.inverse() * ( S * X ).transpose();

			return pinvSX * S * Y;
		}
	};
} // end namespace general_linear_model

/**
 * Main.
 */
int main( int argc, char ** argv )
{
	tkd::CmdParser p( argv[0], "Generalized linear model" );

	parameters list;
	list.verbose = false;

	p.AddArgument( list.inputFileName, "input" ) ->AddAlias( "i" ) ->SetInput( "filename" ) ->SetDescription(
			"Input file: list of covariate(s) and response function (column format)" ) ->SetRequired( true );

	p.AddArgument( list.separation, "separation" ) ->AddAlias( "s" ) ->SetInput( "string" ) ->SetDescription(
			"Data separator string (default: \\t" );

	p.AddArgument( list.verbose, "verbose" ) ->AddAlias( "v" ) ->SetInput( "bool" ) ->SetDescription( "Verbose (default: false" );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return EXIT_FAILURE;
	}

	general_linear_model::GLM glm;

	glm.Run( list );

	return EXIT_SUCCESS;
}


#include <iostream>
#include <cmath>
#include <ctime>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <iomanip>
#include <vector>

#include "mystuff.h"

using namespace mosya;

#ifdef ZAPPY_ZAPPY
#include "RandomNormalPair.cpp"
#include "AtoG.cpp"
//#include "GtoA.cpp"
//#include "ProjectPointsOntoEllipse.cpp"
//#include "ProjectPointsOntoHyperbola.cpp"
//#include "ProjectPointsOntoParabola.cpp"
//#include "ProjectPointsOntoConicByEberlyOrig.cpp"
#include "eigen2x2.cpp"
#include "ProjectPointsOntoConicByEberlyModified.cpp"
#include "AdjustProjectedPointsOnConic.cpp"
#include "DistanceToConicApprx.cpp"
#include "ProjectionQuality.cpp"
#include "IsCrossingWindow.cpp"
#include "RootOfCubicEquation.cpp"
//#include "ProjectPointsOntoConicByWEPqz.cpp"
//#include "ProjectPointsOntoConicByWEPeq3Schur.cpp"
#include "ProjectPointsOntoConicByWEPeq3NR.cpp"
//#include "ProjectPointsOntoConicByWEPinv.cpp"
//#include "ProjectPointsOntoConicByQuarticEquation.cpp"
#include "ProjectPointsOntoConicByNewtonAK.cpp"
#endif

//  Main program for testing projection methods
//  Specify the methods by selecting which functions the program calls
//  Choose double or long double precision in file "mystuff.h"

int main()
{
    const int n=2,histCols=18,histRows=6,timing=1;

    integers i,j,k,N=0,N0=0,N1=0,N2=0,N3=0,Nmiss=0;
    int code,hist1[histRows][histCols]={0}, hist2[histRows][histCols]={0};

    const reals Window=One,Emin=pow(10.,-histCols-1);

    reals r1,r2;
    reals E1[histRows][n], E2[histRows][n];
    double Times[histRows]={0.};

    M6x1 A;
    M5x1 ParG;
    Mnx1 X(n,1),Y(n,1),D(n,1),Dmin(n,1),Q(n,1);
    Mnx1 Xproj(n,1),Yproj(n,1),XprojA(n,1),YprojA(n,1),Xbest(n,1),Ybest(n,1);
    Mnxm XprojAll(n,histRows),YprojAll(n,histRows);

    vector<reals> Xv(n),Yv(n);

    clock_t iniTime, finTime;        // auxiliary variables for timing
    timespec ts1, ts2;

    srand ( (unsigned)time(NULL) );  // seed the random generator
    cout.precision(15);

    while(1)   //  main loop over random conics and points
	{
            //  generate a conic:

            RandomNormalPair(r1,r2);  A(0) = r1;  A(1) = r2;
            RandomNormalPair(r1,r2);  A(2) = r1;  A(3) = r2;
            RandomNormalPair(r1,r2);  A(4) = r1;  A(5) = r2;

            reals eps=1.e-13; RandomNormalPair(r1,r2);  A(2) = A(1)*A(1)/A(0) + eps*r1;  // make it a parabola (if desired)

            A.normalize();      //  normalization of the parameter vector (unnecessary)

            //  discard the conic if it is of the wrong type:

            if (IsCrossingWindow(A,Window)==0)  {  Nmiss++; continue;  }  //  discard conics that do not cross the data window

            AtoG(A,ParG,code);                       //   determine the conic type
            if (code>3) { N0++;  continue; }         //   discard degenerate conics

            if (code==1) N1++;   //  count ellipses
            if (code==2) N2++;   //  count hyperbolas
            if (code==3) N3++;   //  count parabolas
            N++;                 //  count all conics together

            //   generate n data points:

            for (i=0; i<n; i++)
                {
                    Xv[i] = Window*(Two*rand()/RAND_MAX - One);
                    Yv[i] = Window*(Two*rand()/RAND_MAX - One);
                    X(i) = Xv[i];
                    Y(i) = Yv[i];
                    Dmin(i) = REAL_MAX;
                }

            //   First projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            ProjectPointsOntoConicByWEPeq3NR(X,Y,A,Xproj,Yproj);  //  choose a projection method here
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[0] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[0][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(0) = Xproj;
            YprojAll.col(0) = Yproj;

            //   Second projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            AdjustProjectedPointsOnConic(X,Y,A,Xproj,Yproj,XprojA,YprojA,2);  //  choose a projection method here
            Xproj = XprojA;  Yproj = YprojA;
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[1] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[1][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(1) = Xproj;
            YprojAll.col(1) = Yproj;

            //   Third projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            ProjectPointsOntoConicByNewtonAK(X,Y,A,Xproj,Yproj);  //  choose a projection method here
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[2] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[2][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(2) = Xproj;
            YprojAll.col(2) = Yproj;

            //   Fourth projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            AdjustProjectedPointsOnConic(X,Y,A,Xproj,Yproj,XprojA,YprojA,2);  //  choose a projection method here
            Xproj = XprojA;  Yproj = YprojA;
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[3] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[3][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(3) = Xproj;
            YprojAll.col(3) = Yproj;

            //   Fifth projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            ProjectPointsOntoConicByEberlyModified(X,Y,A,Xproj,Yproj);  //  choose a projection method here
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[4] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[4][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(4) = Xproj;
            YprojAll.col(4) = Yproj;

            //   Sixth projection method:

            if (timing) {clock_gettime(CLOCK_REALTIME, &ts1);}
            AdjustProjectedPointsOnConic(X,Y,A,Xproj,Yproj,XprojA,YprojA,2);  //  choose a projection method here
            Xproj = XprojA;  Yproj = YprojA;
            if (timing) {clock_gettime(CLOCK_REALTIME, &ts2); Times[5] += (ts2.tv_sec-ts1.tv_sec) + 1.e-9*(ts2.tv_nsec - ts1.tv_nsec);}
            //if (timing) {cout << endl; cin.ignore();}
            ProjectionQuality(X,Y,A,Xproj,Yproj,Q);
            DistanceToConicApprx(X,Y,A,Xproj,Yproj,D);
            for (i=0; i<n; i++)
                {
                    E2[5][i] = Q(i);
                    if (Dmin(i) > D(i))
                        {
                            Dmin(i) = D(i);
                            Xbest(i) = Xproj(i);
                            Ybest(i) = Yproj(i);
                        }
                }
            XprojAll.col(5) = Xproj;
            YprojAll.col(5) = Yproj;

            //   Record the results of all projection methods

            for (i=0; i<n; i++)
                {
                    for (j=0; j<histRows; j++)   {  E1[j][i] = sqrt(SQR(XprojAll(i,j)-Xbest(i)) + SQR(YprojAll(i,j)-Ybest(i)));  }

                    for (j=0; j<histRows; j++)   {  if (E1[j][i] < Emin) E1[j][i] = Emin;  }
                    for (j=0; j<histRows; j++)   {  if (E2[j][i] < Emin) E2[j][i] = Emin;  }
                }

            for (i=0; i<n; i++)
                {
                    for (j=0; j<histRows; j++)
                        {
                            k = floor(-log10(E1[j][i]));
                            if (k<1) k=1;
                            if (k<=histCols) hist1[j][k-1]++;

                            k = floor(-log10(E2[j][i]));
                            if (k<1) k=1;
                            if (k<=histCols) hist2[j][k-1]++;
                        }
                }

            if (((N*n)%2000000)==0)
                {
                    for (j=0; j<histRows; j++)
                        {
                            for (i=0;i<histCols;i++)  cout << setw(6) << hist1[j][i];
                            cout << endl;
                        }
                    cout << endl;

                    for (j=0; j<histRows; j++)
                        {
                            for (i=0;i<histCols;i++)  cout << setw(6) << hist2[j][i];
                            cout << endl;
                        }
                    cout << endl;

                    if (timing)
                        {
                            cout << " Runnig times per conic (in milisec): ";
                            for (j=0; j<histRows; j++) cout << "  " << setprecision(5) << Times[j]/N*1000000.;
                            cout << endl << endl;;
                        }

                    cout << " conics used:  N = " << N << "  (Ell: " << N1 << "  Hyp: " << N2 << "  Par: " << N3 << ") " << endl;
                    cout << " conics removed:  " << N0 << " degenetare and " << Nmiss << " missing the window" << endl << endl;
                }
            //if ((N*n)>=1000000000) return 0;
        }
}