// This implementation is based on conventional formulation
// Author: Zhiyuan Liu
// Date: Sept, 2018
#include "vtkForwardFlowLogic.h"
// MRML includes
#include <vtkMRMLScene.h>
#include <vtkMRMLModelNode.h>
#include <vtkMRMLModelDisplayNode.h>
#include <vtkMRMLDisplayNode.h>
#include <vtkMRMLMarkupsDisplayNode.h>
#include <vtkMRMLMarkupsFiducialNode.h>
#include <vtkMRMLMarkupsNode.h>
#include "vtkSlicerMarkupsLogic.h"

// VTK includes
#include <vtkIntArray.h>
#include <vtkNew.h>
#include <vtkCenterOfMass.h>
#include <vtkObjectFactory.h>
#include <vtkCurvatures.h>
#include <vtkWindowedSincPolyDataFilter.h>
#include <vtkSmoothPolyDataFilter.h>
#include <vtkParametricEllipsoid.h>
#include <vtkParametricFunctionSource.h>
#include <vtkImplicitPolyDataDistance.h>
#include <vtkPolyDataNormals.h>

#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkDataArray.h>
#include <vtkDoubleArray.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkMassProperties.h>

// Eigen includes
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

// STD includes
#include <cassert>
#include <iostream>

// flow surface to the end: either it's ellipsoidal enough or reach max_iter
int vtkForwardFlowLogic::FlowSurfaceMesh(const std::string &filename, double dt, double smooth_amount, int max_iter, int freq_output)
{
    // std::cout << filename << std::endl;
    // std::cout << dt << std::endl;
    // std::cout << smooth_amount << std::endl;
    // std::cout << max_iter << std::endl;
    // std::cout << freq_output << std::endl;
    vtkSmartPointer<vtkPolyDataReader> reader =
        vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(filename.c_str());
    reader->Update();

    vtkSmartPointer<vtkPolyData> mesh =
        vtkSmartPointer<vtkPolyData>::New();
    mesh = reader->GetOutput();

    vtkSmartPointer<vtkMassProperties> mass_filter =
        vtkSmartPointer<vtkMassProperties>::New();
    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double original_volume = mass_filter->GetVolume();
//    std::cout << "Original Volume: " << original_volume << std::endl;

    // default parameters
    // double dt = 0.001;
    // double smooth_amount = 0.03;
    // int max_iter = 300;
    int iter = 0;
    double tolerance = 0.05;
    double q = 1.0;

    while(q > tolerance && iter < max_iter) {
        // smooth filter
        vtkSmartPointer<vtkWindowedSincPolyDataFilter> smooth_filter =
            vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
        smooth_filter->SetPassBand(smooth_amount);
        smooth_filter->NonManifoldSmoothingOn();
        smooth_filter->NormalizeCoordinatesOn();
        smooth_filter->SetNumberOfIterations(20);
        smooth_filter->FeatureEdgeSmoothingOff();
        smooth_filter->BoundarySmoothingOff();
        smooth_filter->SetInputData(mesh);
        smooth_filter->Update();
        if(smooth_amount > 0) {
            mesh = smooth_filter->GetOutput();
        }

        // normal filter
        vtkSmartPointer<vtkPolyDataNormals> normal_filter =
            vtkSmartPointer<vtkPolyDataNormals>::New();
        normal_filter->SplittingOff();
        normal_filter->ComputeCellNormalsOff();
        normal_filter->ComputePointNormalsOn();
        normal_filter->SetInputData(mesh);
        normal_filter->Update();
        // curvature filter
        vtkSmartPointer<vtkCurvatures> curvature_filter =
            vtkSmartPointer<vtkCurvatures>::New();
        curvature_filter->SetCurvatureTypeToMean();
        curvature_filter->SetInputData(mesh);
        curvature_filter->Update();

        vtkSmartPointer<vtkDoubleArray> H =
            vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Mean_Curvature"));
        if(H == NULL) {
            std::cerr << "error in getting mean curvature" << std::endl;
            return EXIT_FAILURE;
        }
        vtkDataArray* N = normal_filter->GetOutput()->GetPointData()->GetNormals();
        if(N == NULL) {
            std::cerr << "error in getting normals" << std::endl;
            return EXIT_FAILURE;
        }

        // perform the flow
        vtkSmartPointer<vtkPoints> points = mesh->GetPoints();
        for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
            double p[3];
            points->GetPoint(i, p);
            double curr_N[3];
            N->GetTuple(i, curr_N);
            double curr_H = H->GetValue(i);
            for(int idx = 0; idx < 3; ++idx) {
                p[idx] -= dt * curr_H * curr_N[idx];
            }
            points->SetPoint(i, p);
        }
        points->Modified();
        // double test_point[3];
        // points->GetPoint(10, test_point);
        // std::cout << test_point[0] << " , " << test_point[1] << " , " << test_point[2] << std::endl;

        mass_filter->SetInputData(mesh);
        mass_filter->Update();
        double curr_volume = mass_filter->GetVolume();
        for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
            double p[3];
            points->GetPoint(i, p);
            for(int j = 0; j < 3; ++j) {
                p[j] *= std::pow( original_volume / curr_volume , 1.0 / 3.0 );
            }
//            points->SetPoint(i, p);
        }
        points->Modified();

        if((iter +1) % freq_output == 0)
        {
            char modelName[128];
            sprintf(modelName, "output#%04d", iter+1);
            AddModelNodeToScene(mesh, modelName, false);
            vtkSmartPointer<vtkCenterOfMass> centerMassFilter =
                vtkSmartPointer<vtkCenterOfMass>::New();
            centerMassFilter->SetInputData(mesh);
            centerMassFilter->SetUseScalarsAsWeights(false);
            centerMassFilter->Update();
            double center[3];
            centerMassFilter->GetCenter(center);

            ShowFittingEllipsoid(points, curr_volume, center);

        }
        q -= 0.0001;
        iter++;
    }

    return 1;
}

// flow surface in one step
// basic idea: When the user select a mesh file, make a copy of vtk file in the application path.
// In each step of flow, read in that copy, flow it and save it the same place with same name.
// TODO: cleanup the hard disk when the module exits
int vtkForwardFlowLogic::FlowSurfaceOneStep(double dt, double smooth_amount)
{
    std::cout << "flow one step : dt-" << dt << std::endl;
    std::cout << "flow one step : smooth amount-" << smooth_amount << std::endl;
    std::string name = "temp_output.vtk";
    vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(name.c_str());
    reader->Update();
    vtkSmartPointer<vtkPolyData> mesh = reader->GetOutput();
    if(mesh == NULL)
    {
        vtkErrorMacro("No mesh has read in this module. Please select input mesh file first.");
        return -1;
    }
    vtkSmartPointer<vtkMassProperties> mass_filter =
        vtkSmartPointer<vtkMassProperties>::New();
    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double original_volume = mass_filter->GetVolume();
//    std::cout << "Original Volume: " << original_volume << std::endl;
    vtkSmartPointer<vtkWindowedSincPolyDataFilter> smooth_filter =
        vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
    smooth_filter->SetPassBand(smooth_amount);
    smooth_filter->NonManifoldSmoothingOn();
    smooth_filter->NormalizeCoordinatesOn();
    smooth_filter->SetNumberOfIterations(20);
    smooth_filter->FeatureEdgeSmoothingOff();
    smooth_filter->BoundarySmoothingOff();
    smooth_filter->SetInputData(mesh);
    smooth_filter->Update();
    if(smooth_amount > 0) {
        mesh = smooth_filter->GetOutput();
    }

//    normal filter
    vtkSmartPointer<vtkPolyDataNormals> normal_filter =
        vtkSmartPointer<vtkPolyDataNormals>::New();
    normal_filter->SplittingOff();
    normal_filter->ComputeCellNormalsOff();
    normal_filter->ComputePointNormalsOn();
    normal_filter->SetInputData(mesh);
    normal_filter->Update();
    // curvature filter
    vtkSmartPointer<vtkCurvatures> curvature_filter =
        vtkSmartPointer<vtkCurvatures>::New();
    curvature_filter->SetCurvatureTypeToMean();
    curvature_filter->SetInputData(mesh);
    curvature_filter->Update();

    vtkSmartPointer<vtkDoubleArray> H =
        vtkDoubleArray::SafeDownCast(curvature_filter->GetOutput()->GetPointData()->GetArray("Mean_Curvature"));
    if(H == NULL) {
        vtkErrorMacro("error in getting mean curvature");
        return -1;
    }
    vtkDataArray* N = normal_filter->GetOutput()->GetPointData()->GetNormals();
    if(N == NULL) {
        vtkErrorMacro("error in getting normals");
        return -1;
    }

    // perform the flow
    vtkSmartPointer<vtkPoints> points = mesh->GetPoints();

    for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
        double p[3];
        points->GetPoint(i, p);
        double curr_N[3];
        N->GetTuple(i, curr_N);
        double curr_H = H->GetValue(i);

        for(int idx = 0; idx < 3; ++idx) {
            p[idx] -= dt * curr_H * curr_N[idx];
        }
        points->SetPoint(i, p);
    }
    points->Modified();

    mass_filter->SetInputData(mesh);
    mass_filter->Update();
    double curr_volume = mass_filter->GetVolume();
    for(int i = 0; i < points->GetNumberOfPoints(); ++i) {
        double p[3];
        points->GetPoint(i, p);
        for(int j = 0; j < 3; ++j) {
            p[j] *= std::pow( original_volume / curr_volume , 1.0 / 3.0 );
        }
//        points->SetPoint(i, p);
    }
    points->Modified();

    // firstly get other intermediate result invisible
    HideNodesByNameByClass("curvate_flow_result","vtkMRMLModelNode");
    HideNodesByNameByClass("best_fitting_ellipsoid_polydata", "vtkMRMLModelNode");

    // then add this new intermediate result
    std::string modelName("curvate_flow_result");
    AddModelNodeToScene(mesh, modelName.c_str(), true);

    vtkSmartPointer<vtkPolyDataWriter> writer =
        vtkSmartPointer<vtkPolyDataWriter>::New();
    writer->SetInputData(mesh);
    writer->SetFileName(name.c_str());
    writer->Update();

    // compute the fitting ellipsoid
    vtkSmartPointer<vtkCenterOfMass> centerMassFilter =
        vtkSmartPointer<vtkCenterOfMass>::New();
    centerMassFilter->SetInputData(mesh);
    centerMassFilter->SetUseScalarsAsWeights(false);
    centerMassFilter->Update();
    double center[3];
    centerMassFilter->GetCenter(center);

    ShowFittingEllipsoid(points, curr_volume, center);
    return 0;
}
void vtkForwardFlowLogic::AddModelNodeToScene(vtkPolyData* mesh, const char* modelName, bool isModelVisible, double r, double g, double b)
{
    std::cout << "AddModelNodeToScene: parameters:" << modelName << std::endl;
    vtkMRMLScene *scene = this->GetMRMLScene();
    if(!scene)
    {
        vtkErrorMacro(" Invalid scene");
        return;
    }

    // model node
    vtkSmartPointer<vtkMRMLModelNode> modelNode;
    modelNode = vtkSmartPointer<vtkMRMLModelNode>::New();
    modelNode->SetScene(scene);

    modelNode->SetName(modelName);
    modelNode->SetAndObservePolyData(mesh);

    // display node
    vtkSmartPointer<vtkMRMLModelDisplayNode> displayModelNode;

    displayModelNode = vtkSmartPointer<vtkMRMLModelDisplayNode>::New();
    if(displayModelNode == NULL)
    {
        vtkErrorMacro("displayModelNode is NULL");
        return;
    }
    displayModelNode->SetColor(r, g, b);
    displayModelNode->SetScene(scene);
    displayModelNode->SetLineWidth(2.0);
    displayModelNode->SetBackfaceCulling(0);
    displayModelNode->SetRepresentation(1);
    if(isModelVisible)
    {
        // make the 1st mesh after flow visible
        displayModelNode->SetVisibility(1);
    }
    else
    {
        displayModelNode->SetVisibility(0);
    }

    scene->AddNode(displayModelNode);
    modelNode->AddAndObserveDisplayNodeID(displayModelNode->GetID());

    scene->AddNode(modelNode);

}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2013, 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file gsr.hpp
    \brief GSR 1 factor model
*/

#ifndef quantlib_gsr_hpp
#define quantlib_gsr_hpp

#include <ql/time/schedule.hpp>
#include <ql/math/integrals/simpsonintegral.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/distributions/normaldistribution.hpp>

#include <ql/models/shortrate/onefactormodels/gaussian1dmodel.hpp>
#include <ql/processes/gsrprocess.hpp>

#include <boost/math/special_functions.hpp>

namespace QuantLib {

//! One factor gsr model, formulation is in forward measure

class Gsr : public Gaussian1dModel, public CalibratedModel {

  public:
    // constant mean reversion
    Gsr(const Handle<YieldTermStructure> &termStructure,
        const std::vector<Date> &volstepdates,
        const std::vector<Real> &volatilities, const Real reversion,
        const Real T = 60.0);
    // piecewise mean reversion (with same step dates as volatilities)
    Gsr(const Handle<YieldTermStructure> &termStructure,
        const std::vector<Date> &volstepdates,
        const std::vector<Real> &volatilities,
        const std::vector<Real> &reversions, const Real T = 60.0);
    // constant mean reversion with floating model data
    Gsr(const Handle<YieldTermStructure> &termStructure,
        const std::vector<Date> &volstepdates,
        const std::vector<Handle<Quote> > &volatilities,
        const Handle<Quote> reversion, const Real T = 60.0);
    // piecewise mean reversion with floating model data
    Gsr(const Handle<YieldTermStructure> &termStructure,
        const std::vector<Date> &volstepdates,
        const std::vector<Handle<Quote> > &volatilities,
        const std::vector<Handle<Quote> > &reversions, const Real T = 60.0);

    const Real numeraireTime() const;
    const void numeraireTime(const Real T);

    const Array &reversion() const { return reversion_.params(); }
    const Array &volatility() const { return sigma_.params(); }

    // calibration constraints

    Disposable<std::vector<bool> > FixedReversions() {
        std::vector<bool> res(reversions_.size(), true);
        std::vector<bool> vol(volatilities_.size(), false);
        res.insert(res.end(), vol.begin(), vol.end());
        return res;
    }

    Disposable<std::vector<bool> > FixedVolatilities() {
        std::vector<bool> res(reversions_.size(), false);
        std::vector<bool> vol(volatilities_.size(), true);
        res.insert(res.end(), vol.begin(), vol.end());
        return res;
    }

    Disposable<std::vector<bool> > MoveVolatility(Size i) {
        QL_REQUIRE(i < volatilities_.size(),
                   "volatility with index " << i << " does not exist (0..."
                                            << volatilities_.size() - 1 << ")");
        std::vector<bool> res(reversions_.size() + volatilities_.size(),
                              true);
        res[reversions_.size() + i] = false;
        return res;
    }

    Disposable<std::vector<bool> > MoveReversion(Size i) {
        QL_REQUIRE(i < reversions_.size(),
                   "reversion with index " << i << " does not exist (0..."
                                           << reversions_.size() - 1 << ")");
        std::vector<bool> res(reversions_.size() + volatilities_.size(),
                              true);
        res[i] = false;
        return res;
    }

    // With fixed reversion calibrate the volatilities one by one
    // to the given helpers. It is assumed that that volatility step
    // dates are suitable for this, i.e. they should be identical to
    // the fixing dates of the helpers (except for the last one where
    // we do not need a step). Also note that the endcritera reflect
    // only the status of the last calibration when using this method.
    void calibrateVolatilitiesIterative(
        const std::vector<boost::shared_ptr<CalibrationHelper> > &helpers,
        OptimizationMethod &method, const EndCriteria &endCriteria,
        const Constraint &constraint = Constraint(),
        const std::vector<Real> &weights = std::vector<Real>()) {

        for (Size i = 0; i < helpers.size(); i++) {
            std::vector<boost::shared_ptr<CalibrationHelper> > h(1, helpers[i]);
            calibrate(h, method, endCriteria, constraint, weights,
                      MoveVolatility(i));
        }
    }

    // With fixed volatility calibrate the reversions one by one
    // to the given helpers. In this case the step dates must be chosen
    // according to the maturities of the calibration instruments.
    void calibrateReversionsIterative(
        const std::vector<boost::shared_ptr<CalibrationHelper> > &helpers,
        OptimizationMethod &method, const EndCriteria &endCriteria,
        const Constraint &constraint = Constraint(),
        const std::vector<Real> &weights = std::vector<Real>()) {

        for (Size i = 0; i < helpers.size(); i++) {
            std::vector<boost::shared_ptr<CalibrationHelper> > h(1, helpers[i]);
            calibrate(h, method, endCriteria, constraint, weights,
                      MoveReversion(i));
        }
    }

  protected:
    const Real numeraireImpl(const Time t, const Real y,
                             const Handle<YieldTermStructure> &yts) const;

    const Real zerobondImpl(const Time T, const Time t, const Real y,
                            const Handle<YieldTermStructure> &yts) const;

    void generateArguments() {
        boost::static_pointer_cast<GsrProcess>(stateProcess_)->flushCache();
        notifyObservers();
    }

    void update() { LazyObject::update(); }

    void performCalculations() const {
        Gaussian1dModel::performCalculations();
        updateTimes();
        updateState();
    }

  private:
    void updateTimes() const;
    void updateState() const;
    void initialize(Real);

    Parameter &reversion_, &sigma_;

    std::vector<Handle<Quote> > volatilities_;
    std::vector<Handle<Quote> > reversions_;
    std::vector<Date> volstepdates_; // this is shared between vols and reversions
                                     // in case of piecewise reversions
    mutable std::vector<Time> volsteptimes_;
    mutable Array volsteptimesArray_; // FIXME this is redundant (just a copy of
                                      // volsteptimes_)
};

inline const Real Gsr::numeraireTime() const {
    return boost::dynamic_pointer_cast<GsrProcess>(stateProcess_)
        ->getForwardMeasureTime();
}

inline const void Gsr::numeraireTime(const Real T) {
    boost::dynamic_pointer_cast<GsrProcess>(stateProcess_)
        ->setForwardMeasureTime(T);
}
}

#endif


#include <boost/math/distributions/uniform.hpp>


#include <Rcpp.h>
#include <iostream>
#include <fstream>
//#include <cmath>
#include <igl/copyleft/cgal/mesh_boolean.h>
#include <Eigen/Core>
using namespace Rcpp;

  // [[Rcpp::export]]
List mesh_boolean( NumericMatrix VAi, NumericMatrix FAi, NumericMatrix VBi, NumericMatrix FBi,  std::string op_type) {

  int VA_nrow = VAi.nrow(), VA_ncol = VAi.ncol();
  int VB_nrow = VBi.nrow(), VB_ncol = VBi.ncol();
  int FA_nrow = FAi.nrow(), FA_ncol = FAi.ncol();
  int FB_nrow = FBi.nrow(), FB_ncol = FBi.ncol();

  // std::cout << VA_nrow << "\t" << VA_ncol << "\t" << VB_nrow << "\t" << VB_ncol << "\t" << FA_nrow << "\t" << FA_ncol << "\t" << FB_nrow << "\t" << FB_ncol << "\n" ;

  Eigen::MatrixXd VA(VA_nrow,VA_ncol), VB(VB_nrow,VB_ncol),VC;
  Eigen::VectorXi J;
  Eigen::MatrixXi FA(FA_nrow,FA_ncol), FB(FB_nrow,FB_ncol),FC;

      for(int i=0; i < VA_nrow; i++){
        for(int j=0; j< VA_ncol; j++){
          VA(i,j) = VAi(i,j);
        }
      }

      for(int i=0; i < VB_nrow; i++){
        for(int j=0; j< VB_ncol; j++){
          VB(i,j) = VBi(i,j);
        }
      }

      for(int i=0; i < FA_nrow; i++){
        for(int j=0; j< FA_ncol; j++){
          FA(i,j) = FAi(i,j);
        }
      }

      for(int i=0; i < FB_nrow; i++){
        for(int j=0; j< FB_ncol; j++){
          FB(i,j) = FBi(i,j);
        }
      }

      if(op_type == "intersect"){
        igl::copyleft::cgal::mesh_boolean(VA,FA,VB,FB,igl::MESH_BOOLEAN_TYPE_INTERSECT,VC,FC,J);
      }else if(op_type == "union"){
        igl::copyleft::cgal::mesh_boolean(VA,FA,VB,FB,igl::MESH_BOOLEAN_TYPE_UNION,VC,FC,J);
      }else if(op_type == "xor"){
        igl::copyleft::cgal::mesh_boolean(VA,FA,VB,FB,igl::MESH_BOOLEAN_TYPE_XOR,VC,FC,J);
      }else if(op_type == "minus"){
        igl::copyleft::cgal::mesh_boolean(VA,FA,VB,FB,igl::MESH_BOOLEAN_TYPE_MINUS,VC,FC,J);
      }else if(op_type == "resolve"){
        igl::copyleft::cgal::mesh_boolean(VA,FA,VB,FB,igl::MESH_BOOLEAN_TYPE_RESOLVE,VC,FC,J);
      }

      // std::cout << "a" << "\n";
      // std::cout << VC.rows() << "\t" << VC.cols() << "\t" << FC.rows() << "\t" << FC.cols() << "\n";

      NumericMatrix VCo(VC.rows(), VC.cols()), FCo(FC.rows(), FC.cols());

      for(int i=0; i < VC.rows(); i++){
        for(int j=0; j< VC.cols(); j++){
          VCo(i,j) = VC(i,j);
        }
      }

      for(int i=0; i < FC.rows(); i++){
        for(int j=0; j< FC.cols(); j++){
          FCo(i,j) = FC(i,j);
        }
      }

    return List::create(Named("VC") = VCo, Named("FC") = FCo);

  }


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/thread/thread.hpp>
#include <tf/transform_listener.h>

ros::Publisher obstacle_pub;
ros::Publisher floor_pub;

pcl::PointCloud<pcl::PointXYZ>::Ptr floor_cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_cloud;

float below_threshold;
tf::TransformListener* listener;

void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
    std::string base_frame("base_link");

    geometry_msgs::PointStamped pout;
    geometry_msgs::PointStamped pin;
    pin.header.frame_id = msg->header.frame_id;
    pin.point.x = 0; pin.point.y = 0; pin.point.z = 0;
    geometry_msgs::Vector3Stamped vout;
    geometry_msgs::Vector3Stamped vin;
    vin.header.frame_id = base_frame;
    vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1;

    float height;
    Eigen::Vector3f normal;
    try {
        listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout);
        height = pout.point.z;
        listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout);
        normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z);
    }
    catch (tf::TransformException ex) {
        ROS_INFO("%s",ex.what());
        return;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);

	Eigen::Vector3f p = -height*normal; // a point in the floor plane
	float d = -p.dot(normal); // = height, d in the plane equation

	obstacle_cloud->points.clear();
	obstacle_cloud->points.reserve(cloud->size());
	floor_cloud->points.clear();
	Eigen::Vector3f temp;
	float floor_dist;
	pcl::PointXYZ point;
	for (size_t i = 0; i < cloud->size(); ++i) {

	    /*temp = cloud->points[i].getVector3fMap(); // DEBUG!

	    if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) {
	        temp -= 0.06*normal;
	    }*/

        // check signed distance to floor
	    floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d;
	    //floor_dist = normal.dot(temp) + d; // DEBUG

	    // if enough below, consider a stair point
	    if (floor_dist < below_threshold) {
	        temp = cloud->points[i].getVector3fMap(); // RELEASE
	        point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11;
	        floor_cloud->points.push_back(point);
	    }
	    else { // add as a normal obstacle or clearing point
	        obstacle_cloud->points.push_back(cloud->points[i]);
	    }
	}

	sensor_msgs::PointCloud2 floor_msg;
    pcl::toROSMsg(*floor_cloud, floor_msg);
	floor_msg.header = msg->header;

	floor_pub.publish(floor_msg);

	sensor_msgs::PointCloud2 obstacle_msg;
    pcl::toROSMsg(*obstacle_cloud, obstacle_msg);
	obstacle_msg.header = msg->header;

	obstacle_pub.publish(obstacle_msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "subsample_cloud");
	ros::NodeHandle n;

	floor_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
	obstacle_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());

	ros::NodeHandle pn("~");
    // topic of input cloud
    if (!pn.hasParam("input")) {
        ROS_ERROR("Could not find parameter input.");
        return -1;
    }
    std::string input;
    pn.getParam("input", input);

    // topic of output cloud
    if (!pn.hasParam("obstacle_output")) {
        ROS_ERROR("Could not find parameter obstacle_output.");
        return -1;
    }
    std::string obstacle_output;
    pn.getParam("obstacle_output", obstacle_output);

    // topic of output cloud
    if (!pn.hasParam("floor_output")) {
        ROS_ERROR("Could not find parameter floor_output.");
        return -1;
    }
    std::string floor_output;
    pn.getParam("floor_output", floor_output);

    double bt;
    pn.param<double>("below_threshold", bt, 0.05);
    below_threshold = -bt;

    listener = new tf::TransformListener();

	ros::Subscriber sub = n.subscribe(input, 1, callback);
    obstacle_pub = n.advertise<sensor_msgs::PointCloud2>(obstacle_output, 1);
    floor_pub = n.advertise<sensor_msgs::PointCloud2>(floor_output, 1);

    ros::spin();

	return 0;
}


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

#include "SurgSim/Particles/PointGenerator.h"

#include <boost/math/special_functions/next.hpp> // For boost::math::float_next()

namespace SurgSim
{

namespace Particles
{
PointGenerator::PointGenerator() :
	m_openOneOneDistribution(boost::math::float_next(-1.0), 1.0),
	m_closedOneOneDistribution(-1.0, boost::math::float_next(1.0)),
	m_closedZeroOneDistribution(0.0, boost::math::float_next(1.0)),
	m_closedZeroOpenOneDistribution(0.0, 1.0)
{
	std::random_device randomDevice;
	m_generator.seed(randomDevice());
}

PointGenerator::~PointGenerator()
{
}

}; // namespace Particles
}; // namespace SurgSim


/**
* Copyright 2019, ftdlyc <yclu.cn@gmail.com>
* Licensed under the MIT license.
*/

#include <algorithm>
#include <functional>
#include <vector>

#include <ceres/ceres.h>
#include <ceres/loss_function.h>

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

#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>

#include "libcalib/calibration.h"
#include "libcalib/ceres_type.h"

namespace calib {

void init_camera_params(const std::vector<std::vector<cv::Point2d>>& world_points,
                        const std::vector<std::vector<cv::Point2d>>& image_points,
                        std::vector<double>& K,
                        std::vector<double>& D,
                        std::vector<std::vector<double>>& Rwc,
                        std::vector<std::vector<double>>& Twc) {
  int num_images = world_points.size();
  int num_points = 0;
  for(int i = 0; i < num_images; ++i) {
    num_points += world_points[i].size();
  }

  std::vector<cv::Mat> homo_vec;
  Eigen::MatrixXd mat_v = Eigen::MatrixXd::Zero(2 * num_images, 6);
  for(int i = 0; i < num_images; ++i) {
    cv::Mat homo = cv::findHomography(world_points[i], image_points[i], cv::RANSAC);
    auto homo_at = [&homo](int x, int y) { return homo.at<double>(y - 1, x - 1); };
    mat_v.block(2 * i, 0, 1, 6) << homo_at(1, 1) * homo_at(2, 1),
        homo_at(1, 1) * homo_at(2, 2) + homo_at(1, 2) * homo_at(2, 1),
        homo_at(1, 2) * homo_at(2, 2),
        homo_at(1, 3) * homo_at(2, 1) + homo_at(1, 1) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(2, 2) + homo_at(1, 2) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(2, 3);
    mat_v.block(2 * i + 1, 0, 1, 6) << homo_at(1, 1) * homo_at(1, 1) - homo_at(2, 1) * homo_at(2, 1),
        homo_at(1, 1) * homo_at(1, 2) + homo_at(1, 2) * homo_at(1, 1) -
            homo_at(2, 1) * homo_at(2, 2) - homo_at(2, 2) * homo_at(2, 1),
        homo_at(1, 2) * homo_at(1, 2) - homo_at(2, 2) * homo_at(2, 2),
        homo_at(1, 3) * homo_at(1, 1) + homo_at(1, 1) * homo_at(1, 3) -
            homo_at(2, 3) * homo_at(2, 1) - homo_at(2, 1) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(1, 2) + homo_at(1, 2) * homo_at(1, 3) -
            homo_at(2, 3) * homo_at(2, 2) - homo_at(2, 2) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(1, 3) - homo_at(2, 3) * homo_at(2, 3);
    homo_vec.emplace_back(homo);
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat_v.transpose() * mat_v, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<double, 6, 6> mat_b = svd.matrixV().cast<double>();
  double b11                        = mat_b(0, 5);
  double b12                        = mat_b(1, 5);
  double b22                        = mat_b(2, 5);
  double b13                        = mat_b(3, 5);
  double b23                        = mat_b(4, 5);
  double b33                        = mat_b(5, 5);
  double cy                         = (b12 * b13 - b11 * b23) / (b11 * b22 - b12 * b12);
  double lambda                     = b33 - (b13 * b13 + cy * (b12 * b13 - b11 * b23)) / b11;
  double fx                         = sqrt(lambda / b11);
  double fy                         = sqrt(lambda * b11 / (b11 * b22 - b12 * b12));
  double radio                      = -b12 * fx * fx * fy / lambda;
  double cx                         = radio * cy / fx - b13 * fx * fx / lambda;
  K[0]                              = fx;
  K[1]                              = fy;
  K[2]                              = 0.;
  K[3]                              = cx;
  K[4]                              = cy;
  Eigen::Matrix3d K_eigen;
  K_eigen << K[0], K[2], K[3],
      0, K[1], K[4],
      0, 0, 1;

  for(int i = 0; i < num_images; ++i) {
    Eigen::Matrix3d rotation_eigen;
    Eigen::Matrix<double, 3, 1> translation_eigen;
    Eigen::Matrix3d homo_eigen;
    cv::cv2eigen(homo_vec[i], homo_eigen);
    double s              = (K_eigen.inverse() * homo_eigen.col(0)).norm();
    rotation_eigen.col(0) = K_eigen.inverse() * homo_eigen.col(0) / s;
    rotation_eigen.col(1) = K_eigen.inverse() * homo_eigen.col(1) / s;
    rotation_eigen.col(2) = rotation_eigen.col(0).cross(rotation_eigen.col(1));
    translation_eigen     = K_eigen.inverse() * homo_eigen.col(2) / s;

    cv::Mat rotation;
    cv::Mat angle_axis;
    cv::eigen2cv(rotation_eigen, rotation);
    cv::Rodrigues(rotation, angle_axis);

    Rwc[i][0] = angle_axis.at<double>(0);
    Rwc[i][1] = angle_axis.at<double>(1);
    Rwc[i][2] = angle_axis.at<double>(2);
    Twc[i][0] = translation_eigen(0, 0);
    Twc[i][1] = translation_eigen(1, 0);
    Twc[i][2] = translation_eigen(2, 0);
  }

  D[0] = 0.;
  D[1] = 0.;
  D[2] = 0.;
  D[3] = 0.;
  D[4] = 0.;
}

double optimize_params(const std::vector<std::vector<cv::Point2d>>& world_points,
                       const std::vector<std::vector<cv::Point2d>>& image_points,
                       std::vector<double>& K,
                       std::vector<double>& D,
                       std::vector<std::vector<double>>& Rwc,
                       std::vector<std::vector<double>>& Twc) {
  std::vector<double> intrinsics = {K[0], K[1], K[3], K[4], D[0], D[1], D[2], D[3], D[4]};
  std::vector<std::vector<double>> se3(world_points.size(), std::vector<double>(6));
  for(int i = 0; i < world_points.size(); ++i) {
    se3[i][0] = Twc[i][0];
    se3[i][1] = Twc[i][1];
    se3[i][2] = Twc[i][2];
    se3[i][3] = Rwc[i][0];
    se3[i][4] = Rwc[i][1];
    se3[i][5] = Rwc[i][2];
  }

  ceres::Problem problem;
  ceres::HuberLoss* loss_fuction = new ceres::HuberLoss(4.5);
  for(int i = 0; i < world_points.size(); ++i) {
    for(int j = 0; j < world_points[i].size(); ++j) {
      problem.AddResidualBlock(new ProjectCostFunction(world_points[i][j], image_points[i][j]), nullptr, intrinsics.data(), se3[i].data());
    }
    problem.SetParameterization(se3[i].data(), new SE3Parameterization());
  }

  ceres::Solver::Options options;
  options.linear_solver_type           = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  K = {intrinsics[0], intrinsics[1], 0., intrinsics[2], intrinsics[3]};
  D = {intrinsics[4], intrinsics[5], intrinsics[6], intrinsics[7], intrinsics[8]};
  for(int i = 0; i < world_points.size(); ++i) {
    Twc[i][0] = se3[i][0];
    Twc[i][1] = se3[i][1];
    Twc[i][2] = se3[i][2];
    Rwc[i][0] = se3[i][3];
    Rwc[i][1] = se3[i][4];
    Rwc[i][2] = se3[i][5];
  }

  double err = sqrt(2 * summary.final_cost / summary.num_residual_blocks);
  return err;
}

double calibrate_camera(const std::vector<std::vector<cv::Point2d>>& world_points,
                        const std::vector<std::vector<cv::Point2d>>& image_points,
                        std::vector<double>& K,
                        std::vector<double>& D,
                        std::vector<std::vector<double>>& Rwc,
                        std::vector<std::vector<double>>& Twc) {
  init_camera_params(world_points, image_points, K, D, Rwc, Twc);
  double err = optimize_params(world_points, image_points, K, D, Rwc, Twc);
  return err;
}

void init_stereo_params(std::vector<std::vector<double>>& Rwc1,
                        std::vector<std::vector<double>>& Rwc2,
                        std::vector<std::vector<double>>& Twc1,
                        std::vector<std::vector<double>>& Twc2,
                        std::vector<double>& R,
                        std::vector<double>& T) {
  auto num_images       = static_cast<int>(Rwc1.size());
  Eigen::MatrixXd mat_a = Eigen::MatrixXd::Zero(9 * num_images, 9);
  Eigen::MatrixXd mat_b = Eigen::MatrixXd::Zero(9 * num_images, 1);
  for(int i = 0; i < num_images; ++i) {
    Eigen::Matrix3d r1_t, r2;
    ceres::AngleAxisToRotationMatrix(Rwc1[i].data(), r1_t.data());
    ceres::AngleAxisToRotationMatrix(Rwc2[i].data(), r2.data());
    r1_t.transposeInPlace();
    mat_a.block(9 * i, 0, 3, 3)     = r1_t;
    mat_a.block(9 * i + 3, 3, 3, 3) = r1_t;
    mat_a.block(9 * i + 6, 6, 3, 3) = r1_t;
    mat_b.block(9 * i, 0, 9, 1) << r2(0, 0), r2(0, 1), r2(0, 2),
        r2(1, 0), r2(1, 1), r2(1, 2),
        r2(2, 0), r2(2, 1), r2(2, 2);
  }
  Eigen::MatrixXd r = (mat_a.transpose() * mat_a).inverse() * mat_a.transpose() * mat_b;
  r.resize(3, 3);
  r.transposeInPlace();

  T[0] = T[1] = T[2] = 0.;
  for(int i = 0; i < num_images; ++i) {
    Eigen::Matrix<double, 3, 1> t, t1(Twc1[i].data()), t2(Twc2[i].data());
    t = t2 - r * t1;
    T[0] += t(0, 0);
    T[1] += t(1, 0);
    T[2] += t(2, 0);
  }
  T[0] /= num_images;
  T[1] /= num_images;
  T[2] /= num_images;
  ceres::RotationMatrixToAngleAxis(r.data(), R.data());
}

double optimize_stereo_params(const std::vector<std::vector<cv::Point2d>>& world_points,
                              const std::vector<std::vector<cv::Point2d>>& image_points_1,
                              const std::vector<std::vector<cv::Point2d>>& image_points_2,
                              std::vector<double>& K1,
                              std::vector<double>& K2,
                              std::vector<double>& D1,
                              std::vector<double>& D2,
                              std::vector<std::vector<double>>& Rwc1,
                              std::vector<std::vector<double>>& Rwc2,
                              std::vector<std::vector<double>>& Twc1,
                              std::vector<std::vector<double>>& Twc2,
                              std::vector<double>& R,
                              std::vector<double>& T) {
  std::vector<double> intrinsics_1 = {K1[0], K1[1], K1[3], K1[4], D1[0], D1[1], D1[2], D1[3], D1[4]};
  std::vector<double> intrinsics_2 = {K2[0], K2[1], K2[3], K2[4], D2[0], D2[1], D2[2], D2[3], D2[4]};
  std::vector<std::vector<double>> se3_1(world_points.size(), std::vector<double>(6));
  std::vector<double> se3 = {T[0], T[1], T[2], R[0], R[1], R[2]};
  for(int i = 0; i < world_points.size(); ++i) {
    se3_1[i][0] = Twc1[i][0];
    se3_1[i][1] = Twc1[i][1];
    se3_1[i][2] = Twc1[i][2];
    se3_1[i][3] = Rwc1[i][0];
    se3_1[i][4] = Rwc1[i][1];
    se3_1[i][5] = Rwc1[i][2];
  }

  ceres::Problem problem;
  ceres::HuberLoss* loss_fuction = new ceres::HuberLoss(4.5);
  for(int i = 0; i < world_points.size(); ++i) {
    for(int j = 0; j < world_points[i].size(); ++j) {
      problem.AddResidualBlock(new StereoCostFunction(world_points[i][j], image_points_1[i][j], image_points_2[i][j]), nullptr,
                               intrinsics_1.data(), intrinsics_2.data(), se3_1[i].data(), se3.data());
    }
    problem.SetParameterization(se3_1[i].data(), new SE3Parameterization());
  }
  problem.SetParameterization(se3.data(), new SE3Parameterization());

  ceres::Solver::Options options;
  options.linear_solver_type           = ceres::DENSE_QR;
  options.trust_region_strategy_type   = ceres::DOGLEG;
  options.minimizer_progress_to_stdout = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  K1 = {intrinsics_1[0], intrinsics_1[1], 0., intrinsics_1[2], intrinsics_1[3]};
  D1 = {intrinsics_1[4], intrinsics_1[5], intrinsics_1[6], intrinsics_1[7], intrinsics_1[8]};
  K2 = {intrinsics_2[0], intrinsics_2[1], 0., intrinsics_2[2], intrinsics_2[3]};
  D2 = {intrinsics_2[4], intrinsics_2[5], intrinsics_2[6], intrinsics_2[7], intrinsics_2[8]};
  R  = {se3[3], se3[4], se3[5]};
  T  = {se3[0], se3[1], se3[2]};

  double q[4], q1[4], q2[4];
  ceres::AngleAxisToQuaternion(R.data(), q);
  for(int i = 0; i < world_points.size(); ++i) {
    Twc1[i][0] = se3_1[i][0];
    Twc1[i][1] = se3_1[i][1];
    Twc1[i][2] = se3_1[i][2];
    Rwc1[i][0] = se3_1[i][3];
    Rwc1[i][1] = se3_1[i][4];
    Rwc1[i][2] = se3_1[i][5];

    ceres::AngleAxisToQuaternion(Rwc1[i].data(), q1);
    ceres::QuaternionProduct(q, q1, q2);
    ceres::QuaternionToAngleAxis(q2, Rwc2[i].data());
    ceres::AngleAxisRotatePoint(R.data(), Twc1[i].data(), Twc2[i].data());
    Twc2[i][0] += T[0];
    Twc2[i][1] += T[1];
    Twc2[i][2] += T[2];
  }

  double err = sqrt(2 * summary.final_cost / summary.num_residual_blocks / 2);
  return err;
}

double calibrate_stereo_camera(const std::vector<std::vector<cv::Point2d>>& world_points,
                               const std::vector<std::vector<cv::Point2d>>& image_points_1,
                               const std::vector<std::vector<cv::Point2d>>& image_points_2,
                               std::vector<double>& K1,
                               std::vector<double>& K2,
                               std::vector<double>& D1,
                               std::vector<double>& D2,
                               std::vector<std::vector<double>>& Rwc1,
                               std::vector<std::vector<double>>& Rwc2,
                               std::vector<std::vector<double>>& Twc1,
                               std::vector<std::vector<double>>& Twc2,
                               std::vector<double>& R,
                               std::vector<double>& T) {
  init_stereo_params(Rwc1, Rwc2, Twc1, Twc2, R, T);
  double err = optimize_stereo_params(world_points, image_points_1, image_points_2, K1, K2, D1, D2, Rwc1, Rwc2, Twc1, Twc2, R, T);
  return err;
}

} // namespace calib


#pragma once

#include <boost/integer.hpp>
#include <boost/core/ignore_unused.hpp>

#include <ratio>
#include <tuple>
#include <type_traits>

namespace ompu {
namespace geo {

template<
    class XRatio,
    class YRatio
>
class RatioPoint
{
public:
    using x_ratio = XRatio;
    using y_ratio = YRatio;
};

template<
    class WRatio,
    class HRatio
>
class RatioSize
{
public:
    using w_ratio = WRatio;
    using h_ratio = HRatio;
};

template<
    class Point,
    class Size
>
class RatioRect
{
public:
    using point_ratio = Point;
    using x_ratio = typename Point::x_ratio;
    using y_ratio = typename Point::y_ratio;

    using size_ratio = Size;
    using w_ratio = typename Size::w_ratio;
    using h_ratio = typename Size::h_ratio;
};

template<class T, class R>
inline constexpr T
static_ratio(T v) noexcept
{
    return v * (T(R::num) / R::den);
}

template<
    class XRatioPerStep,
    class YRatioPerStep,
    class WRatioPerStep,
    class HRatioPerStep
>
class RatioCalculator
{
public:
#if 0
    template<class Pos, class T, class StepT>
    static constexpr Pos
    pos_for_step(T x, T y, T w, T h, StepT step_x, StepT step_y = step_x) noexcept
    {
        return Pos{
            x + static_ratio<XRatioPerStep, T>(w) * step_x,
            y + static_ratio<YRatioPerStep, T>(h) * step_y
        };
    }

    template<class Size, class T, class StepT>
    static constexpr Size
    size_for_step(T, T, T w, T h, StepT step_w, StepT step_h = step_w) noexcept
    {
        return Size{
            static_ratio<WRatioPerStep, T>(w) * step_w,
            static_ratio<HRatioPerStep, T>(h) * step_h
        };
    }
#endif

    template<class Rect, class T>
    static constexpr Rect
    rect_for_step(T x, T y, T w, T h) noexcept
    {
        return Rect{
            x + static_ratio<T, XRatioPerStep>(w),
            y + static_ratio<T, YRatioPerStep>(h),
            static_ratio<T, WRatioPerStep>(w),
            static_ratio<T, HRatioPerStep>(h)
        };
    }
};

}
} // ompu


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      lambertTargeterIzzo.h/.cpp source files, tudat revision 455/466.
 *      PyKEP kepler toolbox, Dario Izzo, ESA Advanced Concepts Team.
 *
 *    Notes
 *      This is a new implementation of the lambertTargeterIzzo class, for better adaptability and
 *      extension towards subclasses and future improvements/additions. Therefore, it replaces the
 *      lambertTargeterIzzo class while still providing the same functionality.
 *
 */

#include <cmath>

#include <boost/math/special_functions.hpp> // For asinh and acosh
#include <boost/exception/all.hpp> // For exceptions in sanity checks
#include <Eigen/Dense> // for cross product issues (can someone explain why, exactly?)

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/MissionSegments/zeroRevolutionLambertTargeterIzzo.h"
#include "Tudat/Mathematics/BasicMathematics/convergenceException.h"

namespace tudat
{
namespace mission_segments
{

//! Get radial velocity at departure.
double ZeroRevolutionLambertTargeterIzzo::getRadialVelocityAtDeparture( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get transverse velocity at departure.
double ZeroRevolutionLambertTargeterIzzo::getTransverseVelocityAtDeparture( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
            angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double ZeroRevolutionLambertTargeterIzzo::getRadialVelocityAtArrival( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at arrival.
double ZeroRevolutionLambertTargeterIzzo::getTransverseVelocityAtArrival( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival
            = angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double ZeroRevolutionLambertTargeterIzzo::getSemiMajorAxis( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

// What about get retrograde flag, tolerance and max number of iterations? Might be useful to check.

//! Execute the solving procedure.
void ZeroRevolutionLambertTargeterIzzo::execute( )
{
    // Sanity checks
    sanityCheckTimeOfFlight( );
    sanityCheckGravitationalParameter( );

    // Transform dimensional parameters to dimensionless parameters if not done already (e.g. in
    // multirevolution case
    if( !transformed ) transformDimensions( );

    // Solve root (single rev)
    double xResult = ZeroRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( );

    // Reconstruct Vs
    computeVelocities( xResult );

    solved = true;
}

//! Sanity check time of flight.
void ZeroRevolutionLambertTargeterIzzo::sanityCheckTimeOfFlight( )
{
    // If time of flight is negative, throw an exception
    if ( timeOfFlight < 0 )
    {
        // Throw exception.
        throw std::runtime_error(
                    "Time-of-flight specified in Lambert problem must be strictly positive. Specified time-of-flight in days." +
                                         std::to_string( timeOfFlight ) );
    }
    // Else, do nothing and continue.
}

//! Sanity check gravitational parameter.
void ZeroRevolutionLambertTargeterIzzo::sanityCheckGravitationalParameter( )
{
    // If gravitational parameter is negative, throw an exception
    if ( gravitationalParameter < 0 )
    {
        // Throw exception.
        throw std::runtime_error(
                    "Gravitational parameter specified in Lambert problem must be strictly positive. Specified gravitational parameter: " +
                    std::to_string( gravitationalParameter ) );
    }
    // Else, do nothing and continue.
}

//! Transform input to sub results in adimensional units.
void ZeroRevolutionLambertTargeterIzzo::transformDimensions( )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Compute normalizing values.
    const double distanceNormalizingValue = cartesianPositionAtDeparture.norm( );
    velocityNormalizingValue = std::sqrt( gravitationalParameter /
                                          distanceNormalizingValue );
    const double timeNormalizingValue = distanceNormalizingValue / velocityNormalizingValue;

    // Compute transfer geometry parameters in adimensional units.
    // Time of Flight.
    normalizedTimeOfFlight = timeOfFlight / timeNormalizingValue;

    // Cosine of transfer angle.
    const double cosineOfTransferAngle =
            cartesianPositionAtDeparture.dot( cartesianPositionAtArrival )
            / (distanceNormalizingValue * cartesianPositionAtArrival.norm( ) );

    // Normalized Cartesian position at arrival.
    normalizedRadiusAtArrival = cartesianPositionAtArrival.norm( ) / distanceNormalizingValue;

    // Chord.
    normalizedChord = std::sqrt( 1.0 + normalizedRadiusAtArrival
                                 * ( normalizedRadiusAtArrival
                                     - 2.0 * cosineOfTransferAngle ) );

    // Semi-perimeter.
    normalizedSemiPerimeter = ( 1.0 + normalizedRadiusAtArrival + normalizedChord ) / 2.0;

    // Assuming a prograde motion, determine whether the transfer corresponds to the long- or the
    // short-way solution: longway if x1*y2 - x2*y1 < 0.
    isLongway = false;
    if ( cartesianPositionAtDeparture.x( ) * cartesianPositionAtArrival.y( )
         - cartesianPositionAtDeparture.y( ) * cartesianPositionAtArrival.x( ) < 0.0 )
    {
        isLongway = true;
    }

    // If retrograde is true, switch longway flag.
    if ( isRetrograde )
    {
        isLongway = !isLongway;
    }

    // Semi-major axis of the minimum energy ellipse.
    normalizedMinimumEnergySemiMajorAxis = normalizedSemiPerimeter / 2.0;

    // Transfer angle.
    transferAngle = std::acos( cosineOfTransferAngle );
    if ( isLongway )
    {
        transferAngle = 2.0 * mathematical_constants::PI - transferAngle;
    }

    // Lambda parameter.
    lambdaParameter = std::sqrt( normalizedRadiusAtArrival )
            * std::cos( transferAngle / 2.0 ) / normalizedSemiPerimeter;

    // Set transformed flag to true, as the dimension transformation has been performed
    transformed = true;
}

//! Compute time-of-flight using Lagrange's equation.
double ZeroRevolutionLambertTargeterIzzo::computeTimeOfFlight( const double xParameter )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Determine semi-major axis.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;
        // If long transfer arc
        if ( isLongway )
        {
            betaParameter = -2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange including multiple revolutions.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) ) );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;
        // If long transfer arc
        if ( isLongway )
        {
            betaParameter = -2.0 * boost::math::asinh ( std::sqrt( ( normalizedSemiPerimeter
                                                                     - normalizedChord )
                                                                   / ( -2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * boost::math::asinh ( std::sqrt( ( normalizedSemiPerimeter
                                                                    - normalizedChord )
                                                                  / ( -2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlightLagrange = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlightLagrange;
    }
}

//! Solve the time of flight equation for x.
double ZeroRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Find root (secant method, currently hard coded)
    // Optimize log(t_spec).
    const double logarithmOfTheSpecifiedTimeOfFlight = std::log( normalizedTimeOfFlight );

    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1 = std::log( 0.5 ), x2 = std::log( 1.5 );

    double y1 = std::log( computeTimeOfFlight( -0.5 ) ) - logarithmOfTheSpecifiedTimeOfFlight;

    double y2 = std::log( computeTimeOfFlight( 0.5 ) ) - logarithmOfTheSpecifiedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && (y1 != y2)
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = std::log( computeTimeOfFlight( std::exp( xNew ) - 1.0 ) )
                - logarithmOfTheSpecifiedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        throw std::runtime_error(
                    "Multi-Revolution Lambert targeter failed to converge to a solution. Reached the maximum number of iterations: " +
                    std::to_string( maximumNumberOfIterations ) );
    }

    // Recovering x parameter and returning it.
    double xParameter = std::exp( xNew ) - 1.0;
    return xParameter;
}

//! Compute velocities at departure and arrival.
void ZeroRevolutionLambertTargeterIzzo::computeVelocities( const double xParameter )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Then it is possible to retrieve a sensible value from the x-parameter computed)
    // Determine semi-major axis of the conic.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // Declare variables.
    double etaParameter, etaParameterSquared, psiParameter;

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter = 2.0 * std::asin( std::sqrt( ( normalizedSemiPerimeter
                                                             - normalizedChord )
                                                           / ( 2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter in Izzo's approach (no explanation available).
        psiParameter = ( alphaParameter - betaParameter ) / 2.0;

        // Eta parameter in Izzo's approach (no explanation available).
        etaParameterSquared = 2.0 * semiMajorAxis * std::sin( psiParameter )
                * std::sin( psiParameter ) / normalizedSemiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter = 2.0 * boost::math::asinh (
                    std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                               / ( -2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter in Izzo's approach (no explanation available).
        psiParameter = (alphaParameter - betaParameter ) / 2.0;

        // Eta parameter in Izzo's approach (no explanation available).
        etaParameterSquared = -2.0 * semiMajorAxis * std::sinh( psiParameter )
                * std::sinh( psiParameter ) / normalizedSemiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Determine semi-latus rectum, p.
    const double semiLatusRectum = ( normalizedRadiusAtArrival
                                     / ( normalizedMinimumEnergySemiMajorAxis
                                         * etaParameterSquared ) )
            * std::sin( transferAngle / 2.0 )
            * std::sin( transferAngle / 2.0 );

    // Velocity components at departure.
    const double radialVelocityAtDeparture =
            ( 1.0 / ( etaParameter * std::sqrt( normalizedMinimumEnergySemiMajorAxis ) ) )
            * ( 2.0 * lambdaParameter * normalizedMinimumEnergySemiMajorAxis
                - ( lambdaParameter + xParameter * etaParameter ) );

    const double transverseVelocityAtDeparture = std::sqrt( semiLatusRectum );

    // Velocity components at arrival.
    const double transverseVelocityAtArrival = transverseVelocityAtDeparture
            / normalizedRadiusAtArrival;
    const double radialVelocityAtArrival = ( transverseVelocityAtDeparture
                                             - transverseVelocityAtArrival )
            / std::tan( transferAngle / 2.0 )
            - radialVelocityAtDeparture;

    // Determining inertial vectors
    // Determine radial unit vectors.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Determine plane of motion.
    Eigen::Vector3d angularMomentumVector;

    if ( isLongway )
    {
        angularMomentumVector = radialUnitVectorAtArrival.cross( radialUnitVectorAtDeparture );
    }
    else
    {
        angularMomentumVector = radialUnitVectorAtDeparture.cross( radialUnitVectorAtArrival );
    }

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Compute transverse unit vectors.
    const Eigen::Vector3d transverseUnitVectorAtDeparture =
            radialUnitVectorAtDeparture.cross( angularMomentumUnitVector );
    const Eigen::Vector3d transverseUnitVectorAtArrival =
            radialUnitVectorAtArrival.cross( angularMomentumUnitVector );

    // Reconstruct non-dimensional velocity vectors.
    cartesianVelocityAtDeparture << radialVelocityAtDeparture * radialUnitVectorAtDeparture
                                    - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture;

    cartesianVelocityAtArrival << radialVelocityAtArrival * radialUnitVectorAtArrival
                                  - transverseVelocityAtArrival * transverseUnitVectorAtArrival;

    // Return to dimensions of initial problem definition.
    cartesianVelocityAtDeparture *= velocityNormalizingValue;
    cartesianVelocityAtArrival *= velocityNormalizingValue;
}

} // namespace mission_segments
} // namespace tudat


// Copyright (c) 2015-2020 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "allele_frequency_bias.hpp"

#include <algorithm>
#include <iterator>
#include <numeric>
#include <cmath>

#include <boost/variant.hpp>

#include "core/types/allele.hpp"
#include "io/variant/vcf_record.hpp"
#include "utils/concat.hpp"
#include "../facets/samples.hpp"
#include "alt_allele_count.hpp"
#include "allele_frequency.hpp"

namespace octopus { namespace csr {

const std::string AlleleFrequencyBias::name_ = "AFB";

std::unique_ptr<Measure> AlleleFrequencyBias::do_clone() const
{
    return std::make_unique<AlleleFrequencyBias>(*this);
}

Measure::ValueType AlleleFrequencyBias::get_value_type() const
{
    return double {};
}

namespace {

auto sum_alt_allele_counts(const Measure::Array<Measure::ValueType>& counts)
{
    return std::accumulate(std::cbegin(counts), std::cend(counts), 0u, [] (auto total, auto count) { return total + boost::get<int>(count); });
}

} // namespace

Measure::ResultType AlleleFrequencyBias::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto alt_allele_counts = boost::get<Array<Array<ValueType>>>(AltAlleleCount{}.evaluate(call, facets));
    const auto allele_frequencies = boost::get<Array<Array<Optional<ValueType>>>>(AlleleFrequency{}.evaluate(call, facets));
    const auto num_alleles = 1 + call.alt().size();
    const auto num_samples = samples.size();
    assert(allele_frequencies.size() == num_samples);
    assert(alt_allele_counts.size() == num_samples);
    Array<Array<Optional<ValueType>>> result(num_samples, Array<Optional<ValueType>>(num_alleles));
    for (std::size_t s {0}; s < num_samples; ++s) {
        assert(alt_allele_counts[s].size() == num_alleles - 1);
        assert(allele_frequencies[s].size() == num_alleles);
        const auto ploidy = call.genotype(samples[s]).size();
        const auto non_ref_count = sum_alt_allele_counts(alt_allele_counts[s]);
        assert(ploidy >= non_ref_count);
        const auto ref_count = ploidy - non_ref_count;
        for (std::size_t a {0}; a < num_alleles; ++a) {
            if (allele_frequencies[s][a]) {
                const auto expected_frequency = static_cast<double>(a == 0 ? ref_count : boost::get<int>(alt_allele_counts[s][a - 1])) / ploidy;
                const auto emperical_frequency = boost::get<double>(*allele_frequencies[s][a]);
                result[s][a] = std::abs(expected_frequency - emperical_frequency);
            }
        }
    }
    return result;
}

Measure::ResultCardinality AlleleFrequencyBias::do_cardinality() const noexcept
{
    return ResultCardinality::samples_and_alleles;
}

const std::string& AlleleFrequencyBias::do_name() const
{
    return name_;
}

std::string AlleleFrequencyBias::do_describe() const
{
    return "Absolute difference between empirical allele frequency (AF) and expected allele frequency given genotype";
}

std::vector<std::string> AlleleFrequencyBias::do_requirements() const
{
    auto result = concat(AltAlleleCount{}.requirements(), AlleleFrequency{}.requirements());
    result.push_back("Samples");
    return result;
}

boost::optional<Measure::Aggregator> AlleleFrequencyBias::do_aggregator() const noexcept
{
    return Measure::Aggregator::min_tail;
}

} // namespace csr
} // namespace octopus


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP

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

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
\author See http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename Point1,
    typename Point2 = Point1,
    typename CalculationType = void
>
class vincenty
{
public :
    typedef typename promote_floating_point
        <
            typename select_most_precise
                <
                    typename select_calculation_type
                        <
                            Point1,
                            Point2,
                            CalculationType
                        >::type,
                    double // to avoid bad results in float
                >::type
        >::type calculation_type;

    inline vincenty()
    {}

    explicit inline vincenty(geometry::detail::ellipsoid<calculation_type> const& e)
        : m_ellipsoid(e)
    {}

    inline calculation_type apply(Point1 const& p1, Point2 const& p2) const
    {
        return calculate(get_as_radian<0>(p1), get_as_radian<1>(p1),
                        get_as_radian<0>(p2), get_as_radian<1>(p2));
    }

    inline geometry::detail::ellipsoid<calculation_type> ellipsoid() const
    {
        return m_ellipsoid;
    }

private :
    geometry::detail::ellipsoid<calculation_type> m_ellipsoid;

    inline calculation_type calculate(calculation_type const& lon1,
                calculation_type const& lat1,
                calculation_type const& lon2,
                calculation_type const& lat2) const
    {
        calculation_type const c2 = 2;
        calculation_type const pi = geometry::math::pi<calculation_type>();
        calculation_type const two_pi = c2 * pi;

        // lambda: difference in longitude on an auxiliary sphere
        calculation_type L = lon2 - lon1;
        calculation_type lambda = L;

        if (L < -pi) L += two_pi;
        if (L > pi) L -= two_pi;

        if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
        {
            return calculation_type(0);
        }

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        calculation_type const c1 = 1;
        calculation_type const one_min_f = c1 - m_ellipsoid.f();

        calculation_type const U1 = atan(one_min_f * tan(lat1)); // above (1)
        calculation_type const U2 = atan(one_min_f * tan(lat2)); // above (1)

        calculation_type const cos_U1 = cos(U1);
        calculation_type const cos_U2 = cos(U2);
        calculation_type const sin_U1 = sin(U1);
        calculation_type const sin_U2 = sin(U2);

        // alpha: azimuth of the geodesic at the equator
        calculation_type cos2_alpha;
        calculation_type sin_alpha;

        // sigma: angular distance p1,p2 on the sphere
        // sigma1: angular distance on the sphere from the equator to p1
        // sigma_m: angular distance on the sphere from the equator to the midpoint of the line
        calculation_type sigma;
        calculation_type sin_sigma;
        calculation_type cos2_sigma_m;

        calculation_type previous_lambda;

        calculation_type const c3 = 3;
        calculation_type const c4 = 4;
        calculation_type const c6 = 6;
        calculation_type const c16 = 16;

        calculation_type const c_e_12 = 1e-12;

        do
        {
            previous_lambda = lambda; // (13)
            calculation_type sin_lambda = sin(lambda);
            calculation_type cos_lambda = cos(lambda);
            sin_sigma = sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
            calculation_type cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
            sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
            cos2_alpha = c1 - math::sqr(sin_alpha);
            cos2_sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)

            calculation_type C = m_ellipsoid.f()/c16 * cos2_alpha * (c4 + m_ellipsoid.f() * (c4 - c3 * cos2_alpha)); // (10)
            sigma = atan2(sin_sigma, cos_sigma); // (16)
            lambda = L + (c1 - C) * m_ellipsoid.f() * sin_alpha *
                (sigma + C * sin_sigma * ( cos2_sigma_m + C * cos_sigma * (-c1 + c2 * math::sqr(cos2_sigma_m)))); // (11)

        } while (geometry::math::abs(previous_lambda - lambda) > c_e_12
                && geometry::math::abs(lambda) < pi);

        calculation_type sqr_u = cos2_alpha * (math::sqr(m_ellipsoid.a()) - math::sqr(m_ellipsoid.b())) / math::sqr(m_ellipsoid.b()); // above (1)

        // Oops getting hard here
        // (again, problem is that ttmath cannot divide by doubles, which is OK)
        calculation_type const c47 = 47;
        calculation_type const c74 = 74;
        calculation_type const c128 = 128;
        calculation_type const c256 = 256;
        calculation_type const c175 = 175;
        calculation_type const c320 = 320;
        calculation_type const c768 = 768;
        calculation_type const c1024 = 1024;
        calculation_type const c4096 = 4096;
        calculation_type const c16384 = 16384;

        calculation_type A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
        calculation_type B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
        calculation_type delta_sigma = B * sin_sigma * ( cos2_sigma_m + (B/c4) * (cos(sigma)* (-c1 + c2 * cos2_sigma_m)
                - (B/c6) * cos2_sigma_m * (-c3 + c4 * math::sqr(sin_sigma)) * (-c3 + c4 * cos2_sigma_m))); // (6)

        return m_ellipsoid.b() * A * (sigma - delta_sigma); // (19)
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Point1, typename Point2>
struct tag<strategy::distance::vincenty<Point1, Point2> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Point1, typename Point2>
struct return_type<strategy::distance::vincenty<Point1, Point2> >
{
    typedef typename strategy::distance::vincenty<Point1, Point2>::calculation_type type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct similar_type<vincenty<Point1, Point2>, P1, P2>
{
    typedef vincenty<P1, P2> type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct get_similar<vincenty<Point1, Point2>, P1, P2>
{
    static inline vincenty<P1, P2> apply(vincenty<Point1, Point2> const& input)
    {
        return vincenty<P1, P2>(input.ellipsoid());
    }
};

template <typename Point1, typename Point2>
struct comparable_type<vincenty<Point1, Point2> >
{
    typedef vincenty<Point1, Point2> type;
};

template <typename Point1, typename Point2>
struct get_comparable<vincenty<Point1, Point2> >
{
    static inline vincenty<Point1, Point2> apply(vincenty<Point1, Point2> const& input)
    {
        return input;
    }
};

template <typename Point1, typename Point2>
struct result_from_distance<vincenty<Point1, Point2> >
{
    template <typename T>
    static inline typename return_type<vincenty<Point1, Point2> >::type apply(vincenty<Point1, Point2> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SINHC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SINHC_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/sinhc_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/if_else.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinhc_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {

    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
      // respectively rational(double) approx inspired from cephes sinh approx.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is ((e-rec(e))/2)/x
      // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (x == Inf<A0>()) return x;
      #endif
      if( x < One<A0>())
      {
        return detail::sinhc_kernel<A0>::compute(sqr(x));
      }
      else
      {
        auto test1 = (x >  Maxlog<A0>()-Log_2<A0>());
        A0 fac = if_else(test1, Half<A0>(), One<A0>());
        A0 tmp = exp(x*fac);
        A0 tmp1 = (Half<A0>()*tmp)/x;
        return if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
      }
     }
  };
} } }

#endif


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/sqrt_2o_3.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check sqrt_2o_3 behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrt_2o_3;
  using boost::simd::Sqrt_2o_3;

  STF_TYPE_IS(decltype(Sqrt_2o_3<T>()), T);
  STF_EQUAL(Sqrt_2o_3<T>(), T(0));
  STF_EQUAL(sqrt_2o_3( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check sqrt_2o_3 behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::sqrt_2o_3;
  using boost::simd::Sqrt_2o_3;
  using boost::simd::Ratio;

  STF_TYPE_IS(decltype(Sqrt_2o_3<T>()), T);

  auto z1 = Sqrt_2o_3<T>();
  STF_ULP_EQUAL(z1*z1, (Ratio<T,2,3>()), 0.5);

  auto z2 = sqrt_2o_3( as(T{}) );
  STF_ULP_EQUAL(z2*z2, (Ratio<T,2,3>()), 0.5);

}


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/Frustum.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathFrustumTestSuite);

BOOST_AUTO_TEST_CASE(FrustumCanTestsPointContained) {
	{
		const auto f = Frustum(Handedness::RIGHT, 1.0f, 0.5f, 0.1f, 100.0f);

		BOOST_CHECK(f.contains({ 0.0f, 0.0f, -50.0f }));

		BOOST_CHECK(f.contains({ 0.0f, 0.0f, -0.1f }));
		BOOST_CHECK(f.contains({ -1.0f, 0.0f, -1.0f }));
		BOOST_CHECK(f.contains({ 1.0f, 0.0f, -1.0f }));
		BOOST_CHECK(f.contains({ 0.0f, 0.0f, -100.0f }));

		BOOST_CHECK(!f.contains({ 0.0f, 0.0f, -0.09f }));
		BOOST_CHECK(!f.contains({ -1.1f, 0.0f, -1.0f }));
		BOOST_CHECK(!f.contains({ 1.1f, 0.0f, -1.0f }));
		BOOST_CHECK(!f.contains({ 0.0f, 0.0f, -100.1f }));
	}

	{
		const auto f = Frustum(Handedness::LEFT, 1.0f, 0.5f, 0.1f, 100.0f);

		BOOST_CHECK(f.contains({ 0.0f, 0.0f, 50.0f }));

		BOOST_CHECK(f.contains({ 0.0f, 0.0f, 0.1f }));
		BOOST_CHECK(f.contains({ -1.0f, 0.0f, 1.0f }));
		BOOST_CHECK(f.contains({ 1.0f, 0.0f, 1.0f }));
		BOOST_CHECK(f.contains({ 0.0f, 0.0f, 100.0f }));

		BOOST_CHECK(!f.contains({ 0.0f, 0.0f, 0.09f }));
		BOOST_CHECK(!f.contains({ -1.1f, 0.0f, 1.0f }));
		BOOST_CHECK(!f.contains({ 1.1f, 0.0f, 1.0f }));
		BOOST_CHECK(!f.contains({ 0.0f, 0.0f, 100.1f }));
	}
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathFrustumTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // anonymous namespace


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::oneminus(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_EQUAL(bs::oneminus(aa1), bb);
}

STF_CASE_TPL("Check oneminus saturated on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void tests(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::saturated_(bs::oneminus)(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_EQUAL(bs::saturated_(bs::oneminus)(aa1), bb);
}

STF_CASE_TPL("Check oneminus on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  tests<T, N>(runtime);
  tests<T, N/2>(runtime);
  tests<T, N*2>(runtime);
}

STF_CASE_TPL (" oneminus signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::oneminus;
  using p_t = bs::pack<T>;
  using r_t = decltype(oneminus(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(oneminus(bs::Mone<p_t>()), bs::Two<p_t>());
  STF_EQUAL(oneminus(bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(oneminus(bs::Valmax<p_t>()), bs::Valmin<p_t>()+bs::Two<p_t>());
  STF_EQUAL(oneminus(bs::Zero<p_t>()), bs::One<p_t>());
}

STF_CASE_TPL (" oneminus unsigned_uint",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::oneminus;
  using p_t = bs::pack<T>;
  using r_t = decltype(oneminus(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(oneminus(bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(oneminus(bs::Two<p_t>()), bs::Valmax<p_t>());
  STF_EQUAL(oneminus(bs::Zero<p_t>()), bs::One<p_t>());
}

STF_CASE_TPL(" oneminus floating", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::oneminus;
  using p_t = bs::pack<T>;
  using r_t = decltype(oneminus(p_t()));

  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(oneminus(bs::Inf<p_t>()), bs::Minf<p_t>());
  STF_IEEE_EQUAL(oneminus(bs::Nan<p_t>()), bs::Nan<p_t>());
  STF_EQUAL(oneminus(bs::Minf<p_t>()), bs::Inf<p_t>());
#endif
  STF_EQUAL(oneminus(bs::One<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(oneminus(bs::Two<p_t>()), bs::Mone<p_t>());
  STF_EQUAL(oneminus(bs::Zero<p_t>()), bs::One<p_t>());
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(const char* name, Matrix& A)
{
    std::cout << name << "\n";

    mtl::mat::inserter<Matrix> ins(A, 3);
    ins[0][1] << 4.; ins[0][2] << 7;
    // ins[0][3] << 6.; ins[0][0] << 8;

    ins.print(0);
    ins.print(1);
    // ins.make_empty(0);
    ins.print();
}

int main(int, char**)
{
    mtl::compressed2D<double> A(4, 5);
    // mtl::compressed2D<double> A(4, 5);

    test("CRS", A);

    return 0;
}


//
// Created by Hamza El-Kebir on 5/8/21.
//

#ifndef LODESTAR_ORDINARYDIFFERENTIALEQUATION_HPP
#define LODESTAR_ORDINARYDIFFERENTIALEQUATION_HPP

#ifdef LS_USE_GINAC

#include <Eigen/Dense>
#include "ginac/ginac.h"

#include "Lodestar/systems/StateSpace.hpp"

#include <string>
#include <deque>

namespace ls {
    namespace symbolic {
        class OrdinaryDifferentialEquation {
        public:
            OrdinaryDifferentialEquation() :
                    functions_(GiNaC::lst{GiNaC::ex(0)}),
                    states_(GiNaC::lst{GiNaC::symbol("x")}),
                    inputs_(GiNaC::lst{GiNaC::symbol("u")}),
                    time_(GiNaC::symbol("t"))
            {
                makeSymbolMap();
            }

            OrdinaryDifferentialEquation(const GiNaC::lst &functions,
                                         const GiNaC::lst &states,
                                         const GiNaC::lst &inputs) :
                    functions_(functions),
                    states_(states),
                    inputs_(inputs),
                    time_(GiNaC::symbol("t"))
            {
                makeSymbolMap();
            }

            OrdinaryDifferentialEquation(const GiNaC::lst &functions,
                                         const GiNaC::lst &states,
                                         const GiNaC::lst &inputs,
                                         const GiNaC::symbol &time) :
                    functions_(functions),
                    states_(states),
                    inputs_(inputs),
                    time_(time)
            {
                makeSymbolMap();
            }

            GiNaC::exmap generateExpressionMap() const;

            GiNaC::exmap generateExpressionMap(
                    const std::vector<GiNaC::relational> &relationals) const;

            GiNaC::exmap
            generateExpressionMap(const std::vector<double> &states,
                                  const std::vector<double> &inputs) const;

            GiNaC::exmap generateExpressionMap(double t,
                                               const std::vector<double> &states,
                                               const std::vector<double> &inputs) const;

            GiNaC::symbol getSymbol(const std::string &symbolName) const;

            GiNaC::symbol getStateSymbol(unsigned int i) const;

            GiNaC::symbol getInputSymbol(unsigned int i) const;

            GiNaC::symbol getTimeSymbol() const;

            const GiNaC::lst &getFunctions() const;

            void setFunctions(const GiNaC::lst &functions);

            const GiNaC::lst &getStates() const;

            void setStates(const GiNaC::lst &states);

            const GiNaC::lst &getInputs() const;

            void setInputs(const GiNaC::lst &inputs);

            Eigen::MatrixXd evalf(const GiNaC::exmap &m) const;

            Eigen::MatrixXd evalf(const std::vector<double> &states,
                                  const std::vector<double> &inputs) const;

            Eigen::MatrixXd
            evalf(double t, const std::vector<double> &states,
                  const std::vector<double> &inputs) const;

            GiNaC::matrix generateJacobian(const GiNaC::lst &variables) const;

            GiNaC::matrix generateJacobianStates() const;

            std::string generateJacobianStatesCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            std::string generateJacobianStatesArrayInputCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            GiNaC::matrix generateJacobianInputs() const;

            std::string generateJacobianInputsCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            std::string generateJacobianInputsArrayInputCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            Eigen::MatrixXd generateJacobianMatrix(const GiNaC::lst &variables,
                                                   const GiNaC::exmap &exmap) const;

            Eigen::MatrixXd
            generateJacobianMatrix(const GiNaC::matrix &jacobian,
                                   const GiNaC::exmap &exmap) const;

            Eigen::MatrixXd
            generateJacobianMatrixStates(const GiNaC::exmap &exmap) const;

            Eigen::MatrixXd
            generateJacobianMatrixInputs(const GiNaC::exmap &exmap) const;

            std::string generateMatrixCppFunc(const GiNaC::matrix &ginacMatrix, const std::string &functionName, const bool dynamicType = false) const;

            std::string generateMatrixArrayInputCppFunc(const GiNaC::matrix &ginacMatrix, const std::string &functionName, const bool dynamicType = false) const;

            systems::StateSpace<> linearize(const GiNaC::exmap &exmap) const;

            systems::StateSpace<> linearize(const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            systems::StateSpace<> linearize(double t,
                                          const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            systems::StateSpace<> linearize(const GiNaC::matrix &jacobianStates,
                                          const GiNaC::matrix &jacobianInputs,
                                          const GiNaC::exmap &exmap) const;

            systems::StateSpace<> linearize(const GiNaC::matrix &jacobianStates,
                                          const GiNaC::matrix &jacobianInputs,
                                          const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            systems::StateSpace<> linearize(const GiNaC::matrix &jacobianStates,
                                          const GiNaC::matrix &jacobianInputs,
                                          double t,
                                          const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            static Eigen::MatrixXd matrixToMatrixXd(const GiNaC::matrix &mat);

            static Eigen::MatrixXd matrixToMatrixXd(const GiNaC::ex &ex);

        protected:
            GiNaC::lst functions_;
            GiNaC::symbol time_;
            GiNaC::lst states_;
            GiNaC::lst inputs_;

            std::map<std::string, GiNaC::symbol> symbolMap_;

            void makeSymbolMap();

            static void replaceString(std::string &str, const std::string &source, const std::string &dest);
            static void replaceStringAll(std::string &str, const std::string &source, const std::string &dest);
            static std::string stripWhiteSpace(std::string &str);
        };
    }
}

#endif

#endif //LODESTAR_ORDINARYDIFFERENTIALEQUATION_HPP


#pragma once

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include "../outflow_helper.hpp"
#include "lsq_bc_base.hpp"
#include "quadrature/qhermitew.hpp"

namespace boltzmann {
namespace impl_lsq {

class DiffusiveReflection : public LSQ_BC_Base
{
 public:
  typedef Eigen::MatrixXd mat_t;

 public:
  DiffusiveReflection(int K, double vt, double Tw);

  inline void apply(mat_t& dst, const mat_t& src) const;

 private:
  double vt_;
  double Tw_;

  Eigen::MatrixXd Mw_;
  Eigen::MatrixXd D_;
  Eigen::MatrixXd C_;
  boltzmann::impl::outflow_helper outflow_helper_;
};

DiffusiveReflection::DiffusiveReflection(int K, double vt, double Tw)
    : LSQ_BC_Base(K)
    , vt_(vt)
    , Tw_(Tw)
{
  Mw_.resize(K, K);
  D_.resize(K, K);
  C_.resize(K, K);

  double PI = boost::math::constants::pi<double>();

  boltzmann::QHermiteW hermite_quad(1.0, K);
  auto x = hermite_quad.pts();
  auto w = hermite_quad.wts();

  double Tw32 = std::pow(Tw, 1.5);
  double p = std::sqrt(1 / 2 / PI);

  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      Mw_(i, j) =
          Tw32 * p * std::exp(-(x[i] * x[i] + x[j] * x[j]) / 2 / Tw) * std::sqrt(w[i] * w[j]);
    }
  }
}

inline void
DiffusiveReflection::apply(mat_t& dst, const mat_t& src) const
{
  double rho_p = outflow_helper_.compute(src);

}

}  // impl_lsq
}  // boltzmann


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifndef UUID_995547FAAE0E11DE8CF511E755D89593
#define UUID_995547FAAE0E11DE8CF511E755D89593

#include <boost/qvm/detail/determinant_impl.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/static_assert.hpp>

namespace boost {
namespace qvm {
namespace qvm_detail {
template <class A>
BOOST_QVM_INLINE_OPERATIONS typename deduce_mat<A>::type
cofactor_impl(A const &a) {
  BOOST_QVM_STATIC_ASSERT(mat_traits<A>::rows == mat_traits<A>::cols);
  int const N = mat_traits<A>::rows;
  typedef typename mat_traits<A>::scalar_type T;
  T c[N - 1][N - 1];
  typedef typename deduce_mat<A>::type R;
  R b;
  for (int j = 0; j != N; ++j) {
    for (int i = 0; i != N; ++i) {
      int i1 = 0;
      for (int ii = 0; ii != N; ++ii) {
        if (ii == i)
          continue;
        int j1 = 0;
        for (int jj = 0; jj != N; ++jj) {
          if (jj == j)
            continue;
          c[i1][j1] = mat_traits<A>::read_element_idx(ii, jj, a);
          ++j1;
        }
        ++i1;
      }
      T det = determinant_impl(c);
      if ((i + j) & 1)
        det = -det;
      mat_traits<R>::write_element_idx(i, j, b) = det;
    }
  }
  return b;
}
} // namespace qvm_detail
} // namespace qvm
} // namespace boost

#endif