///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// 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 Industrial Light & Magic 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
// OWNER 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.
//
///////////////////////////////////////////////////////////////////////////

#include "PyIlmBaseConfigInternal.h"

#include "PyImathRandom.h"
#include "PyImathDecorators.h"
#include <Python.h>
#include <boost/python.hpp>
#include <boost/format.hpp>
#include <boost/python/make_constructor.hpp>
#include "PyImath.h"
#include "PyImathMathExc.h"
#include "PyImathFixedArray.h"

namespace PyImath{
using namespace boost::python;

template <class Rand, class T>
static T
nextf2 (Rand &rand, T min, T max)
{
    MATH_EXC_ON;
    return rand.nextf(min, max);
}

template <class Rand>
static float
nextGauss (Rand &rand)
{
    MATH_EXC_ON;
    return gaussRand(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}
template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class Rand>
static Rand *Rand_constructor1(unsigned long int seed)
{
    return new Rand(seed);
}

template <class Rand>
static Rand *Rand_constructor2(Rand rand)
{
    Rand *r = new Rand();
    *r = rand;

    return r;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
hollowSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
solidSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand32>
register_Rand32()
{
    float (IMATH_NAMESPACE::Rand32::*nextf1)(void) = &IMATH_NAMESPACE::Rand32::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;

    class_< IMATH_NAMESPACE::Rand32 > rand32_class("Rand32");
    rand32_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand32>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand32>))
        .def("init", &IMATH_NAMESPACE::Rand32::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand32::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", nextf1,
        	 "r.nextf() -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(float, float) -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand32, float>)

        .def("nextb", &IMATH_NAMESPACE::Rand32::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand32>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    def("hollowSphereRand",&hollowSphereRand<float,IMATH_NAMESPACE::Rand32>,"hollowSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed across the surface of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    def("solidSphereRand",&solidSphereRand<float,IMATH_NAMESPACE::Rand32>,"solidSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed through the volume of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    decoratecopy(rand32_class);

    return rand32_class;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand48>
register_Rand48()
{
    double (IMATH_NAMESPACE::Rand48::*nextf1)(void) = &IMATH_NAMESPACE::Rand48::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;

    class_< IMATH_NAMESPACE::Rand48 > rand48_class("Rand48");
    rand48_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand48>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand48>))
        .def("init", &IMATH_NAMESPACE::Rand48::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand48::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextf", nextf1,
        	 "r.nextf() -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(double,double) -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand48, double>)

        .def("nextb", &IMATH_NAMESPACE::Rand48::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand48>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    decoratecopy(rand48_class);

    return rand48_class;
}

//

PyObject *
Rand32::wrap (const IMATH_NAMESPACE::Rand32 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand32>::type converter;
    PyObject *p = converter (r);
    return p;
}

PyObject *
Rand48::wrap (const IMATH_NAMESPACE::Rand48 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand48>::type converter;
    PyObject *p = converter (r);
    return p;
}

} //namespace PyIMath


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/arithmetic/dec.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/control/expr_iif.hpp>
#include <boost/preprocessor/list/adt.hpp>
#include <boost/preprocessor/repetition/for.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <limits>
#include <pup.h>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"  // IWYU pragma: keep
#include "Utilities/TMPL.hpp"

/// \cond
class DataVector;
/// \endcond

namespace EquationsOfState {
/*!
 * \ingroup EquationsOfStateGroup
 * \brief Equation of state for a dark energy fluid
 *
 * A dark energy fluid equation of state:
 * \f[
 * p = w(z) \rho ( 1.0 + \epsilon)
 * \f]
 * where \f$\rho\f$ is the rest mass density, \f$\epsilon\f$ is the specific
 * internal energy, and \f$w(z) > 0\f$ is a parameter depending on the redshift
 * \f$z\f$.
 */
template <bool IsRelativistic>
class DarkEnergyFluid : public EquationOfState<IsRelativistic, 2> {
 public:
  static_assert(IsRelativistic,
                "Dark energy fluid equation of state only makes sense in a "
                "relativistic setting.");

  struct ParameterW {
    using type = double;
    static constexpr OptionString help = {"Parameter w(z)"};
  };

  static constexpr OptionString help = {
      "A dark energy fluid equation of state.\n"
      "The pressure is related to the rest mass density by "
      "p = w(z) * rho * (1 + epsilon), where p is the pressure, rho is the "
      "rest mass density, epsilon is the specific internal energy, and w(z) is "
      "a parameter."};

  using options = tmpl::list<ParameterW>;

  DarkEnergyFluid() = default;
  DarkEnergyFluid(const DarkEnergyFluid&) = default;
  DarkEnergyFluid& operator=(const DarkEnergyFluid&) = default;
  DarkEnergyFluid(DarkEnergyFluid&&) = default;
  DarkEnergyFluid& operator=(DarkEnergyFluid&&) = default;
  ~DarkEnergyFluid() override = default;

  explicit DarkEnergyFluid(double parameter_w) noexcept;

  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBERS(DarkEnergyFluid, 2)

  WRAPPED_PUPable_decl_base_template(  // NOLINT
      SINGLE_ARG(EquationOfState<IsRelativistic, 2>), DarkEnergyFluid);

 private:
  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBER_IMPLS(2)

  double parameter_w_ = std::numeric_limits<double>::signaling_NaN();
};

/// \cond
template <bool IsRelativistic>
PUP::able::PUP_ID EquationsOfState::DarkEnergyFluid<IsRelativistic>::my_PUP_ID =
    0;
/// \endcond
}  // namespace EquationsOfState


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include "BodyModel.hpp"
#include "Generator.hpp"
#include "utils/body.hpp"
#include "utils/matrix_helpers.hpp"

const float BodyModel::COM_HEIGHT = 200;
const float BodyModel::MOTION_DT = 1 / 100.0f;
const float BodyModel::FOOT_LENGTH = 160;
const float BodyModel::L_RATE = 0.5;

BodyModel::BodyModel() : filAccX(0.0),
                         filAccY(0.0),
                         filteredTotalPressure(0.0),
                         filHighZMPF(0.0),
                         filLowZMPF(0.0),
                         filZMPL(0.0)
{
   stepCounter = 0;
   sagittalAcceleration = 0.0;
   forwardR = forwardL = 0.0;

   isLeftPhase = false;
   isOnFront = false;
   lastIsLeftPhase = false;

   // real body model
   isStopped = false;

   //Used to AutoCalibrate Footsensors
	fsLfr = fsLfl = fsLrr = fsLrl = 0.1;                   // keeps maximum foot sensor readings
	fsRfr = fsRfl = fsRrr = fsRrl = 0.1;                   // small value deemed close to zero initially
}

void BodyModel::update(Odometry *odometry,
                       const SensorValues &sensors) {
   // Various Center of Pressure calcs to help stabilise walk (ZMP)
   stepCounter++;
   // Update maximum sensed foot-sensor readings
	float temp = sensors.sensors[Sensors::LFoot_FSR_FrontLeft]; if(fsLfl<temp and fsLfl<5.0) fsLfl = temp;
	temp = sensors.sensors[Sensors::LFoot_FSR_FrontLeft];       if(fsLfl<temp and fsLfl<5.0) fsLfl = temp;
	temp = sensors.sensors[Sensors::LFoot_FSR_FrontRight];      if(fsLfr<temp and fsLfr<5.0) fsLfr = temp;
	temp = sensors.sensors[Sensors::LFoot_FSR_RearLeft];        if(fsLrl<temp and fsLrl<5.0) fsLrl = temp;
	temp = sensors.sensors[Sensors::LFoot_FSR_RearRight];       if(fsLrr<temp and fsLrr<5.0) fsLrr = temp;
	temp = sensors.sensors[Sensors::RFoot_FSR_FrontLeft];       if(fsRfl<temp and fsRfl<5.0) fsRfl = temp;
	temp = sensors.sensors[Sensors::RFoot_FSR_FrontRight];      if(fsRfr<temp and fsRfr<5.0) fsRfr = temp;
	temp = sensors.sensors[Sensors::RFoot_FSR_RearLeft];        if(fsRrl<temp and fsRrl<5.0) fsRrl = temp;
	temp = sensors.sensors[Sensors::RFoot_FSR_RearRight];       if(fsRrr<temp and fsRrr<5.0) fsRrr = temp;
	lastZMPL = ZMPL;
	ZMPL = 0;
	// Calculate ZMPL (left-right) used to eg switch support foot in Walk2014
	float pressureL =
	  +sensors.sensors[Sensors::LFoot_FSR_FrontLeft]/fsLfl
	  + sensors.sensors[Sensors::LFoot_FSR_FrontRight]/fsLfr
	  + sensors.sensors[Sensors::LFoot_FSR_RearLeft]/fsLrl
	  + sensors.sensors[Sensors::LFoot_FSR_RearRight]/fsLrr;
	float pressureR =
	  +sensors.sensors[Sensors::RFoot_FSR_FrontLeft]/fsRfl
	  + sensors.sensors[Sensors::RFoot_FSR_FrontRight]/fsRfr
	  + sensors.sensors[Sensors::RFoot_FSR_RearLeft]/fsRrl
	  + sensors.sensors[Sensors::RFoot_FSR_RearRight]/fsRrr;
	float totalPressure = pressureL + pressureR;
	if (ABS(totalPressure) > 0.000001f) {
			ZMPL =
			(  .080 * sensors.sensors[Sensors::LFoot_FSR_FrontLeft]/fsLfl
			+ .030 * sensors.sensors[Sensors::LFoot_FSR_FrontRight]/fsLfr
			+ .080 * sensors.sensors[Sensors::LFoot_FSR_RearLeft]/fsLrl
			+ .030 * sensors.sensors[Sensors::LFoot_FSR_RearRight]/fsLrr
			- .030 * sensors.sensors[Sensors::RFoot_FSR_FrontLeft]/fsRfl
			- .080 * sensors.sensors[Sensors::RFoot_FSR_FrontRight]/fsRfr
			- .030 * sensors.sensors[Sensors::RFoot_FSR_RearLeft]/fsRrl
			- .080 * sensors.sensors[Sensors::RFoot_FSR_RearRight]/fsRrr) / totalPressure;
		}

   isOnFront = isOnFrontOfFoot(sensors);
   processUpdate(odometry, sensors);
   observationUpdate(odometry, sensors);
}

bool BodyModel::isFootOnGround(const SensorValues &sensors) {
   float leftFrontL = sensors.sensors[Sensors::LFoot_FSR_FrontLeft];
   float leftFrontR = sensors.sensors[Sensors::LFoot_FSR_FrontRight];
   float rightFrontL = sensors.sensors[Sensors::RFoot_FSR_FrontLeft];
   float rightFrontR = sensors.sensors[Sensors::RFoot_FSR_FrontRight];
   float leftRearL  = sensors.sensors[Sensors::LFoot_FSR_RearLeft];
   float leftRearR  = sensors.sensors[Sensors::LFoot_FSR_RearRight];
   float rightRearL = sensors.sensors[Sensors::RFoot_FSR_RearLeft];
   float rightRearR = sensors.sensors[Sensors::RFoot_FSR_RearRight];
   if (isLeftPhase) {
      return rightFrontL > 0.01 && rightFrontR > 0.01 &&
             rightRearL > 0.01 && rightRearR > 0.01;
   }
   return leftFrontL > 0.01 &&  leftFrontR > 0.01 &&
          leftRearL > 0.01 && leftRearR > 0.01;
}

bool BodyModel::isOnFrontOfFoot(const SensorValues &sensors) {
   float leftFront = sensors.sensors[Sensors::LFoot_FSR_FrontLeft] +
                     sensors.sensors[Sensors::LFoot_FSR_FrontRight];
   float rightFront = sensors.sensors[Sensors::RFoot_FSR_FrontLeft] +
                      sensors.sensors[Sensors::RFoot_FSR_FrontRight];
   float leftRear  = sensors.sensors[Sensors::LFoot_FSR_RearLeft] +
                     sensors.sensors[Sensors::LFoot_FSR_RearRight];
   float rightRear = sensors.sensors[Sensors::RFoot_FSR_RearLeft] +
                     sensors.sensors[Sensors::RFoot_FSR_RearRight];
   if (isLeftPhase) {
      return rightFront > rightRear;
   }
   return leftFront > leftRear;
}

float BodyModel::getFootZMP(bool isLeft, const SensorValues &sensors) {
   if (walkCycle.isDoubleSupportPhase()) return 0;
   float ZMPF = 0;
   float totalPressure = 0;
   if (isLeft) {
      totalPressure += sensors.sensors[Sensors::LFoot_FSR_FrontLeft]
                         + sensors.sensors[Sensors::LFoot_FSR_FrontRight]
                         + sensors.sensors[Sensors::LFoot_FSR_RearLeft]
                         + sensors.sensors[Sensors::LFoot_FSR_RearRight];
   } else {
      totalPressure += sensors.sensors[Sensors::RFoot_FSR_FrontLeft]
                         + sensors.sensors[Sensors::RFoot_FSR_FrontRight]
                         + sensors.sensors[Sensors::RFoot_FSR_RearLeft]
                         + sensors.sensors[Sensors::RFoot_FSR_RearRight];
   }
   if (isLeft) {
      ZMPF =
            (75.0f) * sensors.sensors[Sensors::LFoot_FSR_FrontLeft]
          + (-45.0f) * sensors.sensors[Sensors::LFoot_FSR_RearLeft]
          + (75.0f) * sensors.sensors[Sensors::LFoot_FSR_FrontRight]
          + (-45.0f) * sensors.sensors[Sensors::LFoot_FSR_RearRight];
   } else {
      ZMPF =
           (75.0f) * sensors.sensors[Sensors::RFoot_FSR_FrontRight]
         + (-45.0f) * sensors.sensors[Sensors::RFoot_FSR_RearRight]
         + (75.0f) * sensors.sensors[Sensors::RFoot_FSR_FrontLeft]
         + (-45.0f) * sensors.sensors[Sensors::RFoot_FSR_RearLeft];
   }
   if (totalPressure == 0) return  0;
   ZMPF /= totalPressure;
   return ZMPF;
}

float BodyModel::getHorizontalFootZMP(bool isLeft, const SensorValues &sensors) {
   float ZMPF = 0;
   float totalPressure = 0;
   if (isLeft) {
      totalPressure += sensors.sensors[Sensors::LFoot_FSR_FrontLeft]
                         + sensors.sensors[Sensors::LFoot_FSR_FrontRight]
                         + sensors.sensors[Sensors::LFoot_FSR_RearLeft]
                         + sensors.sensors[Sensors::LFoot_FSR_RearRight];
   } else {
      totalPressure += sensors.sensors[Sensors::RFoot_FSR_FrontLeft]
                         + sensors.sensors[Sensors::RFoot_FSR_FrontRight]
                         + sensors.sensors[Sensors::RFoot_FSR_RearLeft]
                         + sensors.sensors[Sensors::RFoot_FSR_RearRight];
   }
   if (isLeft) {
      ZMPF =
            (20.0f) * sensors.sensors[Sensors::LFoot_FSR_FrontLeft]
          + (20.0f) * sensors.sensors[Sensors::LFoot_FSR_RearLeft]
          + (-20.0f) * sensors.sensors[Sensors::LFoot_FSR_FrontRight]
          + (-20.0f) * sensors.sensors[Sensors::LFoot_FSR_RearRight];
   } else {
      ZMPF =
           (-20.0f) * sensors.sensors[Sensors::RFoot_FSR_FrontRight]
         + (-20.0f) * sensors.sensors[Sensors::RFoot_FSR_RearRight]
         + (20.0f) * sensors.sensors[Sensors::RFoot_FSR_FrontLeft]
         + (20.0f) * sensors.sensors[Sensors::RFoot_FSR_RearLeft];
   }
   if (totalPressure == 0) return  0;
   ZMPF /= totalPressure;
   return ZMPF;
}

void BodyModel::simulationUpdate() {}

void BodyModel::processUpdate(Odometry *odometry, const SensorValues &sensors) {
   float forwardL, forwardR, leftL, leftR, turnLR, liftL, liftR;
   walkCycle.generateWalk(forwardL, forwardR, leftL, leftR, turnLR, liftL, liftR);

   boost::numeric::ublas::matrix<float> b2f =
      kinematics->evaluateDHChain(
         Kinematics::FOOT,
         Kinematics::BODY,
         isLeftPhase ? Kinematics::RIGHT_CHAIN : Kinematics::LEFT_CHAIN);

   boost::numeric::ublas::matrix<float> b2fOther =
      kinematics->evaluateDHChain(
         Kinematics::FOOT,
         Kinematics::BODY,
         isLeftPhase ? Kinematics::LEFT_CHAIN : Kinematics::RIGHT_CHAIN);

   boost::numeric::ublas::matrix<float> n2f =
      kinematics->evaluateDHChain(
         Kinematics::FOOT,
         Kinematics::NECK,
         isLeftPhase ? Kinematics::RIGHT_CHAIN : Kinematics::LEFT_CHAIN);

   boost::numeric::ublas::matrix<float> f2w =
                  kinematics->createFootToWorldTransform(
         isLeftPhase ? Kinematics::RIGHT_CHAIN : Kinematics::LEFT_CHAIN);

   boost::numeric::ublas::matrix<float> b2w = boost::numeric::ublas::prod(f2w, b2f);
   boost::numeric::ublas::matrix<float> n2w = boost::numeric::ublas::prod(f2w, n2f);
   boost::numeric::ublas::matrix<float> origin = vec4<float>(0, 0, 0, 1);
   boost::numeric::ublas::matrix<float> result = boost::numeric::ublas::prod(b2f, origin);
   boost::numeric::ublas::matrix<float> rPend = boost::numeric::ublas::prod(b2w, origin);
   boost::numeric::ublas::matrix<float> neckPend = boost::numeric::ublas::prod(n2w, origin);
   // float deg = atan2(neckPend(0, 0) - rPend(0, 0), neckPend(2, 0) - rPend(2, 0));
//   std::cout << neckPend(0, 0) << " : " << rPend(0, 0) << " " << neckPend(1, 0) << " " << neckPend(2, 0) << " " << RAD2DEG(deg) << std::endl;

   // float alpha = sensors.sensors[Sensors::InertialSensor_AngleY];
   // float beta = deg; // sensors.joints.angles[Joints::LHipPitch];
   // float psi = alpha - beta;
   // float psiKinematic = atan2(rPend(0, 0), COM_HEIGHT);
   // float thau = psi - psiKinematic;

   // Calculate the centre of mass, convert to frame of reference of foot
   // (ie to foot then rotated by body lean)
   boost::numeric::ublas::matrix<float> com = kinematics->evaluateMassChain();
   boost::numeric::ublas::matrix<float> comOther = prod(b2fOther, com); // inserted CoM for other foot - BH
   centreOfMassOther.x = comOther(0, 0);                                // "
   centreOfMassOther.y = comOther(1, 0);                                // "
   centreOfMassOther.z = comOther(2, 0);                                // "
   com = prod(b2f, com);
   centreOfMass.x = com(0, 0);
   centreOfMass.y = com(1, 0);
   centreOfMass.z = com(2, 0);

   //std::cout << "COM x: " << centreOfMass.x << " y: " << centreOfMass.y << " z: " << centreOfMass.z << std::endl;

   //the following is left from 2011 and is not used atm

   // Get the ZMP from the supporting foot
   float zmpFootOffset = 0;
   if (!getWalkCycle().isDoubleSupportPhase()) {
      zmpFootOffset = getFootZMP(!isLeftPhase, sensors);
   }

   //prediction update
   pendulumModel.predictNext(1/100.0, zmpFootOffset);

   //std::cout << "##############" << std::endl;
   //std::cout << "prediction:   " << std::endl;
   //std::cout << "x: " << pendulumModel.x << std::endl;
   //std::cout << "dx: " << pendulumModel.dx << std::endl;
   //std::cout << "accX: " << pendulumModel.accX << std::endl;
   //std::cout << "theta: " << pendulumModel.theta << std::endl;
   //std::cout << "t: " << pendulumModel.walkCycle.t << std::endl;

   //observation update
   float supportFootPosition = result(0, 0);
   //float h = result(2, 0);
   //float deg = atan2(supportFootPosition - zmpFootOffset, h);
   WalkCycle currentWalkCycle = walkCycle;

   /*
   std::cout << "ZMP :" << zmpFootOffset << std::endl;
   std::cout << "observed x:" << supportFootPosition <<  std::endl;
   std::cout << "observed accX:" << sensors.sensors[Sensors::InertialSensor_AccX]
             << std::endl;
   */

   float alpha = 0.2;
   float beta = 0.5;

   pendulumModel.x = (1 - beta) * pendulumModel.x + (beta) * supportFootPosition;
   if (pendulumModel.walkCycle.leftPhase == currentWalkCycle.leftPhase) {
      pendulumModel.dx = (1 - alpha) * pendulumModel.dx + (alpha) * (pendulumModel.x - lastPendulumModel.x) * 100;
   } else { //switching foot
      pendulumModel.x = -pendulumModel.x/1.2;
   }
   pendulumModel.walkCycle = currentWalkCycle;

   lastPendulumModel.x = pendulumModel.x;
   lastPendulumModel.dx = pendulumModel.dx;

   /*
   std::cout << "updated:" << std::endl;
   std::cout << "x: " << pendulumModel.x << std::endl;
   std::cout << "dx: " << pendulumModel.dx << std::endl;
   std::cout << "t: " << pendulumModel.walkCycle.t << std::endl;
   */

   /*foot switched
   if (pendulumModel.walkCycle.leftPhase != currentWalkCycle.leftPhase) {
      if (!currentWalkCycle.leftPhase) {
         // switching to left support phase
         pendulumModel.x = pendulumModel.x + forwardR - forwardL;
      } else {
         pendulumModel.x = pendulumModel.x + forwardL - forwardR;
      }

   }*/
}

void BodyModel::observationUpdate(Odometry *odometry, const SensorValues &sensors) {
   /*
   float supportFootPosition = isLeftPhase ? forwardR : forwardL;
   // supportFootPosition += isOnFront ? FOOT_LENGTH/2 : -FOOT_LENGTH/2;
   float r = sqrt(supportFootPosition * supportFootPosition + COM_HEIGHT * COM_HEIGHT);

   float obsDTheta = lastHatDTheta +
                     sensors.sensors[Sensors::InertialSensor_AccX] * cos(theta) * MOTION_DT / r;
   dTheta += (obsDTheta - dTheta) * 0.5;
   */
}


/**
 * @brief       Battery_model
 * @file        Battery_model.cpp
 * @author      Richard Kirby <rkirby@kspresearch.com>
 * @copyright   Copyright (c) 2018, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */

#include "Battery_model.hpp"

#include <boost/math/tools/rational.hpp>

#include <cmath>
#include <cstdint>
#include <cstdio>
#include <limits>

namespace avionics_sim {

bool Battery_model::initialize(const double initial_soc, const double capacity, const uint8_t num_cells, const double C,
                               const double R) {
    // R must be > 0 or divide by zero error, set C = 0 to turn off transient response.
    if (!(R <= 0.0 || C < 0.0)) {
        m_c = C;
        m_r = R;
    }

    m_num_discharge_curves = 0;
    m_initial_soc = initial_soc;
    m_soc = m_initial_soc;  // in Ah
    m_capacity = capacity;  // in Ah
    m_num_cells = num_cells;
    m_low_current_curve_current = std::numeric_limits<double>::infinity();
    m_high_current_curve_current = -1;
    return true;
}

void Battery_model::update_soc(const double current, const double timestep, double *const voltage, double *const soc) {
    // simple coluomb counter
    if (timestep > 0.0) {
        m_soc += current * timestep / 3600;  // Time step is in seconds, need hours for mAhrs
        *voltage = get_voltage(m_soc * 1000, current, timestep);  // curves are in mA hours
        *soc = m_soc;
    } else {
        *voltage = m_last_voltage;
        *soc = m_soc;
    }
}

void Battery_model::update_soc_ocv(const double current, const double timestep, double *const voltage,
                                   double *const soc) {
    // simple coluomb counter
    if (timestep > 0.0 && current > 0.0) {
        m_soc += current * timestep / 3600;  // Time step is in seconds, need hours for mAhrs
        *voltage = get_voltage_ocv(m_soc, current, timestep);
        *soc = m_soc;
    } else {
        *voltage = m_last_voltage_ocv;
        *soc = m_soc;
    }
}

// discharge curves are in mAhrs
double Battery_model::get_voltage(const double soc, const double current_in, const double timestep) {
    double current = current_in;

    // Ensure that there are at least two discharge curves
    if (m_num_discharge_curves < 2) {
        return m_num_cells * m_last_voltage;
    }

    // Ensure current is between the two discharge curves
    if (current > m_high_current_curve_current) {
        current = m_high_current_curve_current;
    }

    if (current < m_low_current_curve_current) {
        current = m_low_current_curve_current;
    }

    // Find the curves that bracket the load
    int8_t low_current_discharge_curve = -1, high_current_discharge_curve = -1;
    double delta1 = std::numeric_limits<double>::infinity(), delta2 = std::numeric_limits<double>::infinity();

    for (uint8_t i = 0 ; i < m_num_discharge_curves ; i++) {
        if (m_discharge_curves[i].load <= current) {
            if (delta1 > (current - m_discharge_curves[i].load)) {
                delta1 = current - m_discharge_curves[i].load;
                low_current_discharge_curve = i;
            }
        }

        if (m_discharge_curves[i].load >= current) {
            if (delta2 > (m_discharge_curves[i].load - current)) {
                delta2 = m_discharge_curves[i].load - current;
                high_current_discharge_curve = i;
            }
        }
    }

    // The low voltage comes from the high discharge current and vice versa
    double voltage_low = compute_voltage(soc, high_current_discharge_curve);
    double voltage_high = compute_voltage(soc, low_current_discharge_curve);

    // If the load is bracketed by the same curve, then the load is equal to that curve otherwise interpolate
    double voltage;

    if (low_current_discharge_curve == high_current_discharge_curve) {
        voltage = voltage_low;
    } else {
        voltage = (
                      (current - m_discharge_curves[low_current_discharge_curve].load) /
                      (
                          m_discharge_curves[high_current_discharge_curve].load
                          - m_discharge_curves[low_current_discharge_curve].load
                      )
                  )
                  * (voltage_high - voltage_low) + voltage_low;
    }

    // add transient response
    if (timestep > 0) {
        voltage = voltage - (m_c / m_r) * (voltage - m_last_voltage) / (timestep);
        m_last_voltage = voltage;
    }

    return m_num_cells * voltage;
}

// discharge curves are in mAhrs
double Battery_model::get_voltage_ocv(const double soc, const double current_in, const double timestep) {
    double current = current_in;

    // get open circuit voltage, OCV uses curve 0
    double voltage = compute_voltage(soc, 0);   // returns voltage per cell

    // compensate for internal resistance drop
    float soc_percent = (m_capacity - soc) / m_capacity;
    double ir = compute_internal_resistance(soc_percent, 1.0);
    voltage = voltage - (ir * current) / m_num_cells;

    // add transient response
    if (timestep > 0) {
        voltage = voltage - (m_c / m_r) * (voltage - m_last_voltage_ocv) / (timestep);
        m_last_voltage_ocv = voltage;
    }

    return  m_num_cells * voltage;
}

bool Battery_model::add_discharge_curves(const discharge_curve &dc) {
    if (m_num_discharge_curves > max_num_discharge_curves) {
        return false;
    }

    m_discharge_curves.push_back(dc);

    if (m_discharge_curves[m_num_discharge_curves].load < m_low_current_curve_current) {
        m_low_current_curve_current = m_discharge_curves[m_num_discharge_curves].load;
        m_index_of_low_current_curve = m_num_discharge_curves;
    }

    if (m_discharge_curves[m_num_discharge_curves].load > m_high_current_curve_current) {
        m_high_current_curve_current = m_discharge_curves[m_num_discharge_curves].load;
        m_index_of_high_current_curve = m_num_discharge_curves;
    }

    m_num_discharge_curves++;
    return true;
}

bool Battery_model::set_internal_resistance_coefficients(const internal_resistance_curve &ir_coeff) {
    if (m_internal_resistance_curve.size() > 0) {
        return false;    // currently only one internal resistance curve is supported, but
    }

    // using a vector allows support for multiple internal resistance curves, which will change with state of health
    m_internal_resistance_curve.push_back(ir_coeff);
    return true;
}

double  Battery_model::compute_voltage(double soc, const uint8_t curve) {
    return boost::math::tools::evaluate_polynomial(m_discharge_curves[curve].c, soc);
}

double Battery_model::compute_internal_resistance(double soc, double soh) {
    // allows adding additional internal resistance curves for different states of health
    // currently only one curve is supported
    const uint8_t curve =  0;

    return (boost::math::tools::evaluate_polynomial(m_internal_resistance_curve[curve].c, soc,
            m_internal_resistance_num_coef) + m_internal_resistance_curve[curve].harness_resistance);
}

double Battery_model::compute_max_power_available(const double v_volts, const double ir_ohms) {
    return v_volts * v_volts / (4 * ir_ohms);
}

double Battery_model::get_soc_percent() {
    return (m_capacity - m_soc) / m_capacity;
}

}  // namespace avionics_sim


#ifndef BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP
#define BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// 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 <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/aasincos.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_zpoly1.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace mod_ster
    {

            static const double EPSLN = 1e-10;

            struct par_mod_ster
            {
                COMPLEX    *zcoeff;
                double    cchio, schio;
                int        n;
            };

            /* based upon Snyder and Linck, USGS-NMD */

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_mod_ster_ellipsoid : public base_t_fi<base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_mod_ster m_proj_parm;

                inline base_mod_ster_ellipsoid(const Parameters& par)
                    : base_t_fi<base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double sinlon, coslon, esphi, chi, schi, cchi, s;
                    COMPLEX p;

                    sinlon = sin(lp_lon);
                    coslon = cos(lp_lon);
                    esphi = this->m_par.e * sin(lp_lat);
                    chi = 2. * atan(tan((geometry::math::half_pi<double>() + lp_lat) * .5) *
                        pow((1. - esphi) / (1. + esphi), this->m_par.e * .5)) - geometry::math::half_pi<double>();
                    schi = sin(chi);
                    cchi = cos(chi);
                    s = 2. / (1. + this->m_proj_parm.schio * schi + this->m_proj_parm.cchio * cchi * coslon);
                    p.r = s * cchi * sinlon;
                    p.i = s * (this->m_proj_parm.cchio * schi - this->m_proj_parm.schio * cchi * coslon);
                    p = pj_zpoly1(p, this->m_proj_parm.zcoeff, this->m_proj_parm.n);
                    xy_x = p.r;
                    xy_y = p.i;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    int nn;
                    COMPLEX p, fxy, fpxy, dp;
                    double den, rh = 0, z, sinz = 0, cosz = 0, chi, phi = 0, dphi, esphi;

                    p.r = xy_x;
                    p.i = xy_y;
                    for (nn = 20; nn ;--nn) {
                        fxy = pj_zpolyd1(p, this->m_proj_parm.zcoeff, this->m_proj_parm.n, &fpxy);
                        fxy.r -= xy_x;
                        fxy.i -= xy_y;
                        den = fpxy.r * fpxy.r + fpxy.i * fpxy.i;
                        dp.r = -(fxy.r * fpxy.r + fxy.i * fpxy.i) / den;
                        dp.i = -(fxy.i * fpxy.r - fxy.r * fpxy.i) / den;
                        p.r += dp.r;
                        p.i += dp.i;
                        if ((fabs(dp.r) + fabs(dp.i)) <= EPSLN)
                            break;
                    }
                    if (nn) {
                        rh = boost::math::hypot(p.r, p.i);
                        z = 2. * atan(.5 * rh);
                        sinz = sin(z);
                        cosz = cos(z);
                        lp_lon = this->m_par.lam0;
                        if (fabs(rh) <= EPSLN) {
                            lp_lat = this->m_par.phi0;
                            return;
                        }
                        chi = aasin(cosz * this->m_proj_parm.schio + p.i * sinz * this->m_proj_parm.cchio / rh);
                        phi = chi;
                        for (nn = 20; nn ;--nn) {
                            esphi = this->m_par.e * sin(phi);
                            dphi = 2. * atan(tan((geometry::math::half_pi<double>() + chi) * .5) *
                                pow((1. + esphi) / (1. - esphi), this->m_par.e * .5)) - geometry::math::half_pi<double>() - phi;
                            phi += dphi;
                            if (fabs(dphi) <= EPSLN)
                                break;
                        }
                    }
                    if (nn) {
                        lp_lat = phi;
                        lp_lon = atan2(p.r * sinz, rh * this->m_proj_parm.cchio * cosz - p.i *
                            this->m_proj_parm.schio * sinz);
                    } else
                        lp_lon = lp_lat = HUGE_VAL;
                }

                static inline std::string get_name()
                {
                    return "mod_ster_ellipsoid";
                }

            };

            template <typename Parameters>
            void setup(Parameters& par, par_mod_ster& proj_parm)  /* general initialization */
            {
                double esphi, chio;

                if (par.es) {
                    esphi = par.e * sin(par.phi0);
                    chio = 2. * atan(tan((geometry::math::half_pi<double>() + par.phi0) * .5) *
                        pow((1. - esphi) / (1. + esphi), par.e * .5)) - geometry::math::half_pi<double>();
                } else
                    chio = par.phi0;
                proj_parm.schio = sin(chio);
                proj_parm.cchio = cos(chio);
            }

            // Miller Oblated Stereographic
            template <typename Parameters>
            void setup_mil_os(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX /* Miller Oblated Stereographic */
            AB[] = {
                {0.924500,    0.},
                {0.,            0.},
                {0.019430,    0.}
            };

                proj_parm.n = 2;
                par.lam0 = geometry::math::d2r<double>() * 20.;
                par.phi0 = geometry::math::d2r<double>() * 18.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                setup(par, proj_parm);
            }

            // Lee Oblated Stereographic
            template <typename Parameters>
            void setup_lee_os(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX /* Lee Oblated Stereographic */
            AB[] = {
                {0.721316,    0.},
                {0.,            0.},
                    {-0.0088162,     -0.00617325}
            };

                proj_parm.n = 2;
                par.lam0 = geometry::math::d2r<double>() * -165.;
                par.phi0 = geometry::math::d2r<double>() * -10.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                setup(par, proj_parm);
            }

            // Mod. Stererographics of 48 U.S.
            template <typename Parameters>
            void setup_gs48(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX /* 48 United States */
            AB[] = {
                {0.98879,    0.},
                {0.,        0.},
                {-0.050909,    0.},
                {0.,        0.},
                    {0.075528,    0.}
            };

                proj_parm.n = 4;
                par.lam0 = geometry::math::d2r<double>() * -96.;
                par.phi0 = geometry::math::d2r<double>() * -39.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                par.a = 6370997.;
                setup(par, proj_parm);
            }

            // Mod. Stererographics of Alaska
            template <typename Parameters>
            void setup_alsk(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX
            ABe[] = { /* Alaska ellipsoid */
                {.9945303,    0.},
                {.0052083,    -.0027404},
                {.0072721,    .0048181},
                {-.0151089,    -.1932526},
                {.0642675,    -.1381226},
                {.3582802,    -.2884586}},
            ABs[] = { /* Alaska sphere */
                {.9972523,    0.},
                {.0052513,    -.0041175},
                {.0074606,    .0048125},
                {-.0153783,    -.1968253},
                {.0636871,    -.1408027},
                    {.3660976,    -.2937382}
            };

                proj_parm.n = 5;
                par.lam0 = geometry::math::d2r<double>() * -152.;
                par.phi0 = geometry::math::d2r<double>() * 64.;
                if (par.es) { /* fixed ellipsoid/sphere */
                    proj_parm.zcoeff = ABe;
                    par.a = 6378206.4;
                    par.e = sqrt(par.es = 0.00676866);
                } else {
                    proj_parm.zcoeff = ABs;
                    par.a = 6370997.;
                }
                setup(par, proj_parm);
            }

            // Mod. Stererographics of 50 U.S.
            template <typename Parameters>
            void setup_gs50(Parameters& par, par_mod_ster& proj_parm)
            {
                static COMPLEX
            ABe[] = { /* GS50 ellipsoid */
                {.9827497,    0.},
                {.0210669,    .0053804},
                {-.1031415,    -.0571664},
                {-.0323337,    -.0322847},
                {.0502303,    .1211983},
                {.0251805,    .0895678},
                {-.0012315,    -.1416121},
                {.0072202,    -.1317091},
                {-.0194029,    .0759677},
                    {-.0210072,    .0834037}
            },
            ABs[] = { /* GS50 sphere */
                {.9842990,    0.},
                {.0211642,    .0037608},
                {-.1036018,    -.0575102},
                {-.0329095,    -.0320119},
                {.0499471,    .1223335},
                {.0260460,    .0899805},
                {.0007388,    -.1435792},
                {.0075848,    -.1334108},
                {-.0216473,    .0776645},
                    {-.0225161,    .0853673}
            };

                proj_parm.n = 9;
                par.lam0 = geometry::math::d2r<double>() * -120.;
                par.phi0 = geometry::math::d2r<double>() * 45.;
                if (par.es) { /* fixed ellipsoid/sphere */
                    proj_parm.zcoeff = ABe;
                    par.a = 6378206.4;
                    par.e = sqrt(par.es = 0.00676866);
                } else {
                    proj_parm.zcoeff = ABs;
                    par.a = 6370997.;
                }
                setup(par, proj_parm);
            }

        }} // namespace detail::mod_ster
    #endif // doxygen

    /*!
        \brief Miller Oblated Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_mil_os.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct mil_os_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline mil_os_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_mil_os(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Lee Oblated Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_lee_os.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct lee_os_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline lee_os_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_lee_os(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of 48 U.S. projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_gs48.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct gs48_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline gs48_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_gs48(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of Alaska projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_alsk.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct alsk_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline alsk_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_alsk(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of 50 U.S. projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_gs50.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct gs50_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>
    {
        inline gs50_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<Geographic, Cartesian, Parameters>(par)
        {
            detail::mod_ster::setup_gs50(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class mil_os_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<mil_os_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class lee_os_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<lee_os_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class gs48_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gs48_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class alsk_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<alsk_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class gs50_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gs50_ellipsoid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void mod_ster_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("mil_os", new mil_os_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("lee_os", new lee_os_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("gs48", new gs48_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("alsk", new alsk_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("gs50", new gs50_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP


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

#ifdef __SRRG_PARALLEL_KDTREE__
#include <omp.h>
#endif

namespace srrg2_core {

  template <class T, size_t D>
  KDTree_<T, D>::TreeNode::TreeNode(KDTree_<T, D>* tree_, int node_num) :
    _tree(tree_),
    _node_num(node_num),
    _node_type(KDTreeNodeType::Leaf) {
    _mean.setZero();
    _normal.setZero();
    _left_child  = 0;
    _right_child = 0;
    _min_index   = -1;
    _max_index   = -1;
    _num_points  = 0;
  }

  template <class T, size_t D>
  KDTree_<T, D>::TreeNode::TreeNode(KDTree_<T, D>* tree_,
                       int node_num,
                       const VectorTD& mean_,
                       const VectorTD& normal_,
                       TreeNode* left_child,
                       TreeNode* right_child) :
    _tree(tree_),
    _node_num(node_num),
    _node_type(KDTreeNodeType::Middle) {
    _min_index = _max_index = -1;
    assert(normal_.rows() == D);
    assert(mean_.rows() == D);
    _normal      = normal_;
    _mean        = mean_;
    _num_points  = 0;
    _left_child  = left_child;
    _right_child = right_child;
    if (_left_child) {
      _num_points += _left_child->_num_points;
    }
    if (_right_child) {
      _num_points += _right_child->_num_points;
    }
  }

  //! dtor
  template <class T, size_t D>
  KDTree_<T, D>::TreeNode::~TreeNode() {
    if (_left_child) {
      delete _left_child;
      _left_child = 0;
    }
    if (_right_child) {
      delete _right_child;
      _right_child = 0;
    }
  }

  template <class T, size_t D>
  void
  KDTree_<T, D>::TreeNode::
  findNeighbors(VectorTDVector& answers,
                std::vector<int>& indices,
                const VectorTD& query,
                const T maximum_distance,
                KDTreeSearchType search_type) const {
    assert(maximum_distance >= 0);
    const T maximum_squared_distance=pow(maximum_distance,2);
    switch (_node_type) {
    case KDTreeNodeType::Leaf: {
      // ds return all points in this leaf that satisfy the distance
      assert(_max_index >= _min_index);
      const size_t number_of_points = _max_index - _min_index;
      size_t s=answers.size();
      answers.reserve(s+number_of_points);
      indices.reserve(s+number_of_points);
      int k=0;
      for (size_t i = _min_index; i < _max_index; ++i) {
        assert(i < _tree->_points.size());
        const T squared_distance = (_tree->_points[i] - query).squaredNorm();
        if (squared_distance < maximum_squared_distance) {
          answers.emplace_back(_tree->_points[i]);
          indices.emplace_back(_tree->_indices[i]);
          ++k;
        }
      }
      //std::cerr << k << "/" << number_of_points << "/" << indices.size() << " " ;
      return;
    }

    case KDTreeNodeType::Middle: {
      T plane_distance = planeDistance(query);
      //std:: cerr << " - {" << plane_distance << "} ";
      if (search_type==Complete) {
        //std::cerr <<"2(";
        //std::cerr << "answers_size (before): " << answers.size() ;
        //std::cerr << "l(";
        _left_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << "answers_size (left): " << answers.size() << std::endl;
        //std::cerr << ")r(";
        _right_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << "answers_size (right): " << answers.size() << std::endl;
        //std::cerr <<"))";
      } else if (plane_distance < 0) {
        //std::cerr << "l(";
        _left_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << ")";
      } else {
        //std::cerr << "r(";
        _right_child->findNeighbors(answers, indices, query, maximum_distance, search_type);
        //std::cerr << ")";
      }
      return;
    }
    default:
      std::cerr << "KDTree::findNeighbors|ERROR, sanity check (" << _tree
                << ") : " << _tree->sanityCheck() << std::endl;
      throw std::runtime_error("KDTree::findNeighbors|ERROR, unknown node type");
    }
  }

  template <class T, size_t D>
  KDTree_<T, D>::KDTree_(const VectorTDVector& points_, T max_leaf_range, size_t min_leaf_points) {
      good_queries = 0;
      _num_nodes   = 0;
      _points      = points_;
      _aux_points  = points_;
      _indices.resize(points_.size());
      _aux_indices.resize(points_.size());
      _min_leaf_points = min_leaf_points;
      for (size_t i = 0; i < _indices.size(); ++i) {
        _indices[i] = i;
      }
      _root = _buildTree(0, points_.size(), max_leaf_range, 0);
    }

    //! dtor
  template <class T, size_t D>
  KDTree_<T, D>::~KDTree_() {
      if (_root) {
        delete _root;
      }
      _root = 0;
    }

  template <class T, size_t D>
  bool KDTree_<T, D>::sanityCheck() const {
    std::vector<int> checked_indices(_points.size());
    std::fill(checked_indices.begin(), checked_indices.end(), -1);
    const size_t k = sanityCheck(checked_indices, 0, _root);
    if (k != _points.size()) {
      throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, illegal size reported");
    }
    std::sort(checked_indices.begin(), checked_indices.end(), std::less<int>());
    for (size_t i = 0; i < checked_indices.size(); i++) {
      if (i != static_cast<size_t>(checked_indices[i])) {
        throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, missing indices");
        }
    }
    return true;
  }

  template <class T, size_t D>
  int KDTree_<T, D>::sanityCheck(std::vector<int>& checked_indices, int k, const TreeNode* node) const {
    if (!node) {
      return k;
    }
    switch (node->_node_type) {
    case KDTreeNodeType::Leaf:
      for (size_t i = node->_min_index; i < node->_max_index; i++) {
        int idx = _indices[i];
        if (checked_indices[k] != -1) {
          throw std::runtime_error("KDTree::sanityCheck|ERROR, writing on an occupied index");
        }
        checked_indices[k] = idx;
        k++;
      }
      return k;
    case KDTreeNodeType::Middle:
      k = sanityCheck(checked_indices, k, node->_left_child);
      k = sanityCheck(checked_indices, k, node->_right_child);
      return k;
    default:
      throw std::runtime_error("KDTree::sanityCheck|ERROR, illegal type index");
    }
  }

  template <class T, size_t D>
  void KDTree_<T, D>::findNeighbors(VectorTDVector& answers,
                     std::vector<int>& indices,
                     const KDTree_<T, D>::VectorTD& query,
                     const T max_distance,
                     KDTreeSearchType search_type) const {
    if (!_root) {
      throw std::runtime_error("KDTree::findNeighbors|ERROR, no root node");
    }
    answers.clear();
    indices.clear();
    good_queries++;
    _root->findNeighbors(answers, indices, query, max_distance, search_type);
    //std::cerr << "indices.size: " << indices.size() << std::endl;
  }

  template <class T, size_t D>
  T KDTree_<T, D>::findNeighbor(VectorTD& answer,
                                int& index,
                                const VectorTD& query,
                                const T max_distance,
                                const KDTreeSearchType search_type) const {
    if (!_root) {
      throw std::runtime_error("KDTree::findNeighbors|ERROR, no root node");
    }
    index=-1;

    // approx search, we take the quick path unrolling tail recursion
    T max_distance2 = pow(max_distance, 2);
    if (search_type==Approximate) {
      TreeNode* current=_root;
      while(! current->_node_type==Leaf) {
        assert(current->_node_type==Middle && "node is not middle");
        T plane_distance = current->planeDistance(query);
        if(plane_distance<0) {
          current=current->_left_child;
        } else {
          current=current->_right_child;
        }
      }
      assert(current->_node_type==Leaf && "node is leaf");
      assert(current->_max_index >= current->_min_index);
      T best_distance=std::numeric_limits<T>::max();
      for (size_t i = current->_min_index; i < current->_max_index; ++i) {
        const T squared_distance = (_points[i] - query).squaredNorm();
        if (squared_distance < best_distance && squared_distance < max_distance2) {
          best_distance=squared_distance;
          answer=_points[i];
          index=_indices[i];
        }
      }
      return best_distance;
    }

    //exact search, we need a buffer
    VectorTDVector answers;
    std::vector<int> indices;
    _root->findNeighbors(answers, indices, query, max_distance, search_type);
    T best_distance=std::numeric_limits<T>::max();
    for (size_t i=0; i<answers.size(); ++i) {
      T current_distance = (query-answers[i]).squaredNorm();
      if (current_distance<max_distance) {
        answer=answers[i];
        index=indices[i];
      }
    }
    return best_distance;
  }

  template <class T, size_t D>
  T KDTree_<T, D>::_splitPoints(KDTree_<T, D>::VectorTD& mean,
                               KDTree_<T, D>::VectorTD& normal,
                               size_t& num_left_points,
                               const size_t min_index,
                               const size_t max_index) {
    // if points empty, nothing to do
    if (min_index == max_index) {
      return 0;
    }

    const size_t num_points            = max_index - min_index;
    const T inverse_num_points         = 1.0 / num_points;
    VectorTD sum                       = VectorTD::Zero();
    Eigen::Matrix<T, D, D> squared_sum = Eigen::Matrix<T, D, D>::Zero();
    Eigen::Matrix<T, D, D> covariance  = Eigen::Matrix<T, D, D>::Zero();
    for (size_t i = min_index; i < max_index; ++i) {
      sum += _points[i];
      squared_sum += _points[i] * _points[i].transpose();
    }
    mean       = sum * inverse_num_points;
    covariance = squared_sum * inverse_num_points - mean * mean.transpose();

    // eigenvalue decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, D, D>> solver;
    solver.compute(covariance, Eigen::ComputeEigenvectors);
    normal = solver.eigenvectors().col(D - 1).normalized();

    // the following var will contain the range of points along the normal
    // vector
    T max_distance_from_plane = 0;

    // run through the points and split them in the left or the right set
    size_t left_index  = min_index;
    size_t right_index = max_index;

    size_t num_left  = 0;
    size_t num_right = 0;
    for (size_t i = min_index; i < max_index; ++i) {
      T distance_from_plane = normal.dot(_points[i] - mean);
      if (fabs(distance_from_plane) > max_distance_from_plane) {
        max_distance_from_plane = fabs(distance_from_plane);
      }

      bool side = distance_from_plane < 0;
      if (side) {
        _aux_points[left_index]  = _points[i];
        _aux_indices[left_index] = _indices[i];
        left_index++;
        num_left++;
      } else {
        right_index--;
        _aux_points[right_index]  = _points[i];
        _aux_indices[right_index] = _indices[i];
        num_right++;
      }
    }
    assert(max_index - min_index == num_right + num_left);
    for (size_t i = min_index; i < max_index; ++i) {
      _points[i]  = _aux_points[i];
      _indices[i] = _aux_indices[i];
    }

    num_left_points = num_left;
    return max_distance_from_plane;
  }

  template <class T, size_t D>
    typename KDTree_<T, D>::TreeNode*
    KDTree_<T, D>::_buildTree(const size_t min_index, const size_t max_index, const T max_leaf_range, int level) {
      const size_t num_points = max_index - min_index;
      if (!num_points) {
        return 0;
      }

      VectorTD mean;
      VectorTD normal;
      size_t num_left_points = 0;

      const T range = _splitPoints(mean, normal, num_left_points, min_index, max_index);
      assert(range >= 0);
      TreeNode* node = 0;
      if (range < max_leaf_range || num_points < _min_leaf_points) {
        node = new TreeNode(this, _num_nodes);
        node->_min_index=min_index;
        node->_max_index=max_index;
        node->_num_points = num_points;
        //std::cerr << "leaf: " << num_points << std::endl;
      } else {
        TreeNode *left_tree, *right_tree;

#ifdef __SRRG_PARALLEL_KDTREE__
        int num_threads = omp_get_max_threads();
        int split_level = -1;
        if (level > 0)
          split_level = floor(log(num_threads) / log(2));

        if (split_level == level) {
#pragma omp parallel sections
          {
#pragma omp section
            {
              left_tree =
                _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
            }
#pragma omp section
            {
              right_tree =
                _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
            }
          }
        } else {
#endif //__SRRG_PARALLEL_KDTREE__

          left_tree = _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
          right_tree =
            _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
#ifdef __SRRG_PARALLEL_KDTREE__
        }
#endif
        node = new TreeNode(this, _num_nodes, mean, normal, left_tree, right_tree);
      }
      _num_nodes++;
      return node;
    }

  template <class T, size_t D>
  void KDTree_<T, D>::_printKDTree(TreeNode* node) {
    std::cerr << "(";
    switch (node->_node_type) {
    case Leaf:
      std::cerr << node->_max_index - node->_min_index;
      // for (size_t i = node->_min_index; i < node->_max_index; ++i) {
      //   std::cerr << _points[i].transpose() << std::endl;
      // }
      break;
    case Middle:
      _printKDTree(node->_left_child);
      _printKDTree(node->_right_child);
      break;
    }
    std::cerr << ")";

  }

} // namespace srrg2_core


#include <iostream>
#include <armadillo>
#include <mpi.h>
#include <omp.h>
#include <mkl.h>

#include "class_data.hpp"
#include "class_approximation.hpp"
#include "class_partition.hpp"
#include "evaluate_covariance.hpp"
#include "constants.hpp"
#include <string.h>

/////////////////// Below are private member functions /////////////////

void Approximation::prior_allocate_memory()
{
    //Allocate memory for RChol
    RChol = new mat [partition->nRegionsInTotal];

    //Allocate memory for wTilde (first dimension: partition.nKnots, second dimesnion: partition.nLvs-1)
    wTilde = new mat [partition->nRegionsInTotal];

    //Allocate memory for ATilde (first dimension: partition.nKnots, second dimesnion: partition.nKnots, third dimension or the number of slices: (partition.nLvs-1)*partition.nLvs/2)
    ATilde = new cube [partition->nRegionsInTotal];

    //Assign paramter values if predicting
    if(CALCULATION_MODE == "prediction")
    {
        posteriorPredictionMean = new vec [partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];
        posteriorPredictionVariance = new vec [partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];
        BTilde = new double** [partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];
    }
}

void Approximation::get_all_ancestors(unsigned long* ancestorArray, unsigned long currentRegion, const unsigned long &nAncestors)
{
    for(unsigned long iAncestor = 0; iAncestor < nAncestors; iAncestor++)
    {
        currentRegion = (currentRegion-1)/NUM_PARTITIONS_J;
        ancestorArray[nAncestors-iAncestor-1] = currentRegion;
    }
}

//Update: Changed the column-row for KcW and covariance matrix. Need to modify prediction routines accordingly
void Approximation::get_conditional_covariance_matrix(const int &ancestorLevel, double **ancestorNow, double **ancestorBefore, double* nowBefore, const int &numKnotsNow, const int &numKnotsBefore, const int &numKnotsAncestor)
{
    for(int iLevel = 0; iLevel < ancestorLevel; iLevel++)
        cblas_dgemm(CblasColMajor,CblasTrans,CblasNoTrans,numKnotsBefore,numKnotsNow,numKnotsAncestor,-1.0,ancestorBefore[iLevel],numKnotsAncestor,ancestorNow[iLevel],numKnotsAncestor,1.0,nowBefore,numKnotsBefore);
}

void Approximation::get_wTilde_and_ATilde_in_prior(const int &nKnots, const int &currentLevel, const unsigned long &iRegion, vec *Sicy, mat **SicB)
{
    cube* ATildeCurrent;

    if(SAVE_TO_DISK_FLAG)
    {
        ATildeCurrent = new cube;
        (*ATildeCurrent).set_size(nKnots,nKnots,(NUM_LEVELS_M-1)*NUM_LEVELS_M/2);
    }

    unsigned long count = 0;
    for(int jLevel = 0; jLevel < currentLevel; jLevel++)
    {
        mat tmp = (*SicB[jLevel]).t();
        wTilde[iRegion].unsafe_col(jLevel) = tmp*(*Sicy);
        for(int kLevelAfterjLevel = jLevel; kLevelAfterjLevel < currentLevel; kLevelAfterjLevel++)
        {
            if(SAVE_TO_DISK_FLAG)
                (*ATildeCurrent).slice(count++) = tmp*(*SicB[kLevelAfterjLevel]);
            else
                ATilde[iRegion].slice(count++) = tmp*(*SicB[kLevelAfterjLevel]);
        }
    }

    if(SAVE_TO_DISK_FLAG)
    {
        std::string fileName=TMP_DIRECTORY+"/"+std::to_string(iRegion)+".bin";
        ATildeCurrent->save(fileName,arma_binary);
        delete ATildeCurrent;
    }
}

void Approximation::loop_regions_before_finest_level_in_prior(double ***KCholTimesw)
{
    int maxOpenMPThreads=omp_get_max_threads();

    unsigned long **ancestorArray = new unsigned long* [maxOpenMPThreads];
    for(int i = 0; i < maxOpenMPThreads; i++) ancestorArray[i] = new unsigned long [NUM_LEVELS_M-2];

    //Allocate memory for the current region of KCholTimesw
    for(int currentLevel = 0; currentLevel < NUM_LEVELS_M-1; currentLevel++)
    {
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            KCholTimesw[iRegion] = new double* [currentLevel];
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                KCholTimesw[iRegion][jLevel]= new double [partition->nKnots*partition->nKnots];

            RChol[iRegion].set_size(partition->nKnots,partition->nKnots);
        }
    }

    //Auxilliary variables for matrix computations
    char L='L', N='N';
    int nKnots = partition->nKnots;

    for(int currentLevel = 0; currentLevel < NUM_LEVELS_M-1; currentLevel++)
    {
        #pragma omp parallel for num_threads(maxOpenMPThreads) schedule(dynamic,1)
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            int rankOpenMP = omp_get_thread_num();
            int status;
            //Find the indices of all the ancestors

            get_all_ancestors(ancestorArray[rankOpenMP], iRegion, currentLevel);

            //Loop for all ancestors to get the conditional cross-covariance matrix
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
            {
                evaluate_cross_covariance(KCholTimesw[iRegion][jLevel], partition->knotsX[iRegion], partition->knotsY[iRegion], partition->knotsX[ancestorArray[rankOpenMP][jLevel]], partition->knotsY[ancestorArray[rankOpenMP][jLevel]], partition->nKnots, partition->nKnots, sill, range, nugget);

                get_conditional_covariance_matrix(jLevel, KCholTimesw[iRegion], KCholTimesw[ancestorArray[rankOpenMP][jLevel]],KCholTimesw[iRegion][jLevel],nKnots,nKnots,nKnots);

                dtrtrs_(&L,&N,&N,&nKnots,&nKnots,(RChol[ancestorArray[rankOpenMP][jLevel]]).memptr(),&nKnots,KCholTimesw[iRegion][jLevel],&nKnots,&status);
            }

            //Get the conditional variance-covariance matrix of this region
            evaluate_variance_covariance(RChol[iRegion].memptr(), partition->knotsX[iRegion], partition->knotsY[iRegion], partition->nKnots, sill, range, nugget);

            get_conditional_covariance_matrix(currentLevel, KCholTimesw[iRegion], KCholTimesw[iRegion], RChol[iRegion].memptr(),nKnots,nKnots,nKnots);

            //dpotrf_(&L,&nKnots,(RChol[iRegion]).memptr(),&nKnots,&status);
            RChol[iRegion] = chol(RChol[iRegion],"lower");

        }

        timeval timeNow;
        gettimeofday(&timeNow, NULL);

        if(WORKER == 0) cout<<"Prior: Level "<<currentLevel+1<<" is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    for(int i = 0; i < maxOpenMPThreads; i++) delete[] ancestorArray[i];
    delete[] ancestorArray; ancestorArray = NULL;
}

template<typename T> void Approximation::receive(T &object, double *tempMemory, int tag, unsigned long iRegion)
{
    std::set<unsigned long>::iterator workerToReceive = WORKERS_FOR_EACH_REGION[iRegion].begin();
    for(workerToReceive++; workerToReceive != WORKERS_FOR_EACH_REGION[iRegion].end(); workerToReceive++)
    {
        MPI_Recv(tempMemory,object.size(),MPI_DOUBLE,*workerToReceive,tag,WORLD,MPI_STATUS_IGNORE);
        for(unsigned long i = 0; i < object.size(); i++) object[i] += tempMemory[i];
    }
}

template<typename T> void Approximation::send(const T &object, int tag, unsigned long iRegion)
{
    MPI_Send(object.memptr(),object.size(),MPI_DOUBLE,*WORKERS_FOR_EACH_REGION[iRegion].begin(),tag,WORLD);
}

void Approximation::aggregate_A(mat &A, double *tempMemory, const unsigned long &indexA, const unsigned long &indexStart, const unsigned long &indexEnd, unsigned long iRegion, bool &supervisor, bool &synchronizeFlag)
{
    bool firstChild = true;

    for(unsigned long jChild = indexStart; jChild < indexEnd + 1; jChild++)
    {
        if( !WORKING_REGION_FLAG[jChild] )
        {
            synchronizeFlag = true;
            continue;
        }
        if(firstChild)
        {
            A = ATilde[jChild].slice(indexA);
            firstChild = false;
        }
        else
            A += ATilde[jChild].slice(indexA);
    }

    if(synchronizeFlag)
    {
        supervisor = WORKER == *WORKERS_FOR_EACH_REGION[iRegion].begin();
        //Check whether this worker is the master worker
        if(supervisor)
        {
            //Supervisor, to receive
            receive<mat>(A, tempMemory, MPI_TAG_MAT, iRegion);
        }
        else
        {
            //Not supervisor, to send
            send<mat>(A, MPI_TAG_MAT, iRegion);
        }
    }
}

void Approximation::aggregate_w_and_A(vec &w, mat &A, double *tempMemory, const int &jLevel, const unsigned long &indexA, const unsigned long &indexStart, const unsigned long &indexEnd, unsigned long iRegion, bool &supervisor, bool &synchronizeFlag)
{
    bool firstChild = true;
    for(unsigned long jChild = indexStart; jChild < indexEnd+1; jChild++)
    {
        if( !WORKING_REGION_FLAG[jChild] )
        {
            synchronizeFlag = true;
            continue;
        }
        if(firstChild)
        {
            w = wTilde[jChild].unsafe_col(jLevel);
            A = ATilde[jChild].slice(indexA);
            firstChild = false;
        }
        else
        {
            w += wTilde[jChild].unsafe_col(jLevel);
            A += ATilde[jChild].slice(indexA);
        }
    }

    if(synchronizeFlag)
    {
        //Check whether this worker is the supervisor
        supervisor = WORKER == *WORKERS_FOR_EACH_REGION[iRegion].begin();

        if(supervisor)
        {
            //Supervisor, to receive
            receive<vec>(w, tempMemory, MPI_TAG_VEC, iRegion);
            receive<mat>(A, tempMemory,  MPI_TAG_MAT, iRegion);
        }
        else
        {
            //Not supervisor, to send
            send<vec>(w, MPI_TAG_VEC, iRegion);
            send<mat>(A, MPI_TAG_MAT, iRegion);
        }
    }
}

void Approximation::get_wTilde_and_ATilde_in_posterior(vec &w, mat &A, double *tempMemory, const unsigned long &nKnots, cube &KCholTimesCurrentA, const vec &KCholTimesCurrentw, const unsigned long &iRegion, const int &currentLevel, bool &supervisor, bool &synchronizeFlag)
{
    if(currentLevel == 0) return;

    wTilde[iRegion].set_size(nKnots,currentLevel);
    ATilde[iRegion].set_size(nKnots,nKnots,(currentLevel+1)*currentLevel/2);

    unsigned long indexAChild = (currentLevel+1)*(currentLevel+2)/2-1;
    unsigned long indexACurrent = currentLevel*(currentLevel+1)/2;
    for(long jLevel = currentLevel-1; jLevel > -1; jLevel--)
    {
        aggregate_w_and_A(w, A, tempMemory, jLevel, --indexAChild, iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, iRegion, supervisor, synchronizeFlag);

        if(supervisor)
        {
            KCholTimesCurrentA.slice(jLevel) = solve(trimatl(RChol[iRegion]),A.t());

            wTilde[iRegion].unsafe_col(jLevel) = w-KCholTimesCurrentA.slice(jLevel).t()*KCholTimesCurrentw;
        }

        for(long kLevelAfterjLevel = currentLevel-1; kLevelAfterjLevel > jLevel-1; kLevelAfterjLevel--)
        {
            aggregate_A(A, tempMemory, --indexAChild, iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, iRegion, supervisor, synchronizeFlag);

            if(supervisor)
                ATilde[iRegion].slice(--indexACurrent) = A-KCholTimesCurrentA.slice(jLevel).t()*KCholTimesCurrentA.slice(kLevelAfterjLevel);
        }
    }
}

void Approximation::synchronizeIndicesRegionsForEachWorker()
{
    //Get the working region indices of this worker at the current level
    INDICES_REGIONS_AT_ONE_LEVEL_BELOW = INDICES_REGIONS_AT_CURRENT_LEVEL;
    INDICES_REGIONS_AT_CURRENT_LEVEL.clear();
    for(std::set<unsigned long>::iterator child = INDICES_REGIONS_AT_ONE_LEVEL_BELOW.begin(); child != INDICES_REGIONS_AT_ONE_LEVEL_BELOW.end(); child++)
    {
        unsigned long ancestor = (*child-1)/NUM_PARTITIONS_J;
        INDICES_REGIONS_AT_CURRENT_LEVEL.insert(ancestor);
        WORKING_REGION_FLAG[ancestor] = true;
    }

    if(INDICES_REGIONS_AT_CURRENT_LEVEL.size() > (unsigned long)INT_MAX)
    {
        cout<<"Program exits with an error: too many levels. It is required that the number of regions at the third finest level is smaller than the maximum value of type \"int\" "<<INT_MAX<<endl;
        exit(EXIT_FAILURE);
    }

    int *numWorkingRegionsEachWorker = new int [MPI_SIZE];
    int numWorkingRegionsThisWorker = INDICES_REGIONS_AT_CURRENT_LEVEL.size();

    unsigned long *indicesWorkingRegionsThisWorker = new unsigned long [numWorkingRegionsThisWorker];
    std::set<unsigned long>::iterator iter = INDICES_REGIONS_AT_CURRENT_LEVEL.begin();
    for(int i = 0; i < numWorkingRegionsThisWorker; i++)
    {
        indicesWorkingRegionsThisWorker[i] = *iter;
        iter++;
    }

    //Synchronize the number of working regions of each worker
    MPI_Allgather(&numWorkingRegionsThisWorker,1,MPI_INT,numWorkingRegionsEachWorker,1,MPI_INT,WORLD);

    //Get the max number of working regions among all workers
    int maxNumWorkingRegionsEachWorker = 0;
    for(int i = 0; i < MPI_SIZE; i++)
        if (maxNumWorkingRegionsEachWorker < numWorkingRegionsEachWorker[i]) maxNumWorkingRegionsEachWorker = numWorkingRegionsEachWorker[i];

    //Synchronize the working region indices of each worker
    unsigned long *indicesWorkingRegionsEachWorker = new unsigned long [(unsigned long)MPI_SIZE*maxNumWorkingRegionsEachWorker];
    int *offset = new int [MPI_SIZE];
    offset[0] = 0;
    for(int i = 1; i < MPI_SIZE; i++)
        offset[i] = offset [i-1] + maxNumWorkingRegionsEachWorker;

    MPI_Allgatherv(indicesWorkingRegionsThisWorker,numWorkingRegionsThisWorker,MPI_UNSIGNED_LONG,indicesWorkingRegionsEachWorker,numWorkingRegionsEachWorker,offset,MPI_UNSIGNED_LONG,WORLD);

    //Update WORKERS_FOR_EACH_REGION
    for(int iWorker = 0; iWorker < MPI_SIZE; iWorker++)
        for(int jIndex = offset[iWorker]; jIndex < offset[iWorker]+numWorkingRegionsEachWorker[iWorker]; jIndex++)
            WORKERS_FOR_EACH_REGION[indicesWorkingRegionsEachWorker[jIndex]].insert(iWorker);

    delete[] offset; offset = NULL;
    delete[] indicesWorkingRegionsEachWorker; indicesWorkingRegionsEachWorker = NULL;
    delete[] indicesWorkingRegionsThisWorker; indicesWorkingRegionsThisWorker = NULL;
    delete[] numWorkingRegionsEachWorker; numWorkingRegionsEachWorker = NULL;
}

void Approximation::free_wTilde_and_ATilde(const unsigned long &indexStartFreeingATilde, const unsigned long &indexEndFreeingATilde)
{
    //#pragma omp parallel for
    for(unsigned long i = indexStartFreeingATilde; i < indexEndFreeingATilde + 1; i++)
    {
        if( !WORKING_REGION_FLAG[i] ) continue;
        ATilde[i].reset();
        wTilde[i].reset();
    }
}

void Approximation::load_ATilde_from_disk(const unsigned long &indexChildrenStart, const unsigned long &indexChildrenEnd, bool secondLastLevel, unsigned long* nKnotsAtFinestLevel, unsigned long offset, unsigned long size1, unsigned long size2)
{
    for(unsigned long i = indexChildrenStart; i < indexChildrenEnd + 1; i++)
    {
        if( !WORKING_REGION_FLAG[i] ) continue;
        if( secondLastLevel && nKnotsAtFinestLevel[i-offset]==0)
        {
            ATilde[i].set_size(size1,size1,size2);
            ATilde[i].zeros();
            continue;
        }
        std::string fileName=TMP_DIRECTORY+"/"+std::to_string(i)+".bin";
        ATilde[i].load(fileName,arma_binary);
    }
}

/////////////////// Above are private member functions /////////////////

/////////////////// Below are public member functions /////////////////

Approximation::Approximation(const double &sill, const double &range, const double &nugget)
{
    (*this).sill=sill;
    (*this).range=range;
    (*this).nugget=nugget;
    loglikelihood = 0;
}

void Approximation::create_prior()
{
    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: calculating prior quantities starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    prior_allocate_memory();

    //Allocate memory for temporary variables storing K^{1/2}*W
    double ***KCholTimesw = new double** [partition->nRegionsInTotal];

    //Loop each region before the finest level
    loop_regions_before_finest_level_in_prior(KCholTimesw);

    //Loop each region in the finest level
    loop_regions_at_finest_level_in_prior(KCholTimesw);

    timeval timeNow;
    gettimeofday(&timeNow, NULL);

    if(WORKER == 0) cout<<"Prior: Level "<<NUM_LEVELS_M<<" is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";

    for(int currentLevel = 0; currentLevel < NUM_LEVELS_M-1; currentLevel++)
    {
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            for(int jLevel = 0; jLevel < currentLevel; jLevel++)
                delete[] KCholTimesw[iRegion][jLevel];
            delete[] KCholTimesw[iRegion];
        }
    }

    if(CALCULATION_MODE=="prediction")
    {
        for(unsigned long iRegion = REGION_START[NUM_LEVELS_M-1]; iRegion < REGION_END[NUM_LEVELS_M-1] + 1; iRegion++)
        {
            if(partition->nKnotsAtFinestLevel[iRegion-partition->nRegionsInTotal+partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]] == 0) continue;

            for(int jLevel = 0; jLevel < NUM_LEVELS_M - 1; jLevel++)
                delete[] KCholTimesw[iRegion][jLevel];
            delete[] KCholTimesw[iRegion];
        }
    }

    delete [] KCholTimesw; KCholTimesw=NULL;

    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: calculating prior quantities is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
    }
}

void Approximation::likelihood()
{
	create_prior();
	posterior_inference();
}

void Approximation::dump_prediction_result()
{
    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: dumping prediction results starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
    }

    std::ofstream file;
    char fileNameThisWorker[255];
    if(MPI_SIZE>1)
        sprintf(fileNameThisWorker,"%s%d",PREDICTION_RESULTS_FILE_NAME.c_str(),WORKER);
    else
        sprintf(fileNameThisWorker,"%s",PREDICTION_RESULTS_FILE_NAME.c_str());

    file.open(fileNameThisWorker, ios::binary);

    unsigned long offset = partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]-partition->nRegionsInTotal;
    if(file.is_open())
    {
        //Write the number of prediction locations
        unsigned long n = 0;
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
            n += partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel];
        file.write((char*)&n,sizeof(unsigned long));

        //Write the prediction longtitude
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
        {
            if(partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]==0) continue;
            file.write((char*)partition->predictionX[indexRegionAtThisLevel],sizeof(double)*partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]);
        }

        //Write the prediction lattitude
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
        {
            if(partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]==0) continue;
            file.write((char*)partition->predictionY[indexRegionAtThisLevel],sizeof(double)*partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]);
        }

        //Write the prediction mean
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
            for(unsigned long jPrediction = 0; jPrediction < partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]; jPrediction++)
            {
                vec designVec(3);
                designVec[1] = partition->predictionX[indexRegionAtThisLevel][jPrediction];
                designVec[2] = partition->predictionY[indexRegionAtThisLevel][jPrediction];
                designVec[0] = 1.0;
                double mean = posteriorPredictionMean[indexRegionAtThisLevel][jPrediction]+dot(designVec,data->coefficients);
                file.write((char*)&mean,sizeof(double));
            }

        //Write the prediction variance
        for(unsigned long indexRegionAtThisLevel = REGION_START[NUM_LEVELS_M-1]+offset; indexRegionAtThisLevel < REGION_END[NUM_LEVELS_M-1]+1+offset; indexRegionAtThisLevel++)
            for(unsigned long jPrediction = 0; jPrediction < partition->nPredictionsAtFinestLevel[indexRegionAtThisLevel]; jPrediction++)
                file.write((char*)&posteriorPredictionVariance[indexRegionAtThisLevel][jPrediction],sizeof(double));

        //Close the data file
        file.close();
    }else
    {
        cout<<"Program exits with an error: fail to open the output file "<<PREDICTION_RESULTS_FILE_NAME<<endl;
        exit(EXIT_FAILURE);
    }

    if(WORKER == 0)
    {
        gettimeofday(&timeNow, NULL);
        cout<<"===========================> Processor 1: dumping prediction results is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
    }
}

Approximation::~Approximation()
{
    delete[] RChol;

    delete[] wTilde;
    delete[] ATilde;

    delete[] KCholTimesCurrentw;
    delete[] KCholTimesCurrentA;

    if(CALCULATION_MODE == "prediction")
    {
        delete[] BTilde;

        delete[] posteriorPredictionMean;
        delete[] posteriorPredictionVariance;
    }
    //cout<<"Approximation is deleted\n";
}