/*
 *  ft_calib.cpp
 *
 *  Modified By Chuan QIN, July 2019
 * 	Logs:
 * 		* Remove the require for IMU sensor, the TF of manipulator is used instead.
 *    * The base frame's inclination to ground is not required, only the local gravity acceleration is required (usually 9.81)
 *    * In order to accelerate the calibration SVD process, the force and torque are calibrated separately
 *
 *
 * 	Original file infomation:
 *  Created on: Sep 26, 2012
 *  Authors:   Francisco Viña
 *            fevb <at> kth.se
 */

/* Copyright (c) 2012, Francisco Viña, CVAP, KTH
   All rights reserved.

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

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

#include <ros/ros.h>
#include <force_torque_sensor_calib/ft_calib.h>
#include <Eigen/Dense>
#include "tf2_eigen/tf2_eigen.h"

namespace Calibration{

FTCalib::FTCalib(double m_local_gravitational_acceleration):m_local_gravitational_acceleration(m_local_gravitational_acceleration)
{
	m_num_meas = 0;
}

FTCalib::~FTCalib(){

}

void FTCalib::addMeasurement(const geometry_msgs::TransformStamped &tf_sensor_to_base,
		const geometry_msgs::WrenchStamped &ft_raw)
{
	if(tf_sensor_to_base.header.frame_id != ft_raw.header.frame_id)
	{
		ROS_ERROR("TF's source frame (%s) is not the same as the ft raw expressed frame (%s)!",
				tf_sensor_to_base.header.frame_id.c_str(), ft_raw.header.frame_id.c_str());
		return;
	}

	m_num_meas++;

	Eigen::Matrix<double, 3,6> A_force = getForceMeasurementMatrix(tf_sensor_to_base);
	Eigen::Matrix<double, 3,6> A_biased_torque = getBiasedTorqueMeasurementMatrix(ft_raw);

	Eigen::Vector3d measured_force, measured_torque;
	measured_force(0) = ft_raw.wrench.force.x;
	measured_force(1) = ft_raw.wrench.force.y;
	measured_force(2) = ft_raw.wrench.force.z;

	measured_torque(0) = ft_raw.wrench.torque.x;
	measured_torque(1) = ft_raw.wrench.torque.y;
	measured_torque(2) = ft_raw.wrench.torque.z;

	if(m_num_meas==1)
	{
		stacked_A_force = A_force;
		stacked_A_biased_torque = A_biased_torque;
		stacked_measured_force = measured_force;
		stacked_measured_torque = measured_torque;
	}

	else
	{
		Eigen::MatrixXd stacked_A_force_tmp = stacked_A_force;
		Eigen::MatrixXd stacked_A_biased_torque_tmp = stacked_A_biased_torque;

		Eigen::VectorXd stacked_measured_force_tmp = stacked_measured_force;
		Eigen::VectorXd stacked_measured_torque_tmp = stacked_measured_torque;

		stacked_A_force.resize(m_num_meas*3, 6);
		stacked_A_biased_torque.resize(m_num_meas*3, 6);

		stacked_measured_force.resize(m_num_meas*3);
		stacked_measured_torque.resize(m_num_meas*3);

		stacked_A_force.topRows((m_num_meas-1)*3) = stacked_A_force_tmp;
		stacked_A_biased_torque.topRows((m_num_meas-1)*3) = stacked_A_biased_torque_tmp;
		stacked_measured_force.topRows((m_num_meas-1)*3) = stacked_measured_force_tmp;
		stacked_measured_torque.topRows((m_num_meas-1)*3) = stacked_measured_torque_tmp;

		stacked_A_force.bottomRows(3) = A_force;
		stacked_A_biased_torque.bottomRows(3) = A_biased_torque;
		stacked_measured_force.bottomRows(3) = measured_force;
		stacked_measured_torque.bottomRows(3) = measured_torque;
	}

}
Eigen::Matrix<double, 3,6> FTCalib::getForceMeasurementMatrix(const geometry_msgs::TransformStamped &tf_sensor_to_base){
	Eigen::Matrix<double, 3, 6> A_force; // the force measurement matrix
	// [fx, fy, fz]^t = A_force * [g_x_in_base * m, g_y_in_base * m, g_z_in_base * m, f_bias_x, f_bias_y, f_bias_z]^t

	Eigen::Matrix3d R_sensor_to_base; // rotation from sensor frame to base frame

	// translate from geometry_msgs::TransformStamped to eigen matrix
	Eigen::Quaternion<double> q;
	Eigen::fromMsg(tf_sensor_to_base.transform.rotation, q);
	q.normalize();
	R_sensor_to_base = q.toRotationMatrix();

	A_force.block<3,3>(0,0) = R_sensor_to_base;
	A_force.block<3,3>(0,3).setIdentity();
	return A_force;
}

Eigen::Matrix<double, 3, 6> FTCalib::getBiasedTorqueMeasurementMatrix(const geometry_msgs::WrenchStamped &ft_raw){
	Eigen::Matrix<double, 3, 6> A_torque_biased; // the torque measurement matrix, which is biased because the force measurement is not unbiased
	// [tx, ty, tz]^t = A_torque * [x_center, y_center, z_center, t_bias_x, t_bias_y, t_bias_z]
	geometry_msgs::Vector3 force = ft_raw.wrench.force;
	A_torque_biased << 			0,	 force.z,	 force.y,	 1,	 0,	 0,
						 force.z, 					0,   force.x,	 0,	 1,	 0,
						 force.y, 	 force.x,					0,	 0,	 0,	 1;
	return A_torque_biased;
}

// Least squares to estimate the FT sensor parameters
Eigen::VectorXd FTCalib::getCalib()
{
	Eigen::VectorXd force_calib_params(6); //[g_x_in_base * mass, g_y_in_base * mass, g_z_in_base * mass, f_bias_x, f_bias_y, f_bias_z]

	force_calib_params = stacked_A_force.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(stacked_measured_force);

	Eigen::MatrixXd unbiased_stacked_A_torque = stacked_A_biased_torque;
	Eigen::Matrix<double,3,6> bias_matrix;
	Eigen::Vector3d force_bias = force_calib_params.segment<3>(3);
	bias_matrix << 						0,	force_bias(2),	force_bias(1),	0,	0,	0,
								force_bias(2),							0,	force_bias(0),	0,	0,	0,
								force_bias(1), 	force_bias(0), 							0,	0,	0,	0;

	for(int i=0; i<m_num_meas; i++){
		unbiased_stacked_A_torque.block<3,6>(i*3,0) -= bias_matrix;
	}

	Eigen::VectorXd torque_calib_params(6); //[x_center_in_sensor_frame, y_center_in_sensor_frame, z_center_in_sensor_frame, t_bias_x, t_bias_y, t_bias_z]
	torque_calib_params = unbiased_stacked_A_torque.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(stacked_measured_torque);

	// ft_calib_params: [mass,
	//									 g_x_in_base, g_y_in_base, g_z_in_base,
	//									 x_center_in_sensor_frame, y_center_in_sensor_frame, z_center_in_sensor_frame,
	//									 f_bias_x, f_bias_y, f_bias_z,
	//									 t_bias_x, t_bias_y, t_bias_z]
	Eigen::VectorXd ft_calib_params;
	ft_calib_params.resize(13);
	ft_calib_params(0) = force_calib_params.head<3>().norm() / m_local_gravitational_acceleration; //mass
	ft_calib_params(1) = force_calib_params(0) / ft_calib_params(0); //g_x_in_base
	ft_calib_params(2) = force_calib_params(1) / ft_calib_params(0); //g_y_in_base
	ft_calib_params(3) = force_calib_params(2) / ft_calib_params(0); //g_z_in_base

	ft_calib_params(4) = torque_calib_params(0); //x_center_in_sensor_frame
	ft_calib_params(5) = torque_calib_params(1); //y_center_in_sensor_frame
	ft_calib_params(6) = torque_calib_params(2); //z_center_in_sensor_frame

	ft_calib_params(7) = force_calib_params(3); // f_bias_x
	ft_calib_params(8) = force_calib_params(4); // f_bias_y
	ft_calib_params(9) = force_calib_params(5); // f_bias_z

	ft_calib_params(10) = torque_calib_params(3); //t_bias_x
	ft_calib_params(11) = torque_calib_params(4); //t_bias_y
	ft_calib_params(12) = torque_calib_params(5); //t_bias_z

	return ft_calib_params;
}

}


#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#define EIGEN_SUPERLU_SUPPORT

#include "Debug.h"
#include "SolverEigen.h"
#include "SolverPetsc.h"
#include "SolverTime.h"
#include "ComputerTime.h"
#include "util.h"

//#include <Eigen/SuperLUSupport>
#include <Eigen/SparseExtra>
#include <Eigen/IterativeSolvers>

extern SolverTime      solverTime;
extern ComputerTime    computerTime;

using namespace std;
using namespace Eigen;

SolverEigen::SolverEigen()
{
  STABILISED = false;

  update_precond = 1;

  if (debug) cout << " SolverEigen constructor\n\n";
}

SolverEigen::~SolverEigen()
{
  if (debug)  cout << " SolverEigen destructor\n\n";

  free();
}

int SolverEigen::initialise(int p1, int p2, int p3)
{
   nRow = nCol = p3;

   soln.resize(nRow);
   soln.setZero();

   rhsVec   = soln;
   solnPrev = soln;

  return 0;
}

int SolverEigen::setSolverAndParameters()
{
    ///////////////////////
    // Create the linear solver and set various options
    ///////////////////////

    ///////////////////////
    //Set operators. Here the matrix that defines the linear system
    //also serves as the preconditioning matrix.
    ///////////////////////

    return 0;
}

void SolverEigen::zeroMtx()
{
  //cout << " nRow = " << nRow << endl;
  //printVector(rhsVec);

  //cout << " nRow = " << nRow << endl;
  //cout << mtx << endl;
  //mtx.setZero();
  mtx *= 0.0;
  rhsVec.setZero();
  //cout << " nRow = " << nRow << endl;

  return;
}

int SolverEigen::free()
{
  return 0;
}

void SolverEigen::printInfo()
{
  //cout << "Eigen solver:  nRow = " << nRow << "\n";
  //cout << "               nnz  = " <<  << "\n\n";
  //printVector(rhsVec);
  //rhsVec.setZero();
  //cout << " nRow = " << nRow << endl;
  //cout << mtx << endl;

  return;
}

void SolverEigen::printMatrixPatternToFile()
{
    /*
    ofstream fout("matrix-pattern2.dat");

    if(fout.fail())
    {
      cout << " Could not open the Output file" << endl;
      exit(1);
    }

    fout << totalDOF << setw(10) << totalDOF << endl;

    for(int k=0; k<mtx.outerSize(); ++k)
    for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      printf("%9d \t %9d \n", it.row(), it.col());

    //for(int ii=0;ii<nRow;ii++)
      //for(int jj=0;jj<nRow;jj++)
        //printf("%5d \t %5d \t %12.6f \n",mtx.coeffRef(ii,jj));

    fout.close();
    */

   FILE * pFile;
   int n;
   char name [100];

   cout << mtx.nonZeros() << endl;
   //pFile = fopen ("Stokes.dat","w");
   pFile = fopen ("Poisson.dat","w");

    fprintf(pFile, "%9d \t %9d \t %9d \n", nRow, nCol, 0 );

    for(int k=0; k<mtx.outerSize(); ++k)
    for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      fprintf(pFile, "%9d \t %9d \t %20.16f \n", it.row(), it.col(), it.value());

   fclose (pFile);

   pFile = fopen ("rhsVec.dat","w");

    for(int k=0; k<nRow; ++k)
      fprintf(pFile, "%9d \t %14.8f \n", k, rhsVec(k));

   fclose (pFile);

   pFile = fopen ("refsoln.dat","w");

    for(int k=0; k<nRow; ++k)
      fprintf(pFile, "%9d \t %14.8f \n", k, soln(k));

   fclose (pFile);
  return;
}

void SolverEigen::printMatrix(int dig, int dig2, bool gfrmt, int indent, bool interactive)
{
  printInfo();

  cout << mtx << endl;
  printf("\n\n");

  return;
}

double SolverEigen::giveMatrixCoefficient(int row, int col)
{
  return  mtx.coeff(row,col);
}

int SolverEigen::factorise()
{
  char fct[] = "SolverEigen::factorise";

  if (currentStatus != ASSEMBLY_OK) { prgWarning(1,fct,"assemble matrix first!"); return 1; }

  if (checkIO)
  {
    // search for "nan" entries in matrix coefficients

    //if (prgNAN(mtx.x.x,NE)) prgError(1,fct,"nan matrix coefficient!");
  }

  computerTime.go(fct);

  currentStatus = FACTORISE_OK;

  solverTime.total     -= solverTime.factorise;
  solverTime.factorise += computerTime.stop(fct);
  solverTime.total     += solverTime.factorise;

  return 0;
}

int  SolverEigen::solve()
{
  char fct[] = "SolverEigen::solve";

  time_t tstart, tend;

  if (currentStatus != FACTORISE_OK) { prgWarning(1,fct,"factorise matrix first!"); return 1; }

  //algoType = 1;

  //cout << mtx << endl;

//#ifdef EIGEN_PARALLELIZE
//#ifdef EIGEN_HAS_OPENMP
//printf("Eigen parallellize is on \n");
//#else
//printf("Eigen parallellize is off \n");
//#endif

  if(algoType == 1)
  {
    //cout << " Solving with Eigen::SimplicialLDLT " << endl;

    SimplicialLDLT<SparseMatrix<double> > solver;

    //SuperLU<SparseMatrixXd > solver;
    tstart = time(0);

    solver.compute(mtx);

    soln = solver.solve(rhsVec);

    //printVector(soln);
    //
    //computeConditionNumber();

    //VectorXd x0 = VectorXd::LinSpaced(nRow, 0.0, 1.0);

    //double  cond = myCondNum(mtx, x0, 50, solver);

    //printf("\n Matrix condition number = %12.6E \n\n\n", cond);

    //soln = solver.solveWithGuess(rhsVec, soln);
    //cout << solver.info() << '\t' << solver.error() << '\t' << solver.iterations() << endl;
    tend = time(0);
    //printf("It took %8.4f second(s) \n ", difftime(tend, tstart) );
  }

  if( algoType == 2 )
  {
    cout << " Solving with Eigen::BiCGSTAB " << endl;

    BiCGSTAB<SparseMatrixXd, IncompleteLUT<double> > solver;

    solver.preconditioner().setDroptol(1.0e-3);
    solver.preconditioner().setFillfactor(3);

    //BiCGSTAB<SparseMatrixXd> solver;

    //GMRES<SparseMatrixXd, IncompleteLUT<double> > solver;

    //GMRES<SparseMatrixXd> solver;

    //ConjugateGradient<SparseMatrixXd, Lower, IncompleteLUT<double> >   solver;

    //solver.set_restart(100);
    //solver.setEigenv(10);

    solver.setMaxIterations(2000);
    solver.setTolerance(1.0e-10);

    tstart = time(0);

    //cout << " iiiiiiiiiiiiiii " << endl;
    solver.compute(mtx);
    //cout << " iiiiiiiiiiiiiii " << endl;

    soln = solver.solve(rhsVec);

    //printf("\n\n");
    //printVector(soln);

    //soln = solver.solveWithGuess(rhsVec, soln);

    tend = time(0);
    printf("It took %8.4f second(s) \n ", difftime(tend, tstart) );

    cout << solver.info() << '\t' << solver.error() << '\t' << solver.iterations() << endl;
  }

  solnPrev = soln;

  return 0;
}

int SolverEigen::factoriseAndSolve()
{
  if(currentStatus != ASSEMBLY_OK)
  {
    cerr << " assemble matrix first! " << endl;
    return 1;
  }

  factorise();

  return solve();
}

int SolverEigen::assembleMatrixAndVector(vector<int>& row, vector<int>& col, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj;
  for(ii=0;ii<row.size();ii++)
  {
    rhsVec[row[ii]] += Flocal(ii);
    for(jj=0;jj<col.size();jj++)
    {
      mtx.coeffRef(row[ii], col[jj]) += Klocal(ii,jj);
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVector(int start, int c1, vector<int>& vec1, vector<int>& vec2, MatrixXd& Klocal, VectorXd& Flocal)
{
  // subroutine for mixed formulation
  // vec1 - for variable 'u'
  // vec2 - for variable 'p'

  int ii, jj, aa, bb, size1, size2;

  //printVector(vec1);
  //printVector(vec2);

  size1 = vec1.size();
  size2 = vec2.size();

  for(ii=0;ii<size1;ii++)
  {
    rhsVec[vec1[ii]] += Flocal(ii);
    for(jj=0;jj<size1;jj++)
       mtx.coeffRef(vec1[ii], vec1[jj]) += Klocal(ii, jj);

    for(jj=0;jj<size2;jj++)
    {
       aa = start + vec2[jj];
       bb = size1 + jj;
       mtx.coeffRef(vec1[ii], aa) += Klocal(ii, bb);
       mtx.coeffRef(aa, vec1[ii]) += Klocal(bb, ii);
    }
  }

  for(ii=0;ii<size2;ii++)
  {
    aa = start + vec2[ii];
    rhsVec[aa] += Flocal(size1+ii);
  }

  if(STABILISED)
  {
    for(ii=0;ii<size2;ii++)
    {
      aa = start + vec2[ii];
      bb = size1 + ii;
      for(jj=0;jj<size2;jj++)
      {
        mtx.coeffRef(aa, start+vec2[jj]) += Klocal(bb, size1+jj);
      }
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVector(int start, int c1, vector<int>& forAssy, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj, aa, bb, size1, r, c;

  size1 = forAssy.size();

  /*
  for(ii=0;ii<size1;ii++)
  {
    rhsVec[forAssy[ii]] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(forAssy[ii], forAssy[jj]) += Klocal(ii, jj);
  }
  */

  for(ii=0; ii<size1; ii++)
  {
    aa = forAssy[ii];
    if( aa != -1 )
    {
      r = start + aa;
      rhsVec[r] += Flocal(ii);

      for(jj=0; jj<size1; jj++)
      {
        bb = forAssy[jj];
        if( bb != -1 )
          mtx.coeffRef(r, start+bb) += Klocal(ii,jj);
      }
    }
  }

  return 0;
}

int SolverEigen::assembleVector(int start, int c1, vector<int>& vec1, VectorXd& Flocal)
{
  int ii, jj;

  for(ii=0;ii<vec1.size();ii++)
  {
    rhsVec[vec1[ii]] += Flocal(ii);
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVectorCutFEM(int start, int c1, vector<int>& grid2cutfem_DOF, vector<int>& forAssy, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj, size1, r;

  //printVector(vec1);
  //printVector(vec2);

/*
  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    r = start+forAssy[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(r, start+forAssy[grid2cutfem_DOF[jj]]) += Klocal(ii, jj);
  }
*/
//
  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    r = forAssy[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(r, forAssy[grid2cutfem_DOF[jj]]) += Klocal(ii, jj);
  }
//

/*
  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    r = forAssy[ii];

    rhsVec[r] += Flocal(ii);

    for(jj=0;jj<size1;jj++)
      mtx.coeffRef(r, forAssy[jj]) += Klocal(ii, jj);
  }
*/

  return 0;
}

int SolverEigen::assembleMatrixAndVectorCutFEM2(int start1, int start2, vector<int>& grid2cutfem_DOF,
     vector<int>& forAssy1, vector<int>& forAssy2, MatrixXd& Klocal, VectorXd& Flocal1, VectorXd& Flocal2)
{
  // to assemble coupling matrices arriving from
  // jump conditions in the gradients across an interface

  int ii, jj, size1, kk, r, c;

  //printVector(vec1);
  //printVector(vec2);

  size1 = grid2cutfem_DOF.size();

  for(ii=0;ii<size1;ii++)
  {
    // force vector for domain #2
    r = start2 + forAssy2[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal2(ii);

    // force vector for domain #1
    r = start1 + forAssy1[grid2cutfem_DOF[ii]];

    rhsVec[r] += Flocal1(ii);

    // coupling matrix
    for(jj=0;jj<size1;jj++)
    {
      c = start2 + forAssy2[grid2cutfem_DOF[jj]];
      mtx.coeffRef(r, c) += Klocal(ii, jj);
      mtx.coeffRef(c, r) += Klocal(ii, jj);
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVectorCutFEM3(int start1, int start2, vector<int>& row, vector<int>& col,
     vector<int>& forAssy1, vector<int>& forAssy2, MatrixXd& Klocal, VectorXd& Flocal1)
{

  return 0;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/compose.hpp>
#include <mpllibs/metamonad/metafunction.hpp>

#include "common.hpp"

#include <boost/mpl/plus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/assert.hpp>

#include <boost/test/unit_test.hpp>

namespace
{
  using boost::mpl::plus;
  using boost::mpl::times;

  MPLLIBS_METAFUNCTION(inc, (N)) (( plus<N, int1> ));
  MPLLIBS_METAFUNCTION(take_first, (A)(B)) (( A ));
}

BOOST_AUTO_TEST_CASE(test_compose)
{
  using mpllibs::metamonad::compose;

  using boost::mpl::apply_wrap1;
  using boost::mpl::apply_wrap2;
  using boost::mpl::equal_to;

  // test_same_function_twice
  BOOST_MPL_ASSERT((
    equal_to<apply_wrap1<compose<inc<>, inc<> >, int1>::type, int3>
  ));

  // test_same_function_five_times
  BOOST_MPL_ASSERT((
    equal_to<
      apply_wrap1<compose<inc<>, inc<>, inc<>, inc<>, inc<> >, int1>::type,
      int6
    >
  ));

  // test_order
  BOOST_MPL_ASSERT((
    equal_to<apply_wrap1<compose<double_value<>, inc<> >, int13>::type, int28>
  ));

  // test_two_arguments_for_the_first_function
  BOOST_MPL_ASSERT((
    equal_to<
      apply_wrap2<compose<double_value<>, take_first<> >, int1, int3>::type,
      int2
    >
  ));
}


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

#include "test_global.hpp" // This must be the first header to be included.
#include "mdds/point_quad_tree.hpp"

#include <algorithm>
#include <memory>
#include <vector>
#include <sstream>
#include <boost/cstdint.hpp>

using namespace std;
using namespace mdds;
using ::boost::uint16_t;

struct data_printer
{
    void operator()(const string* p)
    {
        cout << *p << " ";
    }
};

template<typename _DbType>
struct search_result_printer
{
    void operator()(const pair<const typename _DbType::point, const typename _DbType::value_type>& r) const
    {
        cout << "  (x=" << r.first.x << ", y=" << r.first.y << ", value='" << *r.second << "')" << endl;
    }
};

void pqt_test_basic()
{
    stack_printer __stack_printer__("::pqt_test");
    typedef point_quad_tree<uint16_t, const string*> db_type;
    db_type db;

    string A("A");
    string B("B");
    string C("C");
    string D("D");
    string E("E");
    string F("F");
    string G("G");
    string H("H");
    string I("I");
    string J("J");
    string K("K");
    string L("L");
    string M("M");
    string N("N");

    db.insert(25, 32, &A);
    db.insert(5, 45, &B);
    db.insert(52, 10, &C);
    db.insert(80, 5, &D);
    db.insert(40, 50, &E);
    db.insert(10, 10, &F);
    db.insert(20, 20, &G);
    db.insert(80, 80, &H);
    db.insert(58, 46, &I);
    db.insert(36, 55, &J);
    db.insert(26, 52, &K);
    db.insert(38, 68, &L);
    db.insert(39, 78, &M);
    db.insert(72, 52, &N);

    assert(db.size() == 14);

    cout << "node count = " << get_node_instance_count() << endl;
    db.dump_tree_svg("./obj/test.svg");

    {
        db_type::data_array_type result;
        db.search_region(10, 10, 60, 20, result);
        cout << "search region: (10, 10, 60, 20)" << endl;
        cout << "result: ";
        for_each(result.begin(), result.end(), data_printer());
        cout << endl;

        result.clear();
        db.search_region(10, 10, 61, 61, result);
        cout << "search region: (10, 10, 61, 61)" << endl;
        cout << "result: ";
        for_each(result.begin(), result.end(), data_printer());
        cout << endl;
    }

    db_type::search_results result = db.search_region(10, 10, 60, 20);
    db_type::search_results::const_iterator itr = result.begin(), itr_end = result.end();
    cout << "result: " << endl;
    for_each(result.begin(), result.end(), search_result_printer<db_type>());

    result = db.search_region(10, 10, 61, 61);
    itr = result.begin(), itr_end = result.end();
    cout << "result: " << endl;
    for_each(result.begin(), result.end(), search_result_printer<db_type>());

    db.remove(20, 20);
    db.remove(40, 50);
    assert(db.size() == 12);
    db.dump_tree_svg("./obj/test-remove.svg");

    db.clear();
    assert(db.empty());
    assert(db.size() == 0);
}

void pqt_test_insertion_removal()
{
    stack_printer __stack_printer__("::pqt_test_insertion_removal");
    typedef point_quad_tree<int32_t, const string*> db_type;
    db_type db;

    // Check its empty-ness...
    assert(db.empty());
    assert(db.size() == 0);

    // Create all data instances.
    vector<unique_ptr<string>> data_store;
    data_store.reserve(100);
    for (size_t i = 0; i < 100; ++i)
    {
        ostringstream os;
        os << "0x" << hex << i;
        data_store.emplace_back(new string(os.str()));
    }

    vector<db_type::node_data> expected;

    // Insert data one by one, and verify each insertion.
    for (int32_t i = 0; i < 10; ++i)
    {
        for (int32_t j = 0; j < 10; ++j)
        {
            int32_t x = i * 10 + 1, y = j * 10 + 1;
            size_t index = i * 10 + j;
            const string* data_ptr = data_store[index].get();
            cout << "inserting '" << *data_ptr << "' at (" << x << "," << y << ")" << endl;
            db.insert(x, y, data_ptr);
            expected.push_back(db_type::node_data(x, y, data_ptr));

            vector<db_type::node_data> stored_data;
            db.get_all_stored_data(stored_data);
            assert(stored_data.size() == (index + 1));
            assert(db.size() == (index + 1));
            assert(!db.empty());
            bool success = db.verify_data(expected);
            assert(success);
        }
    }
    db.dump_tree_svg("./obj/pqt_test_insertion.svg");

    // Remove data one by one, and check the size after each removal.
    size_t node_count = 100;
    for (int32_t i = 0; i < 10; ++i)
    {
        for (int32_t j = 0; j < 10; ++j)
        {
            int32_t x = i * 10 + 1, y = j * 10 + 1;
            db.remove(x, y);
            size_t n = db.size();
            cout << "removing node at (" << x << "," << y << ")  "
                 << "size after removal: " << n << endl;
            --node_count;
            assert(node_count == n);
        }
    }
}

void pqt_test_remove_root()
{
    stack_printer __stack_printer__("::pqt_test_remove_root");
    typedef point_quad_tree<int32_t, const string*> db_type;
    string O("O");
    string NW("NW");
    string NE("NE");
    string SW("SW");
    string SE("SE");
    db_type db;

    // Insert all data and verify their storage.
    db.insert(10, 10, &O);
    db.insert(20, 0, &NE);
    db.insert(0, 0, &NW);
    db.insert(20, 20, &SE);
    db.insert(0, 20, &SW);
    db.dump_tree_svg("./obj/pqt_test_remove_root-1.svg");

    vector<db_type::node_data> expected;
    expected.push_back(db_type::node_data(10, 10, &O));
    expected.push_back(db_type::node_data(20, 0, &NE));
    expected.push_back(db_type::node_data(0, 0, &NW));
    expected.push_back(db_type::node_data(20, 20, &SE));
    expected.push_back(db_type::node_data(0, 20, &SW));
    bool success = db.verify_data(expected);
    assert(success);
    assert(db.size() == 5);

    // Now, remove the root node.
    db.remove(10, 10);
    db.dump_tree_svg("./obj/pqt_test_remove_root-2.svg");
    expected.clear();
    expected.push_back(db_type::node_data(20, 0, &NE));
    expected.push_back(db_type::node_data(0, 0, &NW));
    expected.push_back(db_type::node_data(20, 20, &SE));
    expected.push_back(db_type::node_data(0, 20, &SW));
    success = db.verify_data(expected);
    assert(success);
    assert(db.size() == 4);
}

void pqt_test_equality()
{
    stack_printer __stack_printer__("::pqt_test_equality");

    typedef point_quad_tree<int32_t, const string*> db_type;
    db_type db1, db2;

    string A("A");
    string B("B");
    string C("C");
    string D("D");
    string E("E");
    string F("F");

    assert(db1 == db2); // both are empty.

    db1.insert(0, 0, &A);
    db2.insert(0, 0, &A);
    assert(db1 == db2);
    db1.remove(0, 0);
    assert(db1 != db2);
    db1.insert(0, 0, &B);
    assert(db1 != db2);
    db2.insert(0, 0, &B); // B overwrites A.
    assert(db1 == db2); // Both should have B at (0,0).
    db1.insert(1, 1, &C);
    db2.insert(2, 2, &C);
    assert(db1 != db2);
    db1.insert(2, 2, &C);
    db2.insert(1, 1, &C);
    assert(db1 == db2);

    // Inserting data in different orders should make no difference in equality.
    db1.insert(1, 3, &D);
    db1.insert(1, 4, &E);
    db1.insert(1, 5, &F);

    db2.insert(1, 5, &F);
    db2.insert(1, 4, &E);
    db2.insert(1, 3, &D);
    assert(db1 == db2);
    db1.remove(1, 4);
    db2.remove(1, 4);
    assert(db1 == db2);

    // Make them empty again.
    db1.clear();
    db2.clear();
    assert(db1 == db2);
}

void pqt_test_assignment()
{
    stack_printer __stack_printer__("::pqt_test_assignment");
    typedef point_quad_tree<int32_t, const string*> db_type;
    db_type db1, db2;
    string A("A");
    string B("B");
    string C("C");
    string D("D");
    string E("E");
    string F("F");

    db1.insert(0, 10, &A);
    db1.insert(2, 5, &B);
    db1.insert(-10, 2, &C);
    db1.insert(5, 7, &D);
    vector<db_type::node_data> expected;
    expected.push_back(db_type::node_data(0, 10, &A));
    expected.push_back(db_type::node_data(2, 5, &B));
    expected.push_back(db_type::node_data(-10, 2, &C));
    expected.push_back(db_type::node_data(5, 7, &D));
    bool success = db1.verify_data(expected);
    assert(success);

    db2 = db1;
    success = db2.verify_data(expected);
    assert(success);
    success = db1.verify_data(expected);
    assert(success);

    db2.insert(12, 45, &E);
    db2.insert(20, 42, &F);
    success = db2.verify_data(expected); // This should fail.
    assert(!success);
    db2 = db1; // Assign once again.
    success = db2.verify_data(expected); // This now should succeed.
    assert(success);
}

void pqt_test_swap()
{
    stack_printer __stack_printer__("::pqt_test_swap");
    typedef point_quad_tree<int32_t, const string*> db_type;
    db_type db1, db2;
    string A("A");
    string B("B");
    string C("C");
    string D("D");
    string E("E");
    string F("F");

    db1.insert(0, 10, &A);
    db1.insert(2, 5, &B);
    db1.insert(-10, 2, &C);
    db1.insert(5, 7, &D);
    vector<db_type::node_data> expected;
    expected.push_back(db_type::node_data(0, 10, &A));
    expected.push_back(db_type::node_data(2, 5, &B));
    expected.push_back(db_type::node_data(-10, 2, &C));
    expected.push_back(db_type::node_data(5, 7, &D));
    bool success = db1.verify_data(expected);
    assert(success);
    assert(db2.empty());

    db1.swap(db2);
    assert(db1.empty());
    assert(!db2.empty());
    success = db2.verify_data(expected);
    assert(success);
}

template<typename _DbType>
bool verify_find(
    const _DbType& db, typename _DbType::key_type x, typename _DbType::key_type y,
    const typename _DbType::value_type data)
{
    try
    {
        typename _DbType::value_type found = db.find(x, y);
        cout << "found at (" << x << "," << y << "): " << found << endl;
        if (found == data)
            return true;
    }
    catch (const typename _DbType::data_not_found&)
    {
        cout << "nothing found at (" << x << "," << y << ")" << endl;
    }
    return false;
}

void pqt_test_find()
{
    stack_printer __stack_printer__("::pqt_test_find");
    typedef point_quad_tree<int32_t, const string*> db_type;
    db_type db;
    string A("A");
    string B("B");
    string C("C");
    string D("D");
    string E("E");
    string F("F");
    db.insert(92, 27, &A);
    db.insert(53, 26, &B);
    db.insert(69, 18, &C);
    db.insert(0, 78, &D);
    db.insert(17, 7, &E);
    db.insert(91, 88, &F);
    assert(db.size() == 6);
    db.dump_tree_svg("obj/pqt_test_find.svg");

    bool check;
    check = verify_find(db, 92, 27, &A);
    assert(check);
    check = verify_find(db, 53, 26, &B);
    assert(check);
    check = verify_find(db, 69, 18, &C);
    assert(check);
    check = verify_find(db, 0, 78, &D);
    assert(check);
    check = verify_find(db, 17, 7, &E);
    assert(check);
    check = verify_find(db, 91, 88, &F);
    assert(check);

    // Check for non-existent data.
    check = verify_find(db, 34, 86, &A);
    assert(!check);
    check = verify_find(db, -1, 7, &A);
    assert(!check);
    check = verify_find(db, 91, 27, &A);
    assert(!check);
}

void pqt_test_node_access()
{
    stack_printer __stack_printer__("::pqt_test_node_access");
    typedef point_quad_tree<int32_t, const string*> db_type;
    db_type db;
    db_type::node_access nac = db.get_node_access();
    assert(!nac);
    string A("A");
    string B("B");
    string C("C");
    string D("D");
    string E("E");
    string F("F");
    db.insert(92, 27, &A);
    db.insert(53, 26, &B);
    db.insert(69, 18, &C);
    db.insert(0, 78, &D);
    db.insert(17, 7, &E);
    db.insert(91, 88, &F);
    assert(db.size() == 6);

    nac = db.get_node_access();
    // Test root node.
    assert(nac);
    assert(nac.x() == 92);
    assert(nac.y() == 27);
    assert(nac.data() == &A);

    bool success = db.verify_node_iterator(nac);
    assert(success);
}

int main()
{
    try
    {
        pqt_test_basic();
        pqt_test_insertion_removal();
        pqt_test_remove_root();
        pqt_test_equality();
        pqt_test_assignment();
        pqt_test_swap();
        pqt_test_find();
        pqt_test_node_access();
        assert(get_node_instance_count() == 0);
    }
    catch (const std::exception& e)
    {
        cout << "Test failed: " << e.what() << endl;
        return EXIT_FAILURE;
    }

    cout << "Test finished successfully!" << endl;
    return EXIT_SUCCESS;
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/51-100/76/problem76.hpp"

BOOST_AUTO_TEST_SUITE( Problem76 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem76::solve(5);
        BOOST_CHECK_EQUAL(res, 6);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem76::solve();
        BOOST_CHECK_EQUAL(res, 190569291);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <blitzml/dataset/sparse_dataset.h>

#include <blitzml/base/math_util.h>

using std::vector;

namespace BlitzML {

template <typename data_t>
SparseColumn<data_t>::SparseColumn(const index_t *indices, const data_t *values,
                                   index_t nnz, index_t length)
    : Column(length, nnz), indices(indices), values(values) { }

template <typename data_t>
value_t SparseColumn<data_t>::inner_product(const vector<value_t> &vec) const {
  value_t result = 0.;
  for (index_t ind = 0; ind < nnz_; ++ind) {
    result += values[ind] * vec[indices[ind]];
  }
  return result;
}

template <typename data_t>
value_t SparseColumn<data_t>::weighted_inner_product(
    const vector<value_t> &vec,
    const vector<value_t> &weights) const {
  value_t result = 0.;
  for (index_t ind = 0; ind < nnz_; ++ind) {
    index_t i = indices[ind];
    result += values[ind] * vec[i] * weights[i];
  }
  return result;
}

template <typename data_t>
value_t SparseColumn<data_t>::weighted_norm_sq(
    const vector<value_t> &weights) const {
  value_t result = 0.;
  for (index_t ind = 0; ind < nnz_; ++ind) {
    result += sq(values[ind]) * weights[indices[ind]];
  }
  return result;
}

template <typename data_t>
void SparseColumn<data_t>::add_multiple(vector<value_t> &target,
                                        value_t scalar) const {

  const index_t* i = indices;
  const data_t* v = values;
  for (index_t n = nnz_; n != 0; --n) {
    target[*i++] += (*v++) * scalar;
  }
}

template <typename data_t>
void SparseColumn<data_t>::weighted_add_multiple(
    vector<value_t> &target, vector<value_t> &weights, value_t scalar) const {
  const index_t* i = indices;
  const data_t* v = values;
  for (index_t n = nnz_; n != 0; --n) {
    target[*i] += (*v) * scalar * weights[*i];
    ++i; ++v;
  }
}

template <typename data_t>
value_t SparseColumn<data_t>::sum() const {
  return sum_array(values, nnz_);
}

template <typename data_t>
value_t SparseColumn<data_t>::mean() const {
  return sum() / length();
}

template <typename data_t>
value_t SparseColumn<data_t>::l2_norm_sq() const {
  return BlitzML::l2_norm_sq(values, nnz_);
}

template class SparseColumn<float>;
template class SparseColumn<double>;
template class SparseColumn<int>;
template class SparseColumn<bool>;

template <typename data_t>
SparseDataset<data_t>::SparseDataset(
    const index_t *indices, const size_t *indptr, const data_t *data,
    index_t height, index_t width, size_t nnz,
    const value_t *b, index_t length_b)
    : Dataset(height, width, nnz, b, length_b) {

  A_cols.resize(width);
  for (index_t j = 0; j < width; ++j) {
    size_t offset = indptr[j];
    index_t col_nnz = static_cast<index_t>(indptr[j + 1] - offset);
    const data_t* col_data = data + offset;
    const index_t* col_indices = indices + offset;
    A_cols[j] = SparseColumn<data_t>(col_indices, col_data, col_nnz, height);
  }
}

template <typename data_t>
bool SparseDataset<data_t>::any_columns_overlapping_in_submatrix(
    index_t first_column_submatrix, index_t end_column_submatrix) const {
  vector<bool> seen_row(height, false);
  for (index_t j = first_column_submatrix; j < end_column_submatrix; ++j) {
    const SparseColumn<data_t>& col = A_cols[j];
    for (const index_t* i = col.indices_begin();
         i != col.indices_end(); ++i) {
      if (seen_row[*i]) {
        return true;
      } else {
        seen_row[*i] = true;
      }
    }
  }
  return false;
}

template class SparseDataset<float>;
template class SparseDataset<double>;
template class SparseDataset<int>;
template class SparseDataset<bool>;

}


#include "humidity.h"
#include "../basic.h"

#include <boost/log/trivial.hpp>

namespace WorldEngine
{

static void HumidityCalculation(World& world);

void HumiditySimulation(World& world)
{
   BOOST_LOG_TRIVIAL(info) << "Humidity simulation start";

   const OceanArrayType&    ocean = world.GetOceanData();
   const HumidityArrayType& h     = world.GetHumidityData();

   HumidityCalculation(world);

   world.SetThreshold(HumidityLevel::Superarid,
                      FindThresholdF(h, world.humids()[0], &ocean));
   world.SetThreshold(HumidityLevel::Perarid,
                      FindThresholdF(h, world.humids()[1], &ocean));
   world.SetThreshold(HumidityLevel::Arid,
                      FindThresholdF(h, world.humids()[2], &ocean));
   world.SetThreshold(HumidityLevel::Semiarid,
                      FindThresholdF(h, world.humids()[3], &ocean));
   world.SetThreshold(HumidityLevel::Subhumid,
                      FindThresholdF(h, world.humids()[4], &ocean));
   world.SetThreshold(HumidityLevel::Humid,
                      FindThresholdF(h, world.humids()[5], &ocean));
   world.SetThreshold(HumidityLevel::Perhumid,
                      FindThresholdF(h, world.humids()[6], &ocean));
   world.SetThreshold(HumidityLevel::Superhumid,
                      std::numeric_limits<float>::max());

   BOOST_LOG_TRIVIAL(info) << "Humidity simulation finish";
}

static void HumidityCalculation(World& world)
{
   const uint32_t width  = world.width();
   const uint32_t height = world.height();

   float precipitationWeight = 1.0f;
   float irrigationWeight    = 3.0f;

   const PrecipitationArrayType& p = world.GetPrecipitationData();
   const IrrigationArrayType&    i = world.GetIrrigationData();
   HumidityArrayType&            h = world.GetHumidityData();
   h.resize(boost::extents[height][width]);

   for (uint32_t y = 0; y < height; y++)
   {
      for (uint32_t x = 0; x < width; x++)
      {
         h[y][x] =
            (p[y][x] * precipitationWeight - i[y][x] * irrigationWeight) /
            (precipitationWeight + irrigationWeight);
      }
   }
}

} // namespace WorldEngine


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    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
 *      [1] J.R. Clynch, Introduction to Earth Gravity, March 2006,
 *          http://www.gmat.unsw.edu.au/snap/gps/clynch_pdfs/intgrav.pdf
 *      [2] WolframAlpha: computational knowledge engine, coriolis calculator
 *          http://www.wolframalpha.com/input/?i=coriolis+effect&a=*C.coriolis+effect-_*Formula-
 *
 */

#define BOOST_TEST_MAIN

#include <functional>
#include <boost/lambda/lambda.hpp>
#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/ReferenceFrames/apparentAccelerationModel.h"

namespace tudat
{
namespace unit_tests
{

using reference_frames::computeApparentAcceleration;
using reference_frames::ApparentAccelerationModel;

BOOST_AUTO_TEST_SUITE( test_reference_frame_transformations )

// Summary of tests.
// Test 1: Check apparent acceleration of static object at Earth equator (=only centrifugal).
// Test 2: Check apparent acceleration of moving object at Earth pole (=only Coriolis).
// Test 3: Check apparent acceleration of moving object in an accelerating, rotational reference
//         frame.
// Test 4: Check apparent acceleration of moving object in inertial reference frame (=0).
// Test 5: Test class implementation.

//! Test apparent acceleration of static object at Earth equator (=only centrifugal).
// Reference data from [1] (page 5), recomputed using Excel for higher precision.
BOOST_AUTO_TEST_CASE( testCentrifugalAccelerationEarth )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationRate( 0.0, 0.0, 7.29212351699e-5 );
    const Eigen::Vector3d rotationAcceleration = Eigen::Vector3d::Zero( );

    // Specify state.
    const Eigen::Vector3d position( 6378000.0, 0.0, 0.0 );
    const Eigen::Vector3d velocity = Eigen::Vector3d::Zero( );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( 0.0339150567038567, 0.0, 0.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( apparentAcceleration, expectedAcceleration, 1.0e-12 );
}

//! Test apparent acceleration of moving object at Earth pole (=only Coriolis).
// Reference data from pen/paper analysis, checked using [2].
BOOST_AUTO_TEST_CASE( testCoriolisAccelerationAtEarthPoles )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationRate( 0.0, 0.0, 7.29212351699e-5 );
    const Eigen::Vector3d rotationAcceleration = Eigen::Vector3d::Zero( );

    // Specify state.
    const Eigen::Vector3d position( 0.0, 0.0, 6378000.0 );
    const Eigen::Vector3d velocity( 10.0, 0.0, 0.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( 0.0, -0.0014584247033981, 0.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( apparentAcceleration, expectedAcceleration, 1.0e-13 );
}

//! Test apparent acceleration of moving object in a general non-inertial reference frame.
// Checked with computations performed in MATLAB.
BOOST_AUTO_TEST_CASE( testApparentAccelerationGeneral )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration( 1.0, 2.0, 3.0 );
    const Eigen::Vector3d rotationRate( 0.1, 0.2, 0.3 );
    const Eigen::Vector3d rotationAcceleration( 0.03, 0.02, 0.01 );

    // Specify state.
    const Eigen::Vector3d position( 100.0, 300.0, 500.0 );
    const Eigen::Vector3d velocity( 10.0, 40.0, 25.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( -2.0, 9.0, -10.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( apparentAcceleration, expectedAcceleration, 1.0e-14 );
}

//! Test apparent acceleration of moving object in an inertial reference frame (=0).
BOOST_AUTO_TEST_CASE( testApparentAccelerationStatic )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationRate = Eigen::Vector3d::Zero( );
    const Eigen::Vector3d rotationAcceleration = Eigen::Vector3d::Zero( );

    // Specify state.
    const Eigen::Vector3d position( 100.0, 300.0, 500.0 );
    const Eigen::Vector3d velocity( 10.0, 40.0, 25.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d apparentAcceleration = computeApparentAcceleration(
            referenceFrameAcceleration, rotationRate, rotationAcceleration, position, velocity );
    const Eigen::Vector3d expectedAcceleration( 0.0, 0.0, 0.0 );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_BASE( apparentAcceleration, expectedAcceleration );
}

//! Test class implementation of apparent acceleration model.
BOOST_AUTO_TEST_CASE( testApparentAccelerationClass )
{
    // Specify reference frame dynamics.
    const Eigen::Vector3d referenceFrameAcceleration( 1.0, 2.0, 3.0 );
    const Eigen::Vector3d rotationRate( 0.1, 0.2, 0.3 );
    const Eigen::Vector3d rotationAcceleration( 0.03, 0.02, 0.01 );
    const Eigen::Vector3d position( 100.0, 300.0, 500.0 );
    const Eigen::Vector3d velocity( 10.0, 40.0, 25.0 );

    // Create functions returning the parameters defined above.
    std::function< Eigen::Vector3d( ) > accelerationFunction =
            [ & ]( ){ return referenceFrameAcceleration; };
    std::function< Eigen::Vector3d( ) > rotationRateFunction =
            [ & ]( ){ return rotationRate; };
    std::function< Eigen::Vector3d( ) > rotationAccelerationFunction =
            [ & ]( ){ return rotationAcceleration; };
    std::function< Eigen::Vector3d( ) > positionFunction =
            [ & ]( ){ return position; };
    std::function< Eigen::Vector3d( ) > velocityFunction =
            [ & ]( ){ return velocity; };

    // Create object of apparentAcceleraitonModel class; pass functions in constructor
    ApparentAccelerationModel apparentAccelerationModel(
           accelerationFunction, rotationRateFunction, rotationAccelerationFunction,
           positionFunction, velocityFunction );
    const Eigen::Vector3d expectedAcceleration( -2.0, 9.0, -10.0 );

    // Compute apparent acceleration.
    const Eigen::Vector3d computedAcceleration = apparentAccelerationModel.getAcceleration( );

    // Check that resulting acceleration matches expected one.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedAcceleration,
                                       expectedAcceleration, 1.0e-14 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*!
 *
 * \file interpolation.hpp
 *
 * \brief File containing n-dimensional interpolation functions
 *        for linear and cubic order.
 *
 *
 */
#pragma once
#include <array>
#include <cmath>
#include <utility>

#include <Eigen/Dense>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

/*! \brief Namespace containing interpolation functionality for
 *         interpolating n-dimensional data using linear or cubic
 *         interpolation
 */
namespace interpolation
{
/*! \brief Struct for interpolation data
 *
 *  The Data struct is a simple wrapper of the std::array that allows
 *  you to have arrays of arrays. The idea is that for example for cubic
 *  interpolation, you need in 1D 4 data points, in 2D 4x4 data points etc.
 *  In 1D this array contains the necessary points for a given x. In 2D
 *  This array contains arrays for a given constant y whose interpolation
 *  results can then again be used for constant x, etc.
 *
 *  @tparam Func   The interpolation function for which to use the data.
 *                 This determines how many data points are needed. E.g.
 *                 cubic interpolation requires 4 points. The function must
 *                 provide the value NUMBER_OF_VALUES.
 *  @tparam Dim    The dimensionality of the data to interpolate. E.g. for
 *                 2 dimensional data, the result would be an array of arrays.
 */
template <typename Func, int Dim>
struct Data
{
    /*! \brief array of Dim-1 dimensional nested arrays.
     */
    std::array<Data<Func, Dim - 1>, Func::NUMBER_OF_VALUES> data;

    /*! \brief function to access this n-dimensional data
     *
     *  @param[in] index_0 the first index of the n-dimensional position
     *                     for which to look up the value
     *  @param[in] indices further indices depending on dimensionality,
     *                     i.e. Dim-1 indices for Dim-dimensional data.
     *
     *  @returns The value in the Dim-dimensional data structure at the
     *           given position
     *
     */
    template <typename... Indices>
    typename Func::VALUE_TYPE operator()(size_t index_0,
                                         Indices... indices) const
    {
        return data[index_0](indices...);
    }

    /*! \brief function to access this n-dimensional data
     *
     *  @param[in] index_0 the first index of the n-dimensional position
     *                     for which to look up the value
     *  @param[in] indices further indices depending on dimensionality,
     *                     i.e. Dim-1 indices for Dim-dimensional data.
     *
     *  @returns A reference to the value in the Dim-dimensional data structure
     *           at the given position
     *
     */
    template <typename... Indices>
    typename Func::VALUE_TYPE& operator()(size_t index_0, Indices... indices)
    {
        return data[index_0](indices...);
    }
};

/*! \brief 1D specialization of data
 *
 *  While all other Data<Func, Dim> structs contain arrays of Data<func,
 *  Dim-1>, the struct for 1D only contains an array of values of the
 *  value type the interpolation function works with. As such it works as
 *  a recursion stop.
 *
 *  @tparam Func   The interpolation function for which to use the data.
 *                 This determines how many data points are needed. E.g.
 *                 cubic interpolation requires 4 points.
 */
template <typename Func>
struct Data<Func, 1>
{
    /*! \brief Look-up type for nested arrays for the case of 1D.
     *
     *  In this special case of 1D the type is simply an array with the
     *  number of values and type defined by the interpolation function Func.
     *  E.g. for cubic<double> it would be std::array<double, 4>.
     */
    std::array<typename Func::VALUE_TYPE, Func::NUMBER_OF_VALUES> data;

    /*! \brief function to access this 1-dimensional data
     *
     *  @param[in] index the index of the 1-dimensional position for which to
     *                   look up the value
     *
     *  @returns The value in the 1-dimensional data structure at the
     *           given position
     *
     */
    typename Func::VALUE_TYPE operator()(size_t index) const
    {
        return data[index];
    }

    /*! \brief function to access this 1-dimensional data
     *
     *  @param[in] index the index of the 1-dimensional position for which to
     *                   look up the value
     *
     *  @returns A reference to the value in the 1-dimensional data structure at
     *           the given position
     *
     */
    typename Func::VALUE_TYPE& operator()(size_t index) { return data[index]; }
};

/*! \brief Cause a compile error for the illegal zero dimensional case
 */
template <typename Func>
struct Data<Func, 0>;

/*! \brief 1D linear interpolation
 *
 *  @tparam T   The datatype to use for the interpolation. E.g.
 *              float/double/etc.
 */
template <typename T>
struct linear
{
    /*! \brief The data type this function is operating with, e.g. for float
     *         images, it is float.
     */
    typedef T VALUE_TYPE;
    /*! \brief The number of datapoints required to perform the interpolation.
     *         Linear interpolation needs two datapoints.
     */
    static constexpr int NUMBER_OF_VALUES = 2;

    /*! \brief Compute the linear interpolation of the given points and
     * position.
     *
     *  Given 2 values, and a position in the interval [0, 1], this function
     *  returns the interpolated value using simple linear interpolation.
     *
     *  \note This function operates in double mode. The result is cast to the
     *        set template type T. NO rounding to nearest etc. is performed for
     *        e.g. integers.
     *
     *  @param[in] p   The 2 values to interpolate
     *  @param[in] x   The position in the interval [0, 1] to interpolate
     *
     *  @returns   The result of the interpolation
     */
    T operator()(const Data<linear<T>, 1>& p, double x)
    {
        return static_cast<T>(p(0) * (1 - x) + p(1) * x);
    }
};

/*! \brief 1D cubic interpolation
 *
 *  @tparam T   The datatype to use for the interpolation. E.g.
 *              float/double/etc.
 */
template <typename T>
struct cubic
{
    /*! \brief The data type this function is operating with, e.g. for float
     *         images, it is float.
     */
    typedef T VALUE_TYPE;
    /*! \brief The number of datapoints required to perform the interpolation.
     *         Cubic interpolation needs four datapoints.
     */
    static constexpr int NUMBER_OF_VALUES = 4;

    /*! \brief Compute the cubic interpolation of the given points and position.
     *
     *  Given 4 values, and a position in the interval [0, 1], this function
     *  returns the interpolated value using a uniform Catmull-Rom spline.
     *
     *  \note This function operates in double mode. The result is cast to the
     *        set template type T. NO rounding to nearest etc. is performed for
     *        e.g. integers.
     *
     *  @param[in] p   The 4 values to interpolate
     *  @param[in] x   The position in the interval [0, 1] to interpolate
     *
     *  @returns   The result of the interpolation
     */
    T operator()(const Data<cubic<T>, 1>& p, double x)
    {
        return static_cast<T>(
            p(1) + 0.5 * x *
                       (p(2) - p(0) +
                        x * (2.0 * p(0) - 5.0 * p(1) + 4.0 * p(2) - p(3) +
                             x * (3.0 * (p(1) - p(2)) + p(3) - p(0)))));
    }
};

/*! \brief Namespace containing implementation details for the interpolation
 *         functionality.
 */
namespace detail
{
/*! \brief Function to extract data from a pybind11 array using
 *         a std::array for indexing
 *
 *  Additional function required to realize the variadic indexing using
 *  the std::array.
 *
 *  @param[in] image          The image from which to extract a value
 *  @param[in] array_indices  The location of the data point to extract
 *
 *  @returns                  The value at the given position
 *
 *  @tparam T                 The data type of the image
 *  @tparam Dim               The dimensions of the image
 *  @tparam I                 Template parameter for unpacking the std::array
 */
template <typename T, int Dim, std::size_t... I>
T get_image_value_impl(
    const pybind11::detail::unchecked_reference<T, Dim>& image,
    const std::array<int, Dim>& array_indices, std::index_sequence<I...>)
{
    return image(array_indices[I]...);
}

/*! \brief Function to extract data from a pybind11 array using
 *         a std::array for indexing
 *  @param[in] image          The image from which to extract a value
 *  @param[in] array_indices  The location of the data point to extract
 *
 *  @returns                  The value at the given position
 *
 *  @tparam T                 The data type of the image
 *  @tparam Dim               The dimensions of the image
 */
template <typename T, int Dim>
T get_image_value(const pybind11::detail::unchecked_reference<T, Dim>& image,
                  const std::array<int, Dim>& array_indices)
{
    return get_image_value_impl<T, Dim>(image, array_indices,
                                std::make_index_sequence<Dim>{});
}

/*! \brief Apply Func to the given data and position.
 *
 * This function works by realising that the n-dimensional interpolation can
 * be broken down into a list of (n-1)-dimensional interpolations that are
 * interpolated via a 1D interpolation. This recursive call to
 * lower-dimensional interpolation is realized with recursive template
 * programming.
 *
 *  @param[in] indices   Indices used to unpack the array p to call the
 *                       lower dimensional interpolation on each sub-array
 *                       (for 2D and higher dimensional data).
 *  @param[in] chunk     N-dimensional array data for the interpolation.
 *                       Needs the right number of points in each dimension,
 *                       e.g. 4 for cubic interpolation. Should be nested
 *                       arrays in C order.
 *  @param[in] x         The first coordinate value of the N-dimensional
 *                       position at which to interpolate
 *  @param[in] xs        The last N-1 coordinate values of the N-dimensional
 *                       position. One argument for each dimension.
 *
 *  @returns             The interpolation value
 *
 *  @tparam Func         The interpolation order func
 *  @tparam Ts           The type of the position coordinate values. This is
 *                       enforced to be double
 *  @tparam I            Needed only to unpack array into separate
 *                       function calls.
 */
template <typename Func, typename... Ts, std::size_t... I>
static typename Func::VALUE_TYPE
apply_func_impl(std::index_sequence<I...> indices,
                const Data<Func, sizeof...(Ts) + 1>& chunk, double x, Ts... xs)
{
    if constexpr (sizeof...(Ts) > 0)
    {
        return apply_func_impl<Func>(
            indices,
            {{apply_func_impl(indices, std::get<I>(chunk.data), xs...)...}}, x);
    }
    else
    {
        return Func()(chunk, x);
    }
}

/*! \brief Extract interpolation patch from given data around given position.
 *
 *  This function uses variadic templates to iterate over n-dimensional
 *  data using the given boundary function.
 *
 *  @param[in,out] chunk           N-dimensional array data for the
 *                                 interpolation. Needs the right number of
 *                                 points in each dimension, e.g. 4 for cubic
 *                                 interpolation.
 *  @param[in] image               The image from which to extract the data.
 *  @param[in] lower_corner        Lower corner of the sub-cube to extract from
 *                                 the image into the given chunk.
 *  @param[in] background_value    The background value to use in case the value
 *                                 is outside the image domain (might be ignored
 *                                 depending on the boundary function used)
 *  @param[in] loop_indices        The loop indices of all the for loops to
 *                                 fill the n-dimensional chunk
 *
 *  @tparam Func                   The interpolation order func
 *  @tparam BoundaryFunc           The boundary type to use. E.g.
 *                                 ConstantBoundary
 *  @tparam Dim                    The dimensionality of the given image
 */
template <typename Func, typename BoundaryFunc, int Dim, typename... Ts>
static void extract_impl(
    Data<Func, Dim>& chunk,
    const pybind11::detail::unchecked_reference<typename Func::VALUE_TYPE, Dim>&
        image,
    const std::array<int, Dim>& lower_corner,
    typename Func::VALUE_TYPE background_value, Ts... loop_indices)
{
    for (int i = 0; i < Func::NUMBER_OF_VALUES; ++i)
    {
        if constexpr (Dim == sizeof...(Ts) + 1)
        {
            std::array<int, Dim> voxel_position{{loop_indices..., i}};

            for (int l = 0; l < Dim; ++l)
            {
                voxel_position[l] += lower_corner[l];
            }
            chunk(loop_indices..., i) =
                BoundaryFunc::template apply<typename Func::VALUE_TYPE, Dim>(image, voxel_position, background_value);
        }
        else
        {
            extract_impl<Func, BoundaryFunc, Dim>(chunk, image, lower_corner,
                                                  background_value,
                                                  loop_indices..., i);
        }
    }
}
}; // namespace detail

/*! \brief Struct for dealing with n-dimensional image boundaries by
 *         returning a constant values for coordinates out of bounds.
 */
struct ConstantBoundary
{
    /*! \brief Apply the constant boundary to the given image and position.
     *
     *  @param[in] image            The image to sample
     *  @param[in] voxel_position   The position at which to sample
     *  @param[in] background_value The background value to use if the given
     *                              position is out of bounds
     *
     *  @returns                    The value at the given position using the
     *                              rules defined by ConstantBoundary for
     *                              positions outside of the given image.
     *
     *  @tparam T                   The data type of the given image
     *  @tparam Dim                 The dimensions of the given image
     */
    template <typename T, int Dim>
    static T apply(const pybind11::detail::unchecked_reference<T, Dim>& image,
                   const std::array<int, Dim>& voxel_position,
                   T background_value)
    {
        for (int l = 0; l < Dim; ++l)
        {
            if (voxel_position[l] < 0 || voxel_position[l] >= image.shape(l))
            {
                return background_value;
            }
        }
        return detail::get_image_value<T, Dim>(image, voxel_position);
    }
};

/*! \brief Apply Func to the given data and position.
 *
 *  @param[in] chunk  N-dimensional array data for the interpolation. Needs
 *                    the right number of points in each dimension, e.g. 4
 *                    for cubic interpolation. Should be nested arrays in C
 *                    order.
 *  @param[in] x      The first coordinate value of the N-dimensional
 *                    position at which to interpolate
 *  @param[in] xs     The last N-1 coordinate values of the N-dimensional
 *                    position. One argument for each dimension.
 *
 *  @returns          The interpolation value
 *
 *  @tparam Func       The interpolation order func
 *  @tparam Ts        The type of the position coordinate values. This is
 *                    enforced to be double
 */
template <typename Func, typename... Ts>
static typename Func::VALUE_TYPE
apply_func(const Data<Func, sizeof...(Ts) + 1>& chunk, double x, Ts... xs)
{
    return detail::apply_func_impl(
        std::make_index_sequence<Func::NUMBER_OF_VALUES>{}, chunk, x, xs...);
}

/*! \brief Extract interpolation patch from given data around given position.
 *
 *  Different interpolation schemes require a different number of points
 *  to operate. This function takes the appropriate chunk and fills it with
 *  data based on the information of the function given.
 *
 *  @param[in,out] chunk           N-dimensional array data for the
 *                                 interpolation. Needs the right number of
 *                                 points in each dimension, e.g. 4 for cubic
 *                                 interpolation.
 *  @param[in] image               The image from which to extract the data.
 *  @param[in, out] point_floored  The point in the image where to interpolate
 *                                 floored. Will point to the lower corner of
 *                                 the data grid used for the interpolation at
 *                                 the end.
 *  @param[in] background_value    The background value to use in case the value
 *                                 is outside the image domain (might be ignored
 *                                 depending on the boundary function used)
 *  @tparam Func                    The interpolation order func
 *  @tparam BoundaryFunc           The boundary type to use. E.g.
 *                                 ConstantBoundary
 *  @tparam Dim                    The dimensionality of the given image
 */
template <typename Func, typename BoundaryFunc, int Dim>
static void
extract(Data<Func, Dim>& chunk,
        const pybind11::detail::unchecked_reference<typename Func::VALUE_TYPE,
                                                    Dim>& image,
        std::array<int, Dim>& point_floored,
        typename Func::VALUE_TYPE background_value)
{
    for (size_t l = 0; l < Dim; ++l)
    {
        point_floored[l] -= (Func::NUMBER_OF_VALUES - 2) / 2;
    }

    detail::extract_impl<Func, BoundaryFunc, Dim>(chunk, image, point_floored,
                                                  background_value);
}

}; // namespace interpolation


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110117    E. Iorfida        File created.
 *      110128    E. Iorfida        Added boolean variable that sets the necessity of
 *                                  Newton-Raphson method, particularly in unit test.
 *      110202    J. Melman         Renamed certain parameters and added comments to clarify the
 *                                  code more.
 *      110203    E. Iorfida        Changed some variables names and modified punctuation.
 *      110205    J. Melman         Removed the trailing underscores in some public variables. Some
 *                                  comment rephrasing. Changed and added some notes.
 *      110212    J. Melman         Added a reference to my own thesis.
 *      110214    E. Iorfida        Deleted temporary centralBodyRadius, replaced by an element of
 *                                  GeometricShapes.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120508    P. Musegaas       The gravitational parameter is now passed as double.
 *      120530    P. Musegaas       Complete revision. Removed class structure, made it a free
 *                                  function. Added two functions for propagating a gravity assist
 *                                  (powered and unpowered).
 *      120625    P. Musegaas       Minor changes.
 *      120703    T. Secretin       Minor layout changes.
 *      120704    P. Musegaas       Minor change. Reduced negligence of velocity effect from 1 cm/s
 *                                  to 1 micrometer/second.
 *      120713    P. Musegaas       Fixed various bugs (case with both bending + velocity effect
 *                                  was wrong, limit cases failed, rootfinder not ideal, all for
 *                                  original deltaV calculation function). Added iteration on
 *                                  pericenter radius instead of eccentricity. Improved efficiency.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *
 *    References
 *      References for deltaV computation function:
 *          Melman J. Trajectory optimization for a mission to Neptune and Triton, MSc thesis
 *              report, Delft University of Technology, 2007.
 *          Musegaas, P., Optimization of Space Trajectories Including Multiple Gravity Assists and
 *              Deep Space Maneuvers, MSc thesis report, Delft University of Technology, 2012.
 *              [unpublished so far].
 *      Reference for unpowered gravity assist propagation function:
 *          Conway, B.A., Spacecraft Trajectory Optimization, Chapter 7, Cambridge University
 *              Press, 2010.
 *      Reference for powered gravity assist propagation function:
 *          Musegaas, P., Optimization of Space Trajectories Including Multiple Gravity Assists and
 *              Deep Space Maneuvers, MSc thesis report, Delft University of Technology, 2012.
 *              [unpublished so far].
 *
 *    Notes
 *      Gravity assist and swing-by are two different words for the same thing. The delta-V that is
 *      computed for a powered swing-by has not been proven to be the optimum (lowest) to achieve
 *      the desired geometry of incoming and outgoing hyperbolic legs. Some literature research will
 *      have to be done to look at the alternatives.
 *
 *      Note that the exact implementation of Newton Raphson as root finder should be updated if
 *      someone would want to use a different root finding technique.
 *
 *      Note that by default a velocity effect deltaV of less than 1 micrometer/second is deemed
 *      negligable in this code. This value can be set though.
 *
 */

#include <cmath>

#include <boost/bind.hpp>

#include <Eigen/Dense>

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

#include "Tudat/Astrodynamics/MissionSegments/gravityAssist.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Calculate deltaV of a gravity assist.
double gravityAssist( const double centralBodyGravitationalParameter,
                      const Eigen::Vector3d& centralBodyVelocity,
                      const Eigen::Vector3d& incomingVelocity,
                      const Eigen::Vector3d& outgoingVelocity,
                      const double smallestPeriapsisDistance,
                      const bool useEccentricityInsteadOfPericenter,
                      const double speedTolerance,
                      RootFinderPointer rootFinder )
{
    using basic_mathematics::UnivariateProxyPointer;
    using basic_mathematics::UnivariateProxy;

    // Compute incoming and outgoing hyperbolic excess velocity.
    const Eigen::Vector3d incomingHyperbolicExcessVelocity
            = incomingVelocity - centralBodyVelocity;
    const Eigen::Vector3d outgoingHyperbolicExcessVelocity
            = outgoingVelocity - centralBodyVelocity;

    // Compute absolute values of the hyperbolic excess velocities.
    const double absoluteIncomingExcessVelocity = incomingHyperbolicExcessVelocity.norm( );
    const double absoluteOutgoingExcessVelocity = outgoingHyperbolicExcessVelocity.norm( );

    // Compute bending angle.
    double bendingAngle = linear_algebra::computeAngleBetweenVectors(
                            incomingHyperbolicExcessVelocity, outgoingHyperbolicExcessVelocity );

    // Compute maximum achievable bending angle.
    const double maximumBendingAngle =
            std::asin( 1.0 / ( 1.0 + ( smallestPeriapsisDistance *
                                       absoluteIncomingExcessVelocity *
                                       absoluteIncomingExcessVelocity /
                                       centralBodyGravitationalParameter ) ) ) +
            std::asin( 1.0 / ( 1.0 + ( smallestPeriapsisDistance *
                                       absoluteOutgoingExcessVelocity *
                                       absoluteOutgoingExcessVelocity /
                                       centralBodyGravitationalParameter ) ) );

    // Initialize bending effect deltaV, which is zero, unless extra bending angle is required.
    double bendingEffectDeltaV = 0.0;

    // Initialize velocity effect delta V parameter.
    double velocityEffectDeltaV = 0.0;

    // Check if an additional bending angle is required. If so, the additional bending angle
    // maneuver has to be performed. Also the pericenter radius will be the minimum pericenter
    // radius to obtain the largest possible bending angle 'for free'. Hence no root finding is
    // required for this case. As noted above, this may not be ideal for all cases. (for cases
    // in which the excess velocities are relatively small)
    if ( bendingAngle > maximumBendingAngle )
    {
        // Compute required extra bending angle that cannot be delivered by an unpowered swing-by.
        const double extraBendingAngle = bendingAngle - maximumBendingAngle;

        // Compute necessary delta-V due to bending-effect.
        bendingEffectDeltaV = 2.0 * std::min( absoluteIncomingExcessVelocity,
                                              absoluteOutgoingExcessVelocity ) *
                                    std::sin( extraBendingAngle / 2.0 );

        // This means the pericenter radius is now equal to the smallest pericenter radius, to
        // ensure the largest possible bending angle.
        const double pericenterRadius = smallestPeriapsisDistance;

        // Compute semi-major axis of hyperbolic legs.
        const double incomingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteIncomingExcessVelocity /
                                             absoluteIncomingExcessVelocity;
        const double outgoingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteOutgoingExcessVelocity /
                                             absoluteOutgoingExcessVelocity;

        // Compute incoming hyperbolic leg eccentricity.
        const double incomingEccentricity = 1 - pericenterRadius / incomingSemiMajorAxis;

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1 - pericenterRadius / outgoingSemiMajorAxis;

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    else if ( ( std::fabs( absoluteIncomingExcessVelocity - absoluteOutgoingExcessVelocity )
                <= speedTolerance ) )
    {
        // In this case no maneuver has to be performed. Hence no iteration is performed, and the
        // delta V is simply kept at 0.0.
    }

    // Here the required maneuver to patch the incoming and outgoing excess velocities is
    // calculated. In this implementation, the eccentricity will be used as iteration parameter.
    else if ( useEccentricityInsteadOfPericenter )
    {
        // Compute semi-major axis of hyperbolic legs.
        const double incomingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteIncomingExcessVelocity /
                                             absoluteIncomingExcessVelocity;
        const double outgoingSemiMajorAxis = -1.0 * centralBodyGravitationalParameter /
                                             absoluteOutgoingExcessVelocity /
                                             absoluteOutgoingExcessVelocity;

        // Set the gravity assist function with the variables to perform root finder calculations.
        EccentricityFindingFunctions eccentricityFindingFunctions( incomingSemiMajorAxis,
                                                                   outgoingSemiMajorAxis,
                                                                   bendingAngle );

        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &EccentricityFindingFunctions::
                                 computeIncomingEccentricityFunction,
                                 eccentricityFindingFunctions, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind(
                                      &EccentricityFindingFunctions::
                                      computeFirstDerivativeIncomingEccentricityFunction,
                                      eccentricityFindingFunctions, _1 ) );

        // Initialize incoming eccentricity.
        double incomingEccentricity = TUDAT_NAN;

        // Set initial guess of the variable computed in Newton-Rapshon method.
        if ( ( absoluteOutgoingExcessVelocity / absoluteIncomingExcessVelocity ) < 100.0 )
        {
            // In these cases the very low estimate (which is given under else) may in some cases
            // result in no convergence. Hence a higher value of 1.01 is necessary. This will not
            // result in 'going through' 1.0 as mentioned below, because the eccentricity in these
            // cases is always high!
            incomingEccentricity = rootFinder->execute( rootFunction, 1.0 + 1.0e-2 );
        }

        else
        {
            // This is set to a value that is close to 1.0. This is more robust than higher values,
            // because for those higher values Newton Raphson sometimes 'goes through' 1.0. This
            // results in NaN values for the derivative of the eccentricity finding function.
            incomingEccentricity = rootFinder->execute( rootFunction, 1.0 + 1.0e-10 );
        }

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1.0 - ( incomingSemiMajorAxis /
                                                    outgoingSemiMajorAxis ) *
                                            ( 1.0 - incomingEccentricity );

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    // Here the required maneuver to patch the incoming and outgoing excess velocities is
    // calculated. In this implementation, the pericenter radius will be used as iteration
    // parameter.
    else
    {
        // Compute semi-major axis of hyperbolic legs. This is the absolute semi major axis, because
        // it will otherwisely result in the root of a negative function for various cases during
        // the rootfinding process.
        const double absoluteIncomingSemiMajorAxis = 1.0 * centralBodyGravitationalParameter /
                                                     absoluteIncomingExcessVelocity /
                                                     absoluteIncomingExcessVelocity;
        const double absoluteOutgoingSemiMajorAxis = 1.0 * centralBodyGravitationalParameter /
                                                     absoluteOutgoingExcessVelocity /
                                                     absoluteOutgoingExcessVelocity;

        // Set the gravity assist function with the variables to perform root finder calculations.
        PericenterFindingFunctions pericenterFindingFunctions( absoluteIncomingSemiMajorAxis,
                                                               absoluteOutgoingSemiMajorAxis,
                                                               bendingAngle);

        // Create an object containing the function of which we whish to obtain the root from.
        UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                    boost::bind( &PericenterFindingFunctions::computePericenterRadiusFunction,
                                 pericenterFindingFunctions, _1 ) );

        // Add the first derivative of the root function.
        rootFunction->addBinding( -1, boost::bind( &PericenterFindingFunctions::
                                                   computeFirstDerivativePericenterRadiusFunction,
                                                   pericenterFindingFunctions, _1 ) );

        // Set pericenter radius based on result of Newton-Raphson root-finding algorithm.
        const double pericenterRadius = rootFinder->execute( rootFunction,
                                                             smallestPeriapsisDistance );

        // Compute incoming hyperbolic leg eccentricity.
        const double incomingEccentricity = 1.0 + pericenterRadius / absoluteIncomingSemiMajorAxis;

        // Compute outgoing hyperbolic leg eccentricity.
        const double outgoingEccentricity = 1.0 + pericenterRadius / absoluteOutgoingSemiMajorAxis;

        // Compute incoming and outgoing velocities at periapsis.
        const double incomingVelocityAtPeriapsis = absoluteIncomingExcessVelocity *
                std::sqrt( ( incomingEccentricity + 1.0 ) / ( incomingEccentricity - 1.0 ) );
        const double outgoingVelocityAtPeriapsis = absoluteOutgoingExcessVelocity *
                std::sqrt( ( outgoingEccentricity + 1.0 ) / ( outgoingEccentricity - 1.0 ) );

        // Compute necessary delta-V due to velocity-effect.
        velocityEffectDeltaV = std::fabs( incomingVelocityAtPeriapsis -
                                          outgoingVelocityAtPeriapsis );
    }

    // Compute and return the total delta-V.
    return bendingEffectDeltaV + velocityEffectDeltaV;
}

//! Propagate an unpowered gravity assist.
Eigen::Vector3d gravityAssist( const double centralBodyGravitationalParameter,
                               const Eigen::Vector3d& centralBodyVelocity,
                               const Eigen::Vector3d& incomingVelocity,
                               const double rotationAngle,
                               const double pericenterRadius )
{
    // Calculate the incoming velocity.
    const Eigen::Vector3d relativeIncomingVelocity = incomingVelocity - centralBodyVelocity;
    const double absoluteRelativeIncomingVelocity = relativeIncomingVelocity.norm( );

    // Calculate the eccentricity and bending angle.
    const double eccentricity = 1.0 + pericenterRadius / centralBodyGravitationalParameter *
                            absoluteRelativeIncomingVelocity * absoluteRelativeIncomingVelocity;
    const double bendingAngle = 2.0 * std::asin ( 1.0 / eccentricity );

    // Calculate the unit vectors.
    const Eigen::Vector3d unitVector1 = relativeIncomingVelocity /
                                        absoluteRelativeIncomingVelocity;
    const Eigen::Vector3d unitVector2 = unitVector1.cross( centralBodyVelocity ).normalized( );
    const Eigen::Vector3d unitVector3 = unitVector1.cross( unitVector2 );

    // Calculate the relative outgoing velocity.
    const Eigen::Vector3d relativeOutgoingVelocity = absoluteRelativeIncomingVelocity *
            ( std::cos( bendingAngle ) * unitVector1 + std::sin( bendingAngle ) *
              std::cos( rotationAngle ) * unitVector2 + std::sin( bendingAngle ) *
              std::sin( rotationAngle ) * unitVector3 );

    // Add the relative outgoing velocity to the swing-by body velocity and return it.
    return centralBodyVelocity + relativeOutgoingVelocity;
}

//! Propagate a powered gravity assist.
Eigen::Vector3d gravityAssist( const double centralBodyGravitationalParameter,
                               const Eigen::Vector3d& centralBodyVelocity,
                               const Eigen::Vector3d& incomingVelocity,
                               const double rotationAngle,
                               const double pericenterRadius,
                               const double deltaV )
{
    // Calculate the incoming velocity.
    const Eigen::Vector3d relativeIncomingVelocity = incomingVelocity - centralBodyVelocity;
    const double absoluteRelativeIncomingVelocity = relativeIncomingVelocity.norm( );

    // Calculate the incoming eccentricity and bending angle.
    const double incomingEccentricity = 1.0 + pericenterRadius /
                                        centralBodyGravitationalParameter *
                                        absoluteRelativeIncomingVelocity *
                                        absoluteRelativeIncomingVelocity;
    const double incomingBendingAngle = std::asin ( 1.0 / incomingEccentricity );

    // Calculate the pericenter velocities.
    const double incomingPericenterVelocity = std::sqrt( absoluteRelativeIncomingVelocity *
                                                         absoluteRelativeIncomingVelocity *
                                                         ( incomingEccentricity + 1.0 ) /
                                                         ( incomingEccentricity - 1.0 ) );
    const double outgoingPericenterVelocity = incomingPericenterVelocity + deltaV;

    // Calculate magnitude of the absolute relative outgoing velocity.
    const double absoluteRelativeOutgoingVelocity =
            std::sqrt( outgoingPericenterVelocity * outgoingPericenterVelocity -
                       2.0 * centralBodyGravitationalParameter / pericenterRadius );

    // Calculate the remaining bending angles.
    const double outgoingBendingAngle =
            std::asin ( 1.0 / ( 1.0 + absoluteRelativeOutgoingVelocity *
                                absoluteRelativeOutgoingVelocity * pericenterRadius /
                                centralBodyGravitationalParameter ) );
    const double bendingAngle = incomingBendingAngle + outgoingBendingAngle;

    // Calculate the unit vectors.
    const Eigen::Vector3d unitVector1 = relativeIncomingVelocity /
                                        absoluteRelativeIncomingVelocity;
    const Eigen::Vector3d unitVector2 = unitVector1.cross( centralBodyVelocity ).normalized( );
    const Eigen::Vector3d unitVector3 = unitVector1.cross( unitVector2 );

    // Calculate the relative outgoing velocity.
    const Eigen::Vector3d relativeOutgoingVelocity = absoluteRelativeOutgoingVelocity *
            ( std::cos( bendingAngle ) * unitVector1 + std::sin( bendingAngle ) *
              std::cos( rotationAngle ) * unitVector2 + std::sin( bendingAngle ) *
              std::sin( rotationAngle ) * unitVector3 );

    // Add the relative outgoing velocity to the swing-by body velocity and return it.
    return centralBodyVelocity + relativeOutgoingVelocity;
}

//! Compute pericenter radius function.
double PericenterFindingFunctions::computePericenterRadiusFunction( const double pericenterRadius )
{
    return std::asin( absoluteIncomingSemiMajorAxis_ / ( absoluteIncomingSemiMajorAxis_ +
                                                         pericenterRadius ) ) +
           std::asin( absoluteOutgoingSemiMajorAxis_ / ( absoluteOutgoingSemiMajorAxis_ +
                                                         pericenterRadius ) ) - bendingAngle_;
}

//! Compute first-derivative of the pericenter radius function.
double PericenterFindingFunctions::computeFirstDerivativePericenterRadiusFunction(
        const double pericenterRadius )
{
    return -absoluteIncomingSemiMajorAxis_ / ( absoluteIncomingSemiMajorAxis_ + pericenterRadius )
            / std::sqrt( ( pericenterRadius + 2.0 * absoluteIncomingSemiMajorAxis_ )
                         * pericenterRadius ) -
           absoluteOutgoingSemiMajorAxis_ / ( absoluteOutgoingSemiMajorAxis_ + pericenterRadius )
            / std::sqrt( ( pericenterRadius + 2.0 * absoluteOutgoingSemiMajorAxis_ ) *
                         pericenterRadius );
}

//! Compute incoming eccentricity function.
double EccentricityFindingFunctions::computeIncomingEccentricityFunction(
        const double incomingEccentricity )
{
    return std::asin( 1.0 / incomingEccentricity )
            + std::asin( 1.0 / ( 1.0 - incomingSemiMajorAxis_ / outgoingSemiMajorAxis_ *
                                 ( 1.0 - incomingEccentricity ) ) ) - bendingAngle_;
}

//! Compute first-derivative of the incoming eccentricity function.
double EccentricityFindingFunctions::computeFirstDerivativeIncomingEccentricityFunction(
        const double incomingEccentricity )
{
    const double eccentricitySquareMinusOne_ = incomingEccentricity * incomingEccentricity - 1.0;
    const double semiMajorAxisRatio_ = incomingSemiMajorAxis_ / outgoingSemiMajorAxis_ ;
    const double bParameter_ = 1.0 - semiMajorAxisRatio_ * ( 1.0 - incomingEccentricity );

    return -1.0 / ( incomingEccentricity * std::sqrt( eccentricitySquareMinusOne_ ) ) -
            semiMajorAxisRatio_ / ( bParameter_ * std::sqrt( bParameter_ * bParameter_ - 1.0 ) );
}

} // namespace mission_segments
} // namespace tudat