#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "UKF/Core.h"
#include "ahrs.h"

/*
This is an implementation of an Unscented Kalman filter for a 9-axis AHRS,
using accelerometer, gyroscope and magnetometer to estimate attitude and
angular velocity.
*/

/* Value of g in m/s^2. */
#define G_ACCEL (9.80665)

/* Default magnetic field norm in Gauss. */
#define MAG_NORM (0.45)

enum AHRS_Keys {
    /* AHRS filter fields. */
    Attitude,
    AngularVelocity,

    /* Parameter estimation filter fields. */
    AccelerometerBias,
    GyroscopeBias,
    MagnetometerBias,
    MagnetometerScaleFactor,
    MagneticFieldNorm,
    MagneticFieldInclination,

    /* AHRS measurement vector fields. */
    Accelerometer,
    Gyroscope,
    Magnetometer
};

/*
The AHRS state vector contains the following:
- Attitude as a quaternion (NED frame to body frame)
- Angular velocity (body frame, rad/s)
*/
using AHRS_StateVector = UKF::StateVector<
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<AngularVelocity, UKF::Vector<3>>
>;

namespace UKF {
namespace Parameters {
template <> constexpr real_t AlphaSquared<AHRS_StateVector> = 1e-2;
template <> constexpr real_t Kappa<AHRS_StateVector> = 3.0;
}
}

/*
In addition to the AHRS filter, an online parameter estimation filter is also
implemented in order to calculate biases in each of the sensors.
The magnetometer scale factor is represented as a direction cosine matrix
with no normalisation constraint.
*/

using AHRS_SensorErrorVector = UKF::StateVector<
    UKF::Field<AccelerometerBias, UKF::Vector<3>>,
    UKF::Field<GyroscopeBias, UKF::Vector<3>>,
    UKF::Field<MagnetometerBias, UKF::Vector<3>>,
    UKF::Field<MagnetometerScaleFactor, UKF::Vector<3>>,
    UKF::Field<MagneticFieldNorm, real_t>,
    UKF::Field<MagneticFieldInclination, real_t>
>;

namespace UKF {
namespace Parameters {
template <> constexpr real_t AlphaSquared<AHRS_SensorErrorVector> = 1.0;
template <> constexpr real_t Kappa<AHRS_SensorErrorVector> = 3.0;
}

/* AHRS process model. */
template <> template <>
AHRS_StateVector AHRS_StateVector::derivative<>() const {
    AHRS_StateVector output;

    /* Calculate change in attitude. */
    UKF::Quaternion omega_q;
    omega_q.vec() = get_field<AngularVelocity>() * 0.5;
    omega_q.w() = 0;
    output.set_field<Attitude>(omega_q.conjugate() * get_field<Attitude>());

    /* Assume constant angular velocity. */
    output.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));

    return output;
}
}

using AHRS_MeasurementVector = UKF::DynamicMeasurementVector<
    UKF::Field<Accelerometer, UKF::Vector<3>>,
    UKF::Field<Gyroscope, UKF::Vector<3>>,
    UKF::Field<Magnetometer, UKF::Vector<3>>
>;

namespace UKF {
/*
This is the measurement model that's actually used in the filter, because
it's the one which takes the parameter estimation filter state as an input.
*/
template <> template <>
UKF::Vector<3> AHRS_MeasurementVector::expected_measurement
<AHRS_StateVector, Accelerometer, AHRS_SensorErrorVector>(
        const AHRS_StateVector& state, const AHRS_SensorErrorVector& input) {
    return input.get_field<AccelerometerBias>() + state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -G_ACCEL);
}

template <> template <>
UKF::Vector<3> AHRS_MeasurementVector::expected_measurement
<AHRS_StateVector, Gyroscope, AHRS_SensorErrorVector>(
        const AHRS_StateVector& state, const AHRS_SensorErrorVector& input) {
    return input.get_field<GyroscopeBias>() + state.get_field<AngularVelocity>();
}

template <> template <>
UKF::Vector<3> AHRS_MeasurementVector::expected_measurement
<AHRS_StateVector, Magnetometer, AHRS_SensorErrorVector>(
        const AHRS_StateVector& state, const AHRS_SensorErrorVector& input) {
    return input.get_field<MagnetometerBias>().array() + input.get_field<MagnetometerScaleFactor>().array() *
        (state.get_field<Attitude>() * UKF::Vector<3>(
            input.get_field<MagneticFieldNorm>() *
                std::cos(std::atan(input.get_field<MagneticFieldInclination>())),
            0.0,
            -input.get_field<MagneticFieldNorm>() *
                std::sin(std::atan(input.get_field<MagneticFieldInclination>())))).array();
}

}

using AHRS_Filter = UKF::SquareRootCore<
    AHRS_StateVector,
    AHRS_MeasurementVector,
    UKF::IntegratorHeun
>;

namespace UKF {
/*
AHRS parameter estimation filter process model. Since the evolution of sensor
errors is by definition unpredictable, this does nothing.
*/
template <> template <>
AHRS_SensorErrorVector AHRS_SensorErrorVector::derivative<>() const {
    return AHRS_SensorErrorVector::Zero();
}

/*
AHRS parameter estimation filter measurement model. These take in the current
state estimate and sensor scale factor and bias estimates, and use them to
calculate predicted measurements.
These functions are just the same as the state measurement model, but with
their arguments flipped.
*/
template <> template <>
UKF::Vector<3> AHRS_MeasurementVector::expected_measurement
<AHRS_SensorErrorVector, Accelerometer, AHRS_StateVector>(
        const AHRS_SensorErrorVector& state, const AHRS_StateVector& input) {
    return state.get_field<AccelerometerBias>() + input.get_field<Attitude>() * UKF::Vector<3>(0, 0, -G_ACCEL);
}

template <> template <>
UKF::Vector<3> AHRS_MeasurementVector::expected_measurement
<AHRS_SensorErrorVector, Gyroscope, AHRS_StateVector>(
        const AHRS_SensorErrorVector& state, const AHRS_StateVector& input) {
    return state.get_field<GyroscopeBias>() + input.get_field<AngularVelocity>();
}

template <> template <>
UKF::Vector<3> AHRS_MeasurementVector::expected_measurement
<AHRS_SensorErrorVector, Magnetometer, AHRS_StateVector>(
        const AHRS_SensorErrorVector& state, const AHRS_StateVector& input) {
    return state.get_field<MagnetometerBias>().array() + state.get_field<MagnetometerScaleFactor>().array() *
        (input.get_field<Attitude>() * UKF::Vector<3>(
            state.get_field<MagneticFieldNorm>() *
                std::cos(std::atan(state.get_field<MagneticFieldInclination>())),
            0.0,
            -state.get_field<MagneticFieldNorm>() *
                std::sin(std::atan(state.get_field<MagneticFieldInclination>())))).array();
}

}

/* Just use the Euler integrator since there's no process model. */
using AHRS_ParameterEstimationFilter = UKF::SquareRootParameterEstimationCore<
    AHRS_SensorErrorVector,
    AHRS_MeasurementVector
>;

static AHRS_Filter ahrs;
static AHRS_ParameterEstimationFilter ahrs_errors;
static AHRS_MeasurementVector meas;
static UKF::Vector<3> acceleration;

/*
The following functions provide a ctypes-compatible interface for ease of
testing.
*/

void ukf_init() {
    /* Initialise state vector and covariance. */
    ahrs.state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    ahrs.state.set_field<AngularVelocity>(UKF::Vector<3>(0, 0, 0));
    acceleration << UKF::Vector<3>(0, 0, 0);
    ahrs.root_covariance = AHRS_StateVector::CovarianceMatrix::Zero();
    ahrs.root_covariance.diagonal() <<
        1e0, 1e0, 3.2e0,
        1e-3 * UKF::Vector<3>::Ones();

    /* Set measurement noise covariance. */
    ahrs.measurement_root_covariance <<
        0.5 * UKF::Vector<3>::Ones(),
        0.004 * UKF::Vector<3>::Ones(),
        0.1 * UKF::Vector<3>::Ones();

    /* Set process noise covariance. */
    ahrs.process_noise_root_covariance = AHRS_StateVector::CovarianceMatrix::Zero();
    ahrs.process_noise_root_covariance.diagonal() <<
        5e-5 * UKF::Vector<3>::Ones(),
        5e-3 * UKF::Vector<3>::Ones();

    /* Initialise scale factor and bias errors. */
    ahrs_errors.state.set_field<AccelerometerBias>(UKF::Vector<3>(0, 0, 0));
    ahrs_errors.state.set_field<GyroscopeBias>(UKF::Vector<3>(0, 0, 0));
    ahrs_errors.state.set_field<MagnetometerBias>(UKF::Vector<3>(0, 0, 0));
    ahrs_errors.state.set_field<MagnetometerScaleFactor>(UKF::Vector<3>(1, 1, 1));
    ahrs_errors.state.set_field<MagneticFieldNorm>(MAG_NORM);
    ahrs_errors.state.set_field<MagneticFieldInclination>(0.0);

    /* Initialise scale factor and bias error covariance. */
    ahrs_errors.root_covariance = AHRS_SensorErrorVector::CovarianceMatrix::Zero();
    ahrs_errors.root_covariance.diagonal() <<
        0.8 * UKF::Vector<3>::Ones(),
        0.02 * UKF::Vector<3>::Ones(),
        5.0e-2 * UKF::Vector<3>::Ones(), 1.0e-1 * UKF::Vector<3>::Ones(),
        0.4, 0.7;

    /* Set measurement noise covariance. */
    ahrs_errors.measurement_root_covariance << ahrs.measurement_root_covariance;

    /*
    Set bias error process noise – this is derived from bias instability.

    Bias instability is actually characterised as a 1/f flicker noise rather
    than the white noise (which is what we're specifying using the process
    noise covariance), so these are tuned by hand to values which allow the
    filter to track biases over time, but not change too quickly.
    */
    ahrs_errors.process_noise_root_covariance = AHRS_SensorErrorVector::CovarianceMatrix::Zero();
    ahrs_errors.process_noise_root_covariance.diagonal() <<
        1e-5f * UKF::Vector<3>::Ones(),
        1e-7f * UKF::Vector<3>::Ones(),
        1e-5f * UKF::Vector<3>::Ones(), 1e-6f * UKF::Vector<3>::Ones(),
        1e-7f, 1e-7f;
}

void ukf_set_attitude(real_t w, real_t x, real_t y, real_t z) {
    ahrs.state.set_field<Attitude>(UKF::Quaternion(w, x, y, z));
}

void ukf_set_angular_velocity(real_t x, real_t y, real_t z) {
    ahrs.state.set_field<AngularVelocity>(UKF::Vector<3>(x, y, z));
}

void ukf_get_state(struct ukf_state_t *in) {
    in->attitude[0] = ahrs.state.get_field<Attitude>().x();
    in->attitude[1] = ahrs.state.get_field<Attitude>().y();
    in->attitude[2] = ahrs.state.get_field<Attitude>().z();
    in->attitude[3] = ahrs.state.get_field<Attitude>().w();
    in->angular_velocity[0] = ahrs.state.get_field<AngularVelocity>()[0];
    in->angular_velocity[1] = ahrs.state.get_field<AngularVelocity>()[1];
    in->angular_velocity[2] = ahrs.state.get_field<AngularVelocity>()[2];
    in->acceleration[0] = acceleration[0];
    in->acceleration[1] = acceleration[1];
    in->acceleration[2] = acceleration[2];
}

void ukf_set_state(struct ukf_state_t *in) {
    ahrs.state.set_field<Attitude>(
        UKF::Quaternion(in->attitude[3], in->attitude[0], in->attitude[1], in->attitude[2]));
    ahrs.state.set_field<AngularVelocity>(
        UKF::Vector<3>(in->angular_velocity[0], in->angular_velocity[1], in->angular_velocity[2]));
}

void ukf_get_state_covariance(
        real_t state_covariance[AHRS_StateVector::covariance_size()*AHRS_StateVector::covariance_size()]) {
    Eigen::Map<typename AHRS_StateVector::CovarianceMatrix> covariance_map(state_covariance);
    covariance_map = ahrs.root_covariance * ahrs.root_covariance.transpose();
}

void ukf_get_state_covariance_diagonal(
        real_t state_covariance_diagonal[AHRS_StateVector::covariance_size()]) {
    Eigen::Map<UKF::Vector<AHRS_StateVector::covariance_size()>> covariance_map(state_covariance_diagonal);
    covariance_map = (ahrs.root_covariance * ahrs.root_covariance.transpose()).diagonal();
}

void ukf_get_state_error(struct ukf_state_error_t *in) {
    AHRS_StateVector::StateVectorDelta state_error;
    state_error = (ahrs.root_covariance * ahrs.root_covariance.transpose()).cwiseAbs().rowwise().sum().cwiseSqrt();

    in->attitude[0] = state_error[0];
    in->attitude[1] = state_error[1];
    in->attitude[2] = state_error[2];
    in->angular_velocity[0] = state_error[3];
    in->angular_velocity[1] = state_error[4];
    in->angular_velocity[2] = state_error[5];
}

/*
This assumes accelerometer, gyroscope and magnetometer measurements are
present and in that order.
*/
void ukf_get_innovation(struct ukf_innovation_t *in) {
    in->accel[0] = ahrs.innovation[0];
    in->accel[1] = ahrs.innovation[1];
    in->accel[2] = ahrs.innovation[2];
    in->gyro[0] = ahrs.innovation[3];
    in->gyro[1] = ahrs.innovation[4];
    in->gyro[2] = ahrs.innovation[5];
    in->mag[0] = ahrs.innovation[6];
    in->mag[1] = ahrs.innovation[7];
    in->mag[2] = ahrs.innovation[8];
}

void ukf_sensor_clear() {
    meas = AHRS_MeasurementVector();
}

void ukf_sensor_set_accelerometer(real_t x, real_t y, real_t z) {
    meas.set_field<Accelerometer>(UKF::Vector<3>(x, y, z));
}

void ukf_sensor_set_gyroscope(real_t x, real_t y, real_t z) {
    meas.set_field<Gyroscope>(UKF::Vector<3>(x, y, z));
}

void ukf_sensor_set_magnetometer(real_t x, real_t y, real_t z) {
    meas.set_field<Magnetometer>(UKF::Vector<3>(x, y, z));
}

void ukf_set_params(struct ukf_sensor_params_t *in) {
    ahrs.measurement_root_covariance <<
        std::sqrt(in->accel_covariance[0]), std::sqrt(in->accel_covariance[1]), std::sqrt(in->accel_covariance[2]),
        std::sqrt(in->gyro_covariance[0]), std::sqrt(in->gyro_covariance[1]), std::sqrt(in->gyro_covariance[2]),
        std::sqrt(in->mag_covariance[0]), std::sqrt(in->mag_covariance[1]), std::sqrt(in->mag_covariance[2]);
    ahrs_errors.measurement_root_covariance << ahrs.measurement_root_covariance;
}

void ukf_iterate(float dt) {
    /*
    Split the parameter estimation filter into a priori and a posteriori
    steps, to reduce the CPU load each iteration.
    */
    static int step = 0;

    switch(step++) {
        case 0:
            /*
            The time delta is not used by the parameter estimation filter, so
            there's no need to adjust it.
            */
            ahrs_errors.a_priori_step();
            ahrs_errors.innovation_step(meas, ahrs.state);
            break;
        case 1:
            ahrs_errors.a_posteriori_step();

            /* Clip parameters to physically reasonable values. */
            ahrs_errors.state.set_field<MagneticFieldNorm>(
                std::max(0.2f, std::min(0.7f, ahrs_errors.state.get_field<MagneticFieldNorm>())));

            UKF::Vector<3> temp;
            temp = ahrs_errors.state.get_field<AccelerometerBias>();
            temp[0] = std::max(real_t(-G_ACCEL/4.0), std::min(real_t(G_ACCEL/4.0), temp[0]));
            temp[1] = std::max(real_t(-G_ACCEL/4.0), std::min(real_t(G_ACCEL/4.0), temp[1]));
            temp[2] = std::max(real_t(-G_ACCEL/4.0), std::min(real_t(G_ACCEL/4.0), temp[2]));
            ahrs_errors.state.set_field<AccelerometerBias>(temp);

            temp = ahrs_errors.state.get_field<MagnetometerScaleFactor>();
            temp[0] = std::max(real_t(0.5f), std::min(2.0f, temp[0]));
            temp[1] = std::max(real_t(0.5f), std::min(2.0f, temp[1]));
            temp[2] = std::max(real_t(0.5f), std::min(2.0f, temp[2]));
            ahrs_errors.state.set_field<MagnetometerScaleFactor>(temp);

            step = 0;
            break;
    }

    /*
    Do a normal iteration for the AHRS filter, with the current state of
    the parameter estimation filter as the measurement input.
    */
    ahrs.a_priori_step(dt);
    ahrs.innovation_step(meas, ahrs_errors.state);
    ahrs.a_posteriori_step();

    acceleration << meas.get_field<Accelerometer>() - ahrs_errors.state.get_field<AccelerometerBias>() -
        (ahrs.state.get_field<Attitude>() * UKF::Vector<3>(0, 0, -G_ACCEL));
}

void ukf_set_process_noise(real_t process_noise_covariance[AHRS_StateVector::covariance_size()]) {
    Eigen::Map<typename AHRS_StateVector::StateVectorDelta> covariance_map(process_noise_covariance);
    ahrs.process_noise_root_covariance = AHRS_StateVector::CovarianceMatrix::Zero();
    ahrs.process_noise_root_covariance.diagonal() << covariance_map;
    ahrs.process_noise_root_covariance = ahrs.process_noise_root_covariance.llt().matrixU();
}

void ukf_get_parameters(struct ukf_sensor_errors_t *in) {
    in->accel_bias[0] = ahrs_errors.state.get_field<AccelerometerBias>()[0];
    in->accel_bias[1] = ahrs_errors.state.get_field<AccelerometerBias>()[1];
    in->accel_bias[2] = ahrs_errors.state.get_field<AccelerometerBias>()[2];
    in->gyro_bias[0] = ahrs_errors.state.get_field<GyroscopeBias>()[0];
    in->gyro_bias[1] = ahrs_errors.state.get_field<GyroscopeBias>()[1];
    in->gyro_bias[2] = ahrs_errors.state.get_field<GyroscopeBias>()[2];
    in->mag_bias[0] = ahrs_errors.state.get_field<MagnetometerBias>()[0];
    in->mag_bias[1] = ahrs_errors.state.get_field<MagnetometerBias>()[1];
    in->mag_bias[2] = ahrs_errors.state.get_field<MagnetometerBias>()[2];
    in->mag_scale[0] = ahrs_errors.state.get_field<MagnetometerScaleFactor>()[0];
    in->mag_scale[1] = ahrs_errors.state.get_field<MagnetometerScaleFactor>()[1];
    in->mag_scale[2] = ahrs_errors.state.get_field<MagnetometerScaleFactor>()[2];
    in->mag_field_norm = ahrs_errors.state.get_field<MagneticFieldNorm>();
    in->mag_field_inclination = std::atan(ahrs_errors.state.get_field<MagneticFieldInclination>());
}

void ukf_get_parameters_error(struct ukf_sensor_errors_t *in) {
    AHRS_SensorErrorVector::StateVectorDelta parameters_error;
    parameters_error =
        (ahrs_errors.root_covariance * ahrs_errors.root_covariance.transpose()).cwiseAbs().rowwise().sum().cwiseSqrt();

    in->accel_bias[0] = parameters_error[0];
    in->accel_bias[1] = parameters_error[1];
    in->accel_bias[2] = parameters_error[2];
    in->gyro_bias[0] = parameters_error[3];
    in->gyro_bias[1] = parameters_error[4];
    in->gyro_bias[2] = parameters_error[5];
    in->mag_bias[0] = parameters_error[6];
    in->mag_bias[1] = parameters_error[7];
    in->mag_bias[2] = parameters_error[8];
    in->mag_scale[0] = parameters_error[9];
    in->mag_scale[1] = parameters_error[10];
    in->mag_scale[2] = parameters_error[11];
    in->mag_field_norm = parameters_error[12];
    in->mag_field_inclination = parameters_error[13];
}

uint32_t ukf_config_get_state_dim() {
    return AHRS_StateVector::covariance_size();
}

uint32_t ukf_config_get_measurement_dim() {
    return AHRS_MeasurementVector::max_size();
}

enum ukf_precision_t ukf_config_get_precision() {
    if(sizeof(real_t) == 8) {
        return UKF_PRECISION_DOUBLE;
    } else {
        return UKF_PRECISION_FLOAT;
    }
}


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

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

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

// IWYU pragma: no_forward_declare Tensor

namespace EquationsOfState {
/*!
 * \ingroup EquationsOfStateGroup
 * \brief A spectral equation of state
 *
 * This equation of state is determined as a function of \f$x =
 * \ln(\rho/\rho_0)\f$ where \f$\rho\f$ is the rest mass density and
 * \f$\rho_0\f$ is the provided reference density.  The adiabatic
 * index \f$\Gamma(x)\f$ is defined such that
 * \f{equation}{
 * \frac{d \ln p}{dx} = \Gamma(x) = \sum_{n=0}^N
 * \gamma_n x^n
 * \f}
 *
 * for the set of spectral coefficinets \f$\gamma_n\f$ when
 * \f$0 < x < x_u = \ln(\rho_u/\rho_0)\f$, where \f$\rho_u\f$ is the provided
 * upper density.
 *
 * For \f$ x < 0 \f$, \f$ \Gamma(x) = \gamma_0 \f$.
 *
 * For \f$ x > x_u \f$, \f$ \Gamma(x) = \Gamma(x_u) \f$
 *
 *
 */
class Spectral : public EquationOfState<true, 1> {
 public:
  static constexpr size_t thermodynamic_dim = 1;
  static constexpr bool is_relativistic = true;

  struct ReferenceDensity {
    using type = double;
    static constexpr Options::String help = {"Reference density rho_0"};
    static double lower_bound() { return 0.0; }
  };

  struct ReferencePressure {
    using type = double;
    static constexpr Options::String help = {"Reference pressure p_0"};
    static double lower_bound() { return 0.0; }
  };

  struct Coefficients {
    using type = std::vector<double>;
    static constexpr Options::String help = {"Spectral coefficients gamma_i"};
  };

  struct UpperDensity {
    using type = double;
    static constexpr Options::String help = {"Upper density rho_u"};
    static double lower_bound() { return 0.0; }
  };

  static constexpr Options::String help = {
      "A spectral equation of state.  Defining x = log(rho/rho_0), Gamma(x) = "
      "Sum_i gamma_i x^i, then the pressure is determined from d(log P)/dx = "
      "Gamma(x) for x > 0.  For x < 0 the EOS is a polytrope with "
      "Gamma(x)=Gamma(0).  For x > x_u = log(rho_u/rho_0), Gamma(x) = "
      "Gamma(x_u).\n"
      "To get smooth equations of state, it is recommended that the second "
      "and third supplied coefficient should be 0. It is up to the user to "
      "choose coefficients that are physically reasonable, e.g. that "
      "satisfy causality."};

  using options = tmpl::list<ReferenceDensity, ReferencePressure, Coefficients,
                             UpperDensity>;

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

  Spectral(double reference_density, double reference_pressure,
           std::vector<double> coefficients, double upper_density);

  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBERS(Spectral, 1)

  WRAPPED_PUPable_decl_base_template(  // NOLINT
      SINGLE_ARG(EquationOfState<true, 1>), Spectral);

  /// The lower bound of the rest mass density that is valid for this EOS
  double rest_mass_density_lower_bound() const override { return 0.0; }

  /// The upper bound of the rest mass density that is valid for this EOS
  double rest_mass_density_upper_bound() const override {
    return std::numeric_limits<double>::max();
  }

  /// The lower bound of the specific internal energy that is valid for this EOS
  /// at the given rest mass density \f$\rho\f$
  double specific_internal_energy_lower_bound(
      const double /* rest_mass_density */) const override {
    return 0.0;
  }

  /// The upper bound of the specific internal energy that is valid for this EOS
  /// at the given rest mass density \f$\rho\f$
  double specific_internal_energy_upper_bound(
      const double /* rest_mass_density */) const override {
    return std::numeric_limits<double>::max();
  }

  /// The lower bound of the specific enthalpy that is valid for this EOS
  double specific_enthalpy_lower_bound() const override { return 1.0; }

 private:
  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBER_IMPLS(1)

  double gamma(const double x) const;
  double integral_of_gamma(const double x) const;
  double chi_from_density(const double density) const;
  double specific_internal_energy_from_density(const double density) const;
  double specific_enthalpy_from_density(const double density) const;
  double pressure_from_density(const double density) const;
  double pressure_from_log_density(const double x) const;
  double rest_mass_density_from_enthalpy(const double specific_enthalpy) const;

  double reference_density_ = std::numeric_limits<double>::signaling_NaN();
  double reference_pressure_ = std::numeric_limits<double>::signaling_NaN();
  std::vector<double> integral_coefficients_{};
  std::vector<double> gamma_coefficients_{};
  double x_max_ = std::numeric_limits<double>::signaling_NaN();
  double gamma_of_x_max_ = std::numeric_limits<double>::signaling_NaN();
  double integral_of_gamma_of_x_max_ =
      std::numeric_limits<double>::signaling_NaN();
  std::vector<double> table_of_specific_energies_{};
  // Information for Gaussian quadrature
  size_t number_of_quadrature_coefs_ =
      std::numeric_limits<size_t>::signaling_NaN();
  std::vector<double> quadrature_weights_{};
  std::vector<double> quadrature_points_{};
};

}  // namespace EquationsOfState


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include <exception>
#include <stdexcept>

#include <boost/utility.hpp>

namespace scm {
namespace data {
namespace detail {

/*

template<typename val_type>
bool build_lookup_table<val_type>(boost::scoped_array<val_type>& dst,
                                  const piecewise_function_weighted_1d<unsigned char, val_type>& scal_trafu,
                                  unsigned size)
{
    if (size < 1) {
        return (false);
    }

    float    dst_ind_scal_factor = float(size - 1) / 255.0f;

    unsigned dst_ind_begin;
    unsigned dst_ind_end;

    float    dst_ind_begin_weight;
    val_type dst_ind_begin_value;
    val_type dst_ind_end_value;

    float lerp_factor;
    float lerp_factor_w;
    float part_step_size;

    // clear beginning
    if (scal_trafu.empty()) {
        dst_ind_begin = 0;
        dst_ind_end   = size - 1;
    }
    else {
        dst_ind_begin   = 0;
        dst_ind_end     = unsigned(math::floor(float(scal_trafu.stops_begin()->first) * dst_ind_scal_factor));
    }

    for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
        dst[dst_ind] = val_type(0);
    }

    // fill lookup table
    for (scm::piecewise_function_weighted_1d<unsigned char, val_type>::const_stop_iterator it_left = scal_trafu.stops_begin();
         it_left  != scal_trafu.stops_end();
         ++it_left) {

        dst_ind_begin           = unsigned(math::floor(float(it_left->first) * dst_ind_scal_factor));
        dst_ind_begin_value     = it_left->second._value;
        dst_ind_begin_weight    = it_left->second._weight;

        scm::piecewise_function_weighted_1d<unsigned char, val_type>::const_stop_iterator it_right = boost::next(it_left);
        if (it_right != scal_trafu.stops_end()) {
            dst_ind_end         = unsigned(math::floor(float(it_right->first) * dst_ind_scal_factor));
            dst_ind_end_value   = it_right->second._value;
        }
        else {
            dst_ind_end         = dst_ind_begin + 1;
            dst_ind_end_value   = dst_ind_begin_value;
        }

        part_step_size = 1.0f / float(dst_ind_end - dst_ind_begin);
        lerp_factor = 0.0f;
        lerp_factor_w = 0.0f;

        for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            //lerp_factor = math::shoothstep(dst_ind_begin, dst_ind_end, dst_ind);
            //// smoothstep
            //float s = math::clamp(float(dst_ind-dst_ind_begin)/float(dst_ind_end-dst_ind_begin), 0.0f, 1.0f);
            //s = scm::detail::non_linear_value_weight<float>(s, dst_ind_begin_weight);
            //lerp_factor_w = (s*s*(3.0f-2.0f*s));

            lerp_factor_w = scm::detail::non_linear_value_weight<float>(lerp_factor, dst_ind_begin_weight);
            dst[dst_ind] = math::lerp(dst_ind_begin_value, dst_ind_end_value, lerp_factor_w);
            lerp_factor += part_step_size;
        }
    }

    // clear end
    for (unsigned dst_ind = dst_ind_end; dst_ind < size; ++dst_ind) {
        dst[dst_ind] = val_type(0);
    }

    // original code
    //float a;
    //float step = 255.0f / float(size - 1);
    //for (unsigned i = 0; i < size; i++) {
    //    a = float(i) * step;
    //    dst[i] = scal_trafu[a];
    //}

    return (true);
}

*/

template<typename val_type>
struct build_lookup_table_impl<val_type, unsigned char>
{
    static bool build_table(boost::scoped_array<val_type>& dst,
                            const piecewise_function_1d<unsigned char, val_type>& scal_trafu,
                            unsigned size)
    {
        using namespace scm::math;

        if (size < 1) {
            return (false);
        }

        float    dst_ind_scal_factor = float(size - 1) / 255.0f;

        unsigned dst_ind_begin;
        unsigned dst_ind_end;

        val_type dst_ind_begin_value;
        val_type dst_ind_end_value;

        float lerp_factor;
        float part_step_size;

        // clear beginning
        if (scal_trafu.empty()) {
            dst_ind_begin = 0;
            dst_ind_end   = size - 1;
        }
        else {
            dst_ind_begin   = 0;
            dst_ind_end     = unsigned(floor(float(scal_trafu.stops_begin()->first) * dst_ind_scal_factor));
        }

        for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // fill lookup table
        for (typename scm::data::piecewise_function_1d<unsigned char, val_type>::const_stop_iterator it_left = scal_trafu.stops_begin();
            it_left  != scal_trafu.stops_end();
            ++it_left) {

            dst_ind_begin       = unsigned(floor(float(it_left->first) * dst_ind_scal_factor));
            dst_ind_begin_value = it_left->second;

            typename scm::data::piecewise_function_1d<unsigned char, val_type>::const_stop_iterator it_right = boost::next(it_left);
            if (it_right != scal_trafu.stops_end()) {
            dst_ind_end         = unsigned(floor(float(it_right->first) * dst_ind_scal_factor));
            dst_ind_end_value   = it_right->second;
            }
            else {
            dst_ind_end         = dst_ind_begin + 1;
            dst_ind_end_value   = dst_ind_begin_value;
            }

            part_step_size = 1.0f / float(dst_ind_end - dst_ind_begin);
            lerp_factor = 0.0f;

            for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            //lerp_factor = math::shoothstep(dst_ind_begin, dst_ind_end, dst_ind);
            dst[dst_ind] = lerp(dst_ind_begin_value, dst_ind_end_value, lerp_factor);
            lerp_factor += part_step_size;
            }
        }

        // clear end
        for (unsigned dst_ind = dst_ind_end; dst_ind < size; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // original code
        //float a;
        //float step = 255.0f / float(size - 1);
        //for (unsigned i = 0; i < size; i++) {
        //    a = float(i) * step;
        //    dst[i] = scal_trafu[a];
        //}

        return (true);
    }
}; // struct_look_uptable_impl

template<typename val_type>
struct build_lookup_table_impl<val_type, float>
{
    static bool build_table(boost::scoped_array<val_type>&                dst,
                            const piecewise_function_1d<float, val_type>& scal_trafu,
                            unsigned                                      size)
    {
        using namespace scm::math;

        if (size < 1) {
            return (false);
        }

        float    dst_ind_scal_factor = float(size - 1);

        unsigned dst_ind_begin;
        unsigned dst_ind_end;

        val_type dst_ind_begin_value;
        val_type dst_ind_end_value;

        float lerp_factor;
        float part_step_size;

        // clear beginning
        if (scal_trafu.empty()) {
            dst_ind_begin = 0;
            dst_ind_end   = size - 1;
        }
        else {
            dst_ind_begin   = 0;
            dst_ind_end     = unsigned(floor(scal_trafu.stops_begin()->first * dst_ind_scal_factor));
        }

        for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // fill lookup table
        for (typename scm::data::piecewise_function_1d<float, val_type>::const_stop_iterator it_left = scal_trafu.stops_begin();
            it_left  != scal_trafu.stops_end();
            ++it_left) {

            dst_ind_begin       = unsigned(floor(it_left->first * dst_ind_scal_factor));
            dst_ind_begin_value = it_left->second;

            typename scm::data::piecewise_function_1d<float, val_type>::const_stop_iterator it_right = boost::next(it_left);
            if (it_right != scal_trafu.stops_end()) {
            dst_ind_end         = unsigned(floor(it_right->first * dst_ind_scal_factor));
            dst_ind_end_value   = it_right->second;
            }
            else {
            dst_ind_end         = dst_ind_begin + 1;
            dst_ind_end_value   = dst_ind_begin_value;
            }

            part_step_size = 1.0f / float(dst_ind_end - dst_ind_begin);
            lerp_factor = 0.0f;

            for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            //lerp_factor = math::shoothstep(dst_ind_begin, dst_ind_end, dst_ind);
            dst[dst_ind] = lerp(dst_ind_begin_value, dst_ind_end_value, lerp_factor);
            lerp_factor += part_step_size;
            }
        }

        // clear end
        for (unsigned dst_ind = dst_ind_end; dst_ind < size; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // original code
        //float a;
        //float step = 255.0f / float(size - 1);
        //for (unsigned i = 0; i < size; i++) {
        //    a = float(i) * step;
        //    dst[i] = scal_trafu[a];
        //}

        return (true);
    }
}; // struct_look_uptable_impl

} // namespace detail
} // namespace data
} // namespace scm


#ifndef _MSC_VER
#include "update_ops_cpp.hpp"
extern "C"{
#include "utility.h"
}
#else
#include "update_ops_cpp.hpp"
#include "utility.h"
#endif
#include <Eigen/Core>
#include <functional>

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const CTYPE* matrix, CTYPE* state, ITYPE dim) {

    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
    Eigen::Map<const Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>, Eigen::Aligned> eigen_matrix((std::complex<double>*)matrix,matrix_dim,matrix_dim);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for(state_index = 0 ; state_index < loop_dim ; ++state_index ){
        // create base index
        ITYPE basis_0 = state_index;
        for(UINT cursor=0; cursor < target_qubit_index_count ; cursor++){
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index , insert_index );
        }

        // fetch vector
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>& eigen_matrix, CTYPE* state, ITYPE dim){
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for(state_index = 0 ; state_index < loop_dim ; ++state_index ){
        // create base index
        ITYPE basis_0 = state_index;
        for(UINT cursor=0; cursor < target_qubit_index_count ; cursor++){
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index , insert_index );
        }

        // fetch vector
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::MatrixXcd& eigen_matrix, CTYPE* state, ITYPE dim) {

    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
    std::complex<double>* cppstate = reinterpret_cast<std::complex<double>*>(state);
    Eigen::VectorXcd buffer(matrix_dim);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for(state_index = 0 ; state_index < loop_dim ; ++state_index ){
        // create base index
        ITYPE basis_0 = state_index;
        for(UINT cursor=0; cursor < target_qubit_index_count ; cursor++){
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index , insert_index );
        }

        // fetch vector
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            buffer[y] = cppstate[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            cppstate[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_sparse_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::SparseMatrix<std::complex<double>>& eigen_matrix, CTYPE* state, ITYPE dim) {
	// matrix dim, mask, buffer
	const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
	const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
	Eigen::VectorXcd buffer(matrix_dim);
	std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

	// insert index
	const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

	// loop variables
	const ITYPE loop_dim = dim >> target_qubit_index_count;

	ITYPE state_index;
	for (state_index = 0; state_index < loop_dim; ++state_index) {
		// create base index
		ITYPE basis_0 = state_index;
		for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
			UINT insert_index = sorted_insert_index_list[cursor];
			basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index, insert_index);
		}

		// fetch vector
		for (ITYPE y = 0; y < matrix_dim; ++y) {
			buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
		}

		buffer = eigen_matrix * buffer;

		// set result
		for (ITYPE y = 0; y < matrix_dim; ++y) {
			eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
		}
	}
	free((UINT*)sorted_insert_index_list);
	free((ITYPE*)matrix_mask_list);
}

void reversible_boolean_gate(const UINT* target_qubit_index_list, UINT target_qubit_index_count, std::function<ITYPE(ITYPE,ITYPE)> function_ptr, CTYPE* state, ITYPE dim) {

	// matrix dim, mask, buffer
	const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
	const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);

	// insert index
	const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

	// loop variables
	const ITYPE loop_dim = dim >> target_qubit_index_count;

	CTYPE* buffer = (CTYPE*)malloc((size_t)(sizeof(CTYPE)*matrix_dim));
	ITYPE state_index;
	for (state_index = 0; state_index < loop_dim; ++state_index) {
		// create base index
		ITYPE basis_0 = state_index;
		for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
			UINT insert_index = sorted_insert_index_list[cursor];
			basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index, insert_index);
		}

		// compute matrix-vector multiply

		for (ITYPE x = 0; x < matrix_dim; ++x) {
			ITYPE y = function_ptr(x, matrix_dim);
			buffer[y] = state[basis_0 ^ matrix_mask_list[x]];
		}

		// set result
		for (ITYPE y = 0; y < matrix_dim; ++y) {
			state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
		}
	}
	free(buffer);
	free((UINT*)sorted_insert_index_list);
	free((ITYPE*)matrix_mask_list);
}


#include <fstream>
#include <iostream>
#include <vector>

#include <Eigen/Geometry>
#include "GINS.hpp"

using namespace std;

// comment to turn off gnss interrupt
#define INTERRUPT_ON
const double CONVERGENCE_TIME = 500;
const double GNSS_TIME = 180;
const double INTERRUPT_TIME = 60;
const double MOTION_TIME = 456370;

string read_path = "/home/ubuntu/Dataset/";
string imu_data_name = "A15_imu.bin";
string gnss_data_name = "GNSS_RTK.txt";
string truth_name = "truth.nav";

#ifdef INTERRUPT_ON
string output_name = "result_interrupt.nav";
#else
string output_name = "result.nav";
#endif

class Truth {
public:
  double week;
  double second;
  Eigen::Vector3d pos;
  Eigen::Vector3d vel;
  Eigen::Vector3d att;
};

int main(int argc, char const *argv[]) {
  // read files
  ifstream imu_data((read_path + imu_data_name).c_str(), ios::in | ios::binary);
  ifstream gnss_data((read_path + gnss_data_name).c_str());
  ifstream truth_data((read_path + truth_name).c_str());
  ofstream result_data((read_path + output_name).c_str(), ios::trunc);

  if (!imu_data || !gnss_data || !truth_data || !result_data) exit(-1);

  // parse data and save
  iNav::IMUData imu_frame;
  iNav::GnssData gnss_frame;
  Truth truth_frame;
  vector<iNav::IMUData> imu_vec;
  vector<iNav::GnssData> gnss_vec;
  vector<Truth> truth_vec;

  iNav::NavData initial_state;
  initial_state.timestamp = 456300.0;
  initial_state.pos = Eigen::Vector3d(30.444787369, 114.471863247, 20.910);
  initial_state.pos_std = Eigen::Vector3d(0.005, 0.004, 0.008);
  initial_state.vel = Eigen::Vector3d(0, 0, 0);
  initial_state.vel_std = Eigen::Vector3d(0.003, 0.004, 0.004);
  initial_state.att = Eigen::Vector3d(0.854, -2.0345, 185.696);
  initial_state.att_std = Eigen::Vector3d(0.003, 0.003, 0.023);

  iNav::IMUParam imu_param;
  imu_param.ARW = 0.003;
  imu_param.VRW = 0.03;
  imu_param.gyro_bias_std = 0.027;
  imu_param.T_gyro_bias = 4;
  imu_param.acc_bias_std = 15;
  imu_param.T_acc_bias = 4;
  imu_param.gyro_scalar_std = 300;
  imu_param.T_gyro_scalar = 4;
  imu_param.acc_scalar_std = 300;
  imu_param.T_acc_scalar = 4;

  Eigen::Vector3d l_b(0.136, -0.301, -0.184);

  // save data in vector
  while (fabs(imu_frame.timestamp - initial_state.timestamp) > 0.001)
    imu_data.read((char *)&imu_frame, sizeof(imu_frame));
  imu_vec.push_back(imu_frame);

  while (truth_frame.second < initial_state.timestamp) {
    truth_data >> truth_frame.week >> truth_frame.second >>
        truth_frame.pos[0] >> truth_frame.pos[1] >> truth_frame.pos[2] >>
        truth_frame.vel[0] >> truth_frame.vel[1] >> truth_frame.vel[2] >>
        truth_frame.att[0] >> truth_frame.att[1] >> truth_frame.att[2];
  }
  truth_vec.push_back(truth_frame);

  while (gnss_frame.timestamp < initial_state.timestamp) {
    gnss_data >> gnss_frame.timestamp >> gnss_frame.pos[0] >>
        gnss_frame.pos[1] >> gnss_frame.pos[2] >> gnss_frame.pos_std[0] >>
        gnss_frame.pos_std[1] >> gnss_frame.pos_std[2];
  }
  gnss_vec.push_back(gnss_frame);

  while (imu_data.read((char *)&imu_frame, sizeof(imu_frame)))
    imu_vec.push_back(imu_frame);

  while (!gnss_data.eof()) {
    gnss_data >> gnss_frame.timestamp >> gnss_frame.pos[0] >>
        gnss_frame.pos[1] >> gnss_frame.pos[2] >> gnss_frame.pos_std[0] >>
        gnss_frame.pos_std[1] >> gnss_frame.pos_std[2];
    gnss_vec.push_back(gnss_frame);
  }

  while (!truth_data.eof()) {
    truth_data >> truth_frame.week >> truth_frame.second >>
        truth_frame.pos[0] >> truth_frame.pos[1] >> truth_frame.pos[2] >>
        truth_frame.vel[0] >> truth_frame.vel[1] >> truth_frame.vel[2] >>
        truth_frame.att[0] >> truth_frame.att[1] >> truth_frame.att[2];
    truth_vec.push_back(truth_frame);
  }

  iNav::GINS gnss_ins(initial_state, imu_param, l_b, imu_vec[0]);

  iNav::NavData nav_output;
  bool interrupt_flag = false;
  double record_time = initial_state.timestamp;
  int i = 1;
  for (int j = 1; j < imu_vec.size(); j++) {
    if (i < gnss_vec.size()) {
      if (imu_vec[j].timestamp < gnss_vec[i].timestamp) {
        nav_output = gnss_ins.Mechanization(imu_vec[j]);
        gnss_ins.Prediction();
      } else if (!interrupt_flag) {
        nav_output = gnss_ins.GNSSUpdate(gnss_vec[i++], imu_vec[j]);
      } else {
        i++;
        nav_output = gnss_ins.Mechanization(imu_vec[j]);
        gnss_ins.Prediction();
      }
    } else {
      nav_output = gnss_ins.Mechanization(imu_vec[j]);
      gnss_ins.Prediction();
    }

#ifdef INTERRUPT_ON
    if (imu_vec[j].timestamp - initial_state.timestamp < CONVERGENCE_TIME) {
      // wait for convergence
    } else {
      if (imu_vec[j].timestamp - record_time > GNSS_TIME && !interrupt_flag) {
        interrupt_flag = true;
        record_time = imu_vec[j].timestamp;
      }

      if (imu_vec[j].timestamp - record_time > INTERRUPT_TIME &&
          interrupt_flag) {
        interrupt_flag = false;
        record_time = imu_vec[j].timestamp;
      }
    }
#endif

    // nav files output
    if (nav_output.timestamp > MOTION_TIME) {
      result_data.precision(12);
      result_data << 2017 << " " << nav_output.timestamp << " ";
      result_data << nav_output.pos[0] << " " << nav_output.pos[1] << " "
                  << nav_output.pos[2] << " ";
      result_data << nav_output.vel[0] << " " << nav_output.vel[1] << " "
                  << nav_output.vel[2] << " ";
      auto yaw =
          nav_output.att[2] >= 0 ? nav_output.att[2] : nav_output.att[2] + 360;
      result_data << nav_output.att[0] << " " << nav_output.att[1] << " " << yaw
                  << endl;
    }

    // terminal output
    cout.precision(12);
    cout << "---" << endl;
    cout << "my position: " << endl;
    cout << nav_output.timestamp << endl;
    cout << nav_output.pos.transpose() << endl;

    cout << "truth: " << endl;
    cout << truth_vec[j].second << endl;
    cout << truth_vec[j].pos.transpose() << endl;
  }

  return 0;
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2014
//
//============================================================================

#ifndef __Thea_Algorithms_Icp3_hpp__
#define __Thea_Algorithms_Icp3_hpp__

#include "../Common.hpp"
#include "BvhN.hpp"
#include "MetricL2.hpp"
#include "PointTraitsN.hpp"
#include "../AffineTransformN.hpp"
#include "../Math.hpp"
#include "../MatVec.hpp"
#include "../HyperplaneN.hpp"
#include <Eigen/SVD>

namespace Thea {
namespace Algorithms {

/** Align two sets of points in 3D using the Iterative Closest Point (ICP) algorithm. */
template <typename ScalarT = Real>
class Icp3
{
  private:
    typedef Vector<3, ScalarT>       VectorT;  ///< 3D vector.
    typedef Matrix<3, 3, ScalarT>    MatrixT;  ///< 3x3 matrix.
    typedef HyperplaneN<3, ScalarT>  PlaneT;   ///< Plane in 3-space.

    /** The default weight per point. */
    template <typename T> struct DefaultWeightFunc
    {
      double getTranslationWeight(T const & t) const { return 1; }
      double getRotationWeight(T const & t) const { return 1; }
    };

  public:
    typedef AffineTransformN<3, ScalarT> AffineTransformT;  ///< Affine transform in 3 dimensions.

    /**
     * Constructor.
     *
     * @param fractional_error_threshold_ The maximum fractional change in error to determine convergence (negative for
     *   default).
     * @param min_iterations_ The minimum number of iterations (negative for default).
     * @param max_iterations_ The maximum number of iterations (negative for default).
     * @param verbose_ If true, print extra debugging information.
     */
    Icp3(ScalarT fractional_error_threshold_ = -1, intx min_iterations_ = -1, intx max_iterations_ = -1, bool verbose_ = false)
    : fractional_error_threshold(fractional_error_threshold_), min_iterations(min_iterations_), max_iterations(max_iterations_),
      has_up(false), verbose(verbose_)
    {
      if (fractional_error_threshold < 0) fractional_error_threshold = 0.0001;
      if (min_iterations < 0) min_iterations = 3;
      if (max_iterations < 0) max_iterations = 10;
    }

    /** Set the up vector. Subsequent alignments will only rotate around the up vector. */
    void setUpVector(VectorT const & up_) { up = up_.normalized(); has_up = true; }

    /** Check if the up vector has been set. */
    bool hasUpVector() const { return has_up; }

    /**
     * Get the up vector, if it has been set.
     *
     * @see hasUpVector();
     */
    VectorT const & getUpVector() const { return up; }

    /** Clear the up vector. Subsequent alignments will be unconstrained. */
    void clearUpVector() { has_up = false; }

    /** Find the transform that best aligns the point set \a from to the point set \a to. */
    template <typename FromT, typename ToT>
    AffineTransformT align(intx from_num_pts, FromT const * from, intx to_num_pts, ToT const * to, ScalarT * error = nullptr)
                     const
    {
      return align(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, to_num_pts, to, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the point set \a to, with a per-point weight assigned to the
     * cost of aligning each element of \a from.
     */
    template <typename FromT, typename ToT, typename FromWeightFuncT>
    AffineTransformT align(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                           intx to_num_pts, ToT const * to, ScalarT * error = nullptr) const
    {
      if (from_num_pts <= 0 || to_num_pts <= 0)
        return AffineTransformT::identity();

      BvhN<ToT, 3, ScalarT> to_bvh(to, to + to_num_pts);
      to_bvh.enableNearestNeighborAcceleration();

      return align(from_num_pts, from, from_weight_func, to_bvh, error);
    }

    /** Find the transform that best aligns the point set \a from to the proximity query structure \a to. */
    template <typename FromT, typename ToProximityQueryStructureT>
    AffineTransformT align(intx from_num_pts, FromT const * from, ToProximityQueryStructureT const & to,
                           ScalarT * error = nullptr) const
    {
      return align(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, to, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, with a per-point weight
     * assigned to the cost of aligning each element of \a from.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT align(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                           ToProximityQueryStructureT const & to, ScalarT * error = nullptr) const
    {
      return align(from_num_pts, from, from_weight_func, nullptr, to, nullptr, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the point set \a to, assuming both sets have known symmetry
     * planes. The computed alignment will ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, PlaneT const & from_symmetry_plane,
                                    intx to_num_pts, ToT const * to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      return alignSymmetric(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, from_symmetry_plane,
                            to_num_pts, to, to_symmetry_plane, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the point set \a to, with a per-point weight assigned to the
     * cost of aligning each element of \a from, and assuming both sets have known symmetry planes. The computed alignment will
     * ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToT, typename FromWeightFuncT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                    PlaneT const & from_symmetry_plane,
                                    intx to_num_pts, ToT const * to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      if (from_num_pts <= 0 || to_num_pts <= 0)
        return AffineTransformT::identity();

      BvhN<ToT, 3, ScalarT> to_bvh(to, to + to_num_pts);
      to_bvh.enableNearestNeighborAcceleration();

      return alignSymmetric(from_num_pts, from, from_weight_func, from_symmetry_plane, to_bvh, to_symmetry_plane, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, assuming both \a from
     * and \a to have known symmetry planes. The computed alignment will ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToProximityQueryStructureT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, PlaneT const & from_symmetry_plane,
                                    ToProximityQueryStructureT const & to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      return alignSymmetric(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, from_symmetry_plane, to, to_symmetry_plane,
                            error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, with a per-point weight
     * assigned to the cost of aligning each element of \a from, and assuming both \a from and \a to have known symmetry planes.
     * The computed alignment will ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                    PlaneT const & from_symmetry_plane,
                                    ToProximityQueryStructureT const & to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      return align(from_num_pts, from, from_weight_func, &from_symmetry_plane, to, &to_symmetry_plane, error);
    }

  private:
    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, with a per-point weight
     * assigned to the cost of aligning each element of \a from.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT align(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                           PlaneT const * from_symmetry_plane, ToProximityQueryStructureT const & to,
                           PlaneT const * to_symmetry_plane, ScalarT * error = nullptr) const
    {
      if (verbose)
        THEA_CONSOLE << "Icp3(fractional_error_threshold = " << fractional_error_threshold
                     <<    ", min_iterations = " << min_iterations
                     <<    ", max_iterations = " << max_iterations << ')';

      if (from_num_pts <= 0)
      {
        if (error) *error = 0;
        return AffineTransformT::identity();
      }

      Array<VectorT> from_points((size_t)from_num_pts);
      Array<VectorT> to_points((size_t)from_num_pts);
      for (size_t i = 0; i < from_points.size(); ++i)
        from_points[i] = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);

      AffineTransformT old_tr = AffineTransformT::identity();
      ScalarT old_error = measureError(old_tr, from_num_pts, &from_points[0], from_weight_func, to, &to_points[0]);
      if (verbose) THEA_CONSOLE << "[Icp3] Initial error: " << old_error;

      if (old_error <= std::numeric_limits<ScalarT>::min())
      {
        if (error) *error = old_error;
        return old_tr;
      }

      ScalarT new_error = old_error;
      AffineTransformT new_tr = old_tr;
      for (intx i = 0; i < max_iterations; ++i)
      {
        // Align using the point mapping created by the last call to measureError()
        AffineTransformT inc_tr = alignOneStep(from_num_pts, &from_points[0], from_weight_func, from_symmetry_plane,
                                               &to_points[0], to_symmetry_plane);

        // Update the overall transform
        new_tr = inc_tr * old_tr;

        // Compute the new error and the new mapping between points
        if (i < max_iterations - 1 || error)
          new_error = measureError(inc_tr, from_num_pts, &from_points[0], from_weight_func, to, &to_points[0]);

        if (i < max_iterations - 1)
        {
          ScalarT frac_change = (old_error - new_error) / old_error;

          if (verbose)
            THEA_CONSOLE << "[Icp3] Iteration " << i << " error: " << new_error << " (fractional change: " << frac_change << ')';

          if (i >= min_iterations && frac_change < fractional_error_threshold)
          {
            if (frac_change > 0)  // we improved slightly
            {
              if (error) *error = new_error;
              return new_tr;
            }
            else  // the previous alignment was better
            {
              if (error) *error = old_error;
              return old_tr;
            }
          }

          for (size_t j = 0; j < from_points.size(); ++j)  // transform original points to prevent drift
            from_points[j] = new_tr * PointTraitsN<FromT, 3, ScalarT>::getPosition(from[j]);
        }
        else
        {
          if (verbose)
            THEA_CONSOLE << "[Icp3] Iteration " << i << " error: " << new_error;
        }

        old_error = new_error;
        old_tr = new_tr;
      }

      if (error) *error = new_error;
      return new_tr;
    }

    /**
     * Align a point set to a proximity query structure, in one ICP step, after finding the nearest neighbor of each point in
     * the first set in the structure.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT alignOneStep(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                  PlaneT const * from_sym_plane, ToProximityQueryStructureT const & to,
                                  PlaneT const * to_sym_plane, VectorT * to_points) const
    {
      for (intx i = 0; i < from_num_pts; ++i)
      {
        intx index = to.template closestElement<MetricL2>(PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]), -1,
                                                          UniversalCompatibility(), nullptr, &to_points[i]);
        if (index < 0)
          throw Error(format("Icp3: Couldn't get nearest neighbor of source point %ld", i));
      }

      return alignOneStep(from_num_pts, from, from_weight_func, from_sym_plane, to_points, to_sym_plane);
    }

    /** Align one point set to another, in one ICP step, with a known bijective mapping between the points. */
    template <typename FromT, typename FromWeightFuncT>
    AffineTransformT alignOneStep(intx num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                  PlaneT const * from_sym_plane, VectorT const * to, PlaneT const * to_sym_plane) const
    {
      // When both point sets have symmetry planes, we'll align the projections. This does *NOT* minimize the correct error
      // function, but provides a simple approximation to the true solution which is ok for now.
      bool use_symmetry = from_sym_plane && to_sym_plane;

      // Find centroids of the two sets
      VectorT p_mean = VectorT::Zero();
      VectorT q_mean = VectorT::Zero();
      ScalarT sum_weights = 0;
      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT q = to[i];
        if (use_symmetry)
        {
          p = from_sym_plane->closestPoint(p);
          q = to_sym_plane->closestPoint(q);
        }

        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getTranslationWeight(from[i]);
          p_mean += weight * p;
          q_mean += weight * q;
          sum_weights += weight;
        }
        else
        {
          p_mean += p;
          q_mean += q;
          sum_weights += 1;
        }
      }

      p_mean /= sum_weights;
      q_mean /= sum_weights;

      // Find cross-covariance matrix
      MatrixT cov; cov.setZero();
      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT q = to[i];
        if (use_symmetry)
        {
          p = from_sym_plane->closestPoint(p);
          q = to_sym_plane->closestPoint(q);
        }

        VectorT dp = p - p_mean;
        VectorT dq = q - q_mean;

        if (has_up)  // remove the component in the up direction
        {
          dp = dp - (dp.dot(up) * up);
          dq = dq - (dq.dot(up) * up);
        }

        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getRotationWeight(from[i]);
          dp *= weight;
          dq *= weight;
        }

        cov += (dp * dq.transpose());  // outer product
      }

      Eigen::JacobiSVD<MatrixT> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
      MatrixT rot = svd.matrixU() * svd.matrixV().transpose();

      if (use_symmetry && rot.determinant() < 0)  // matching two planar projections can cause flips in the symmetry plane
      {
        // Generate the transformation that flips in the (origin-centered) target symmetry plane
        VectorT n = to_sym_plane->getNormal();
        MatrixT nn = n * n.transpose();  // outer product
        MatrixT flip = MatrixT::Identity() - static_cast<ScalarT>(2) * nn;
        rot = flip * rot;
      }

      VectorT trans = q_mean - rot * p_mean;

      return AffineTransformT(rot, trans);
    }

    /** Measure the alignment error of a mapping from each point of \a from to its nearest neighbor in \a to. */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    static ScalarT measureError(AffineTransformT const & tr,
                                intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                ToProximityQueryStructureT const & to, VectorT * to_points)
    {
      if (from_num_pts <= 0)
        return 0;

      for (intx i = 0; i < from_num_pts; ++i)
      {
        intx index = to.template closestElement<MetricL2>(PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]), -1,
                                                          UniversalCompatibility(), nullptr, &to_points[i]);
        if (index < 0)
          throw Error(format("Icp3: Couldn't get nearest neighbor of source point %ld", i));
      }

      return measureError(tr, from_num_pts, from, from_weight_func, to_points);
    }

    /**
     * Measure the alignment error of a mapping from each point of \a from to each point of \a to, with a known bijective
     * mapping between them.
     */
    template <typename FromT, typename FromWeightFuncT>
    static ScalarT measureError(AffineTransformT const & tr,
                                intx num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                VectorT const * to)
    {
      if (num_pts <= 0)
        return 0;

      VectorT p_mean = VectorT::Zero();
      VectorT q_mean = VectorT::Zero();

      ScalarT sum_weights = 0;
      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT const & q = to[i];

        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getTranslationWeight(from[i]);
          p_mean += weight * p;
          q_mean += weight * q;
          sum_weights += weight;
        }
        else
        {
          p_mean += p;
          q_mean += q;
          sum_weights += 1;
        }
      }

      p_mean /= sum_weights;
      q_mean /= sum_weights;

      ScalarT err = (tr * p_mean - q_mean).squaredNorm();

      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT q = to[i];

        VectorT dp = p - p_mean;
        VectorT dq = q - q_mean;
        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getRotationWeight(from[i]);
          err += weight * weight * (tr.getLinear() * dp - dq).squaredNorm();
        }
        else
          err += (tr.getLinear() * dp - dq).squaredNorm();
      }

      return err;
    }

    double fractional_error_threshold;  ///< Maximum fractional change in error to determine convergence.
    intx min_iterations;  ///< Minimum number of iterations.
    intx max_iterations;  ///< Maximum number of iterations.
    bool has_up;
    VectorT up;
    bool verbose;  ///< Print lots of debugging information?

}; // class Icp3

} // namespace Algorithms
} // namespace Thea

#endif


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#define _USE_MATH_DEFINES
#include <cmath>

#include <exception>
#include <map>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include "rttbTCPLQModel.h"
#include "rttbDvhBasedModels.h"
#include "rttbIntegration.h"
#include "rttbInvalidParameterException.h"
#include "rttbExceptionMacros.h"

namespace rttb
{

	namespace models
	{

		TCPLQModel::TCPLQModel(): TCPModel() {
			_name = "TCPLQModel";
			fillParameterMap();
		}

		TCPLQModel::TCPLQModel(DVHPointer aDVH, BioModelParamType aAlphaMean, BioModelParamType aBeta,
		                       BioModelParamType aRho,
		                       int aNumberOfFractions): TCPModel(aDVH, aNumberOfFractions), _alphaMean(aAlphaMean),
			_alphaVariance(0),
			_alpha_beta(aAlphaMean / aBeta), _rho(aRho)  {
			_name = "TCPLQModel";
			fillParameterMap();
		}

		TCPLQModel::TCPLQModel(DVHPointer aDVH, BioModelParamType aRho, int aNumberOfFractions,
		                       BioModelParamType aAlpha_Beta,
		                       BioModelParamType aAlphaMean, BioModelParamType aAlphaVariance): TCPModel(aDVH, aNumberOfFractions),
			_alphaMean(aAlphaMean),
			_alphaVariance(aAlphaVariance), _alpha_beta(aAlpha_Beta), _rho(aRho) {
			fillParameterMap();
			_name = "TCPLQModel";
		}

		void TCPLQModel::setParameters(const BioModelParamType aAlphaMean,
		                               const BioModelParamType aAlpha_Beta,
		                               const BioModelParamType aRho, const BioModelParamType aAlphaVariance)
		{
			_alphaMean = aAlphaMean;
			_alphaVariance = aAlphaVariance;
			_alpha_beta = aAlpha_Beta;
			_rho = aRho;

			//reset _value, because parameters have changed.
			_value = 0;
		}

		void TCPLQModel::setAlpha(const BioModelParamType aAlphaMean,
		                          const BioModelParamType aAlphaVariance)
		{
			_alphaVariance = aAlphaVariance;
			_alphaMean = aAlphaMean;
		}

		void TCPLQModel::setAlphaBeta(const BioModelParamType aAlpha_Beta)
		{
			_alpha_beta = aAlpha_Beta;
		}

		void TCPLQModel::setRho(const BioModelParamType aRho)
		{
			_rho = aRho;
		}

		const BioModelParamType TCPLQModel::getAlphaBeta()
		{
			return _alpha_beta;
		}

		const BioModelParamType TCPLQModel::getAlphaMean()
		{
			return _alphaMean;
		}

		const BioModelParamType TCPLQModel::getAlphaVariance()
		{
			return _alphaVariance;
		}

		const BioModelParamType TCPLQModel::getRho()
		{
			return _rho;
		}

		long double TCPLQModel::calcTCPi(BioModelParamType aRho, BioModelParamType aAlphaMean, double vj,
		                                 double bedj)
		{
			return exp(-aRho * vj * exp(-aAlphaMean * bedj));
		}

		long double TCPLQModel::calcTCP(std::map<rttb::DoseTypeGy, rttb::DoseCalcType> aBEDDVH,
		                                BioModelParamType aRho,
		                                BioModelParamType aAlphaMean, double aDeltaV)
		{
			std::map<rttb::DoseTypeGy, rttb::DoseCalcType>::iterator it;
			long double tcp = 1;

			for (it = aBEDDVH.begin(); it != aBEDDVH.end(); ++it)
			{
				long double tcpi = this->calcTCPi(aRho, aAlphaMean, (*it).second * aDeltaV, (*it).first);
				tcp = tcp * tcpi;
			}

			return tcp;
		}

		long double TCPLQModel::calcTCPAlphaNormalDistribution(
		    std::map<rttb::DoseTypeGy, rttb::DoseCalcType> aBEDDVH,
		    BioModelParamType aRho, BioModelParamType aAlphaMean,
		    BioModelParamType aAlphaVariance, double aDeltaV)
		{

			std::map<rttb::DoseTypeGy, rttb::DoseCalcType>::iterator it;
			std::vector<DoseCalcType> volumeV2;
			std::vector<DoseTypeGy> bedV2;
			int i = 0;

			for (it = aBEDDVH.begin(); it != aBEDDVH.end(); ++it)
			{
				volumeV2.push_back((*it).second * aDeltaV);
				bedV2.push_back((*it).first);
				i++;
			}

			struct TcpParams params = {aAlphaMean, aAlphaVariance, aRho, volumeV2, bedV2};

			double result = integrateTCP(0, params);

			if (result == -100)
			{
				std::cerr << "Integration error!\n";
				return -1;
			}
			else
			{
				long double tcp = 1 / (pow(2 * M_PI, 0.5) * _alphaVariance) * result;

				return tcp;
			}
		}

		BioModelValueType TCPLQModel::calcModel(const double doseFactor)
		{
			core::DVH variantDVH = core::DVH(_dvh->getDataDifferential(),
			                                 (DoseTypeGy)(_dvh->getDeltaD() * doseFactor),
			                                 _dvh->getDeltaV(), "temporary", "temporary");
			auto spDVH = boost::make_shared<core::DVH>(variantDVH);

			BioModelValueType value = 0;

			if (_alphaVariance == 0)
			{
				if (_alphaMean <= 0 || _alpha_beta <= 0 || _rho <= 0)
				{
					throw core::InvalidParameterException("Parameter invalid: alpha, alpha/beta, rho and number of fractions must >0!");
				}

				if (_numberOfFractions <= 1)
				{
					throw core::InvalidParameterException("Parameter invalid: numberOfFractions must be >1! The dvh should be an accumulated-dvh of all fractions, not a single fraction-dvh!");
				}

				std::map<rttb::DoseTypeGy, rttb::DoseCalcType> dataBED = calcBEDDVH(spDVH, _numberOfFractions,
				        _alpha_beta);

				value = (BioModelValueType)this->calcTCP(dataBED, _rho, _alphaMean, variantDVH.getDeltaV());
				return value;
			}

			//if alpha normal distribution
			else
			{
				if (this->_alpha_beta <= 0 || this->_alphaMean <= 0 || this->_alphaVariance < 0 || _rho <= 0)
				{
					throw core::InvalidParameterException("Parameter invalid: alpha/beta, alphaMean, rho and number of fractions must >0!");
				}

				if (_numberOfFractions <= 1)
				{
					throw core::InvalidParameterException("Parameter invalid: numberOfFractions must be >1! The dvh should be an accumulated-dvh of all fractions, not a single fraction-dvh!");
				}

				std::map<rttb::DoseTypeGy, rttb::DoseCalcType> dataBED = calcBEDDVH(spDVH, _numberOfFractions,
				        _alpha_beta);
				value = (BioModelValueType)(this->calcTCPAlphaNormalDistribution(dataBED, _rho, _alphaMean,
				                            _alphaVariance,
				                            variantDVH.getDeltaV()));
				return value;
			}
		}

		void TCPLQModel::setParameterVector(const ParamVectorType& aParameterVector)
		{
			if (aParameterVector.size() != 4)
			{
				throw core::InvalidParameterException("Parameter invalid: aParameterVector.size must be 4! ");
			}
			else
			{
				_alphaMean = aParameterVector.at(0);
				_alphaVariance = aParameterVector.at(1);
				_alpha_beta = aParameterVector.at(2);
				_rho = aParameterVector.at(3);
			}
		}

		void TCPLQModel::setParameterByID(const int aParamId, const BioModelParamType aValue)
		{
			if (aParamId == 0)
			{
				_alphaMean = aValue;
			}
			else if (aParamId == 1)
			{
				_alphaVariance = aValue;
			}
			else if (aParamId == 2)
			{
				_alpha_beta = aValue;
			}
			else if (aParamId == 3)
			{
				_rho = aValue;
			}
			else
			{
				throw core::InvalidParameterException("Parameter invalid: aParamID must be 0(alphaMean) or 1(alphaVariance) or 2(alpha_beta) or 3(rho)! ");
			}

		}

		const int TCPLQModel::getParameterID(const std::string& aParamName) const
		{
			if (aParamName == "alphaMean")
			{
				return 0;
			}
			else if (aParamName == "alphaVariance")
			{
				return 1;
			}
			else if (aParamName == "alpha_beta")
			{
				return 2;
			}
			else if (aParamName == "rho")
			{
				return 3;
			}
			else
			{
				rttbExceptionMacro(core::InvalidParameterException,
				                   << "Parameter name " << aParamName <<
				                   " invalid: it should be alphaMean or alphaVariance or alpha_beta or rho!");

			}
		}

		std::map<std::string, double> TCPLQModel::getParameterMap() const{
			return parameterMap;
		}

		void TCPLQModel::fillParameterMap(){
			parameterMap["numberOfFraction"] = getNumberOfFractions();
			parameterMap["alphaMean"] = getAlphaMean();
			parameterMap["alphaVariance"] = getAlphaVariance();
			parameterMap["alpha_beta"] = getAlphaBeta();
			parameterMap["rho"] = getRho();
		}

		std::string TCPLQModel::getModelType() const{
			return _name;
		}

	}//end namespace models
}//end namespace rttb


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// gDLS*: Generalized Pose-and-Scale Estimation Given Scale and Gravity Priors
//
// Victor Fragoso, Joseph DeGol, Gang Hua.
// Proc. of the IEEE/CVF Conf. on Computer Vision and Pattern Recognition 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <vector>

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "gdls_star/pinhole_camera.h"
#include "gdls_star/camera_feature_correspondence_2d_3d.h"
#include "gdls_star/estimate_similarity_transformation.h"
#include "gdls_star/gdls_star.h"
#include "gdls_star/gdls_star_robust_estimator.h"
#include "gdls_star/util.h"

namespace py = pybind11;

using msft::PinholeCamera;
using msft::CameraFeatureCorrespondence2D3D;
using msft::GdlsStar;
using msft::GdlsStarRobustEstimator;

struct PySolution {
  Eigen::Vector4d rotation = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0);
  Eigen::Vector3d translation = Eigen::Vector3d::Zero();
  double scale = 1.0;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

struct PyRansacSolution {
  PySolution best_solution;
  std::vector<int> inliers;
};

std::vector<PySolution> EstimateSimilarityTransformation(
    const GdlsStar::Priors& priors,
    const std::vector<CameraFeatureCorrespondence2D3D>& correspondences) {
  std::vector<PySolution> final_solutions;
  GdlsStar::Solution solution;
  // Compute input datum.
  const GdlsStar::Input input = msft::ComputeInputDatum(correspondences);
  GdlsStar estimator;
  estimator.EstimateSimilarityTransformation(input, &solution);
  final_solutions.resize(solution.rotations.size());
  for (int i = 0; i < solution.rotations.size(); ++i) {
    const Eigen::Quaterniond& rotation = solution.rotations[i];
    final_solutions[i].rotation = Eigen::Vector4d(rotation.w(),
                                                  rotation.x(),
                                                  rotation.y(),
                                                  rotation.z());
    final_solutions[i].translation = solution.translations[i];
    final_solutions[i].scale = solution.scales[i];
  }
  return final_solutions;
}

// Computes the similarity transformation given 2D-3D correspondences and priors
// using a RANSAC estimator.
PyRansacSolution EstimateSimilarityTransformationViaRansac(
    const GdlsStarRobustEstimator::RansacParameters& params,
    const GdlsStar::Priors& priors,
    const std::vector<CameraFeatureCorrespondence2D3D>& correspondences) {
  PyRansacSolution final_solution;
  GdlsStarRobustEstimator::RansacSummary ransac_summary;
  GdlsStarRobustEstimator estimator(params);
  const GdlsStar::Solution solution =
      estimator.Estimate(priors, correspondences, &ransac_summary);
  const Eigen::Quaterniond& rotation = solution.rotations[0];
  final_solution.best_solution.rotation = Eigen::Vector4d(rotation.w(),
                                                          rotation.x(),
                                                          rotation.y(),
                                                          rotation.z());
  final_solution.best_solution.translation = solution.translations[0];
  final_solution.best_solution.scale = solution.scales[0];
  final_solution.inliers = ransac_summary.inliers;
  return final_solution;
}

PYBIND11_MODULE(pygdls_star, module) {
  module.doc() = "gDLS* Python module"; // Optional module docstring.

  // Ransac parameter class.
  py::class_<GdlsStarRobustEstimator::RansacParameters>(module, "RansacParams")
      .def(py::init<>())
      .def_readwrite("failure_probability",
                     &GdlsStarRobustEstimator::RansacParameters::failure_probability)
      .def_readwrite("reprojection_error_thresh",
                     &GdlsStarRobustEstimator::RansacParameters::reprojection_error_thresh)
      .def_readwrite("min_iterations",
                     &GdlsStarRobustEstimator::RansacParameters::min_iterations)
      .def_readwrite("max_iterations",
                     &GdlsStarRobustEstimator::RansacParameters::max_iterations);

  // Ransac summary class.
  py::class_<GdlsStarRobustEstimator::RansacSummary>(module, "RansacSummary")
      .def(py::init<>())
      .def_readwrite("inliers",
                     &GdlsStarRobustEstimator::RansacSummary::inliers)
      .def_readwrite("num_iterations",
                     &GdlsStarRobustEstimator::RansacSummary::num_iterations)
      .def_readwrite("confidence",
                     &GdlsStarRobustEstimator::RansacSummary::confidence)
      .def_readwrite("num_hypotheses",
                     &GdlsStarRobustEstimator::RansacSummary::num_hypotheses);

  // Pinhole camera.
  py::class_<PinholeCamera>(module, "PinholeCamera")
      .def(py::init<const double,
                    const Eigen::Vector2d,
                    const Eigen::Vector4d,
                    const Eigen::Vector3d>(),
           py::arg("focal_length") = 1.0,
           py::arg("principal_point") = Eigen::Vector2d::Zero(),
           py::arg("world_to_cam_rot") = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0),
           py::arg("world_to_cam_trans") = Eigen::Vector3d::Zero())
      .def("project_point", &PinholeCamera::ProjectPoint)
      .def("pixel_to_unit_ray", &PinholeCamera::PixelToUnitRay)
      .def("get_position", &PinholeCamera::GetPosition);

  // CameraFeatureCorrespondence2D3D.
  py::class_<CameraFeatureCorrespondence2D3D>(module,
                                              "CameraFeatureCorrespondence2D3D")
      .def(py::init<>())
      .def_readwrite("camera", &CameraFeatureCorrespondence2D3D::camera)
      .def_readwrite("observation",
                     &CameraFeatureCorrespondence2D3D::observation)
      .def_readwrite("point", &CameraFeatureCorrespondence2D3D::point);

  // Priors.
  py::class_<GdlsStar::Priors>(module, "Priors")
      .def(py::init<>())
      .def_readwrite("world_down_direction",
                     &GdlsStar::Priors::world_down_direction)
      .def_readwrite("query_down_direction",
                     &GdlsStar::Priors::query_down_direction)
      .def_readwrite("scale_penalty", &GdlsStar::Priors::scale_penalty)
      .def_readwrite("scale_prior", &GdlsStar::Priors::scale_prior)
      .def_readwrite("gravity_penalty", &GdlsStar::Priors::gravity_penalty);

  // PySolution.
  py::class_<PySolution>(module, "Solution")
      .def(py::init<>())
      .def_readwrite("rotation", &PySolution::rotation)
      .def_readwrite("translation", &PySolution::translation)
      .def_readwrite("scale", &PySolution::scale);

  // PyRansacSolution.
  py::class_<PyRansacSolution>(module, "RansacSolution")
      .def(py::init<>())
      .def_readwrite("best_solution", &PyRansacSolution::best_solution)
      .def_readwrite("inliers", &PyRansacSolution::inliers);

  // Estimate similarity transformation using plain gDLS*.
  module.def("estimate", &EstimateSimilarityTransformation);
  module.def("estimate_ransac", &EstimateSimilarityTransformationViaRansac);
}


/*
Copyright 2020 Standard Cyborg

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include "standard_cyborg/math/Transform.hpp"

#include "standard_cyborg/math/Mat3x4.hpp"
#include "standard_cyborg/math/Mat3x3.hpp"
#include "standard_cyborg/util/DataUtils.hpp"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#ifndef DEBUG
#define EIGEN_NO_DEBUG  1
#endif
#include <Eigen/Core>
#include <Eigen/Cholesky>
#pragma clang diagnostic pop

namespace standard_cyborg {
namespace math {

// This function is derived from the transforms3d python package and specialized for 3x3 matrices.
// You may find the original source of this function at:
//     https://github.com/matthew-brett/transforms3d/blob/8f81b063686f9b892bdbd4775d02615dce028105/transforms3d/affines.py#L156-L246
//
// **********************
// Copyright and Licenses
// **********************
//
// Retrieved from https://github.com/matthew-brett/transforms3d/blob/master/LICENSE on Dec. 2, 2019
//
// Transforms3d
// ============
//
// The transforms3d package, including all examples, code snippets and attached
// documentation is covered by the 2-clause BSD license.
//
//    Copyright (c) 2009-2017, Matthew Brett and Christoph Gohlke
//    All rights reserved.
//
//    Redistribution and use in source and binary forms, with or without
//    modification, are permitted provided that the following conditions are
//    met:
//
//    1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
//    2. 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.
//
//    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
//    IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
//    THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
//    PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
//    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
//    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
//    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// 3rd party code and data
// =======================
//
// Some code distributed within the transforms3d sources was developed by other
// projects. This code is distributed under its respective licenses that are
// listed below.
//
// Sphinx autosummary extension
// ----------------------------
//
// This extension has been copied from NumPy (Jul 16, 2010) as the one shipped with
// Sphinx 0.6 doesn't work properly.
//
// ::
//
//  Copyright (c) 2007-2009 Stefan van der Walt and Sphinx team
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are met:
//
//    a. Redistributions of source code must retain the above copyright notice,
//       this list of conditions and the following disclaimer.
//    b. 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.
//    c. Neither the name of the Enthought 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 REGENTS 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.

typedef Eigen::Matrix<float, 3, 3> EMat3;
typedef Eigen::Vector3f EVec3;

Transform Transform::fromMat3x4(const math::Mat3x4& A) {
    Transform transform;

    // Start by extracting the translation. This part is very easy.
    transform.translation = {A.m03, A.m13, A.m23};

    // R = rotation
    // Z = zoom (scale)
    // S = shear
    EMat3 RZS;
    RZS << A.m00, A.m01, A.m02,
           A.m10, A.m11, A.m12,
           A.m20, A.m21, A.m22;

    // Use the Cholesky decomposition to perform the separation
    // This part very closely mirrors the original implementation, only specialized for 3x3 matrices
    EMat3 RZSt_RZS (RZS.transpose() * RZS);
    Eigen::LLT<EMat3> cholesky;
    cholesky.compute(RZSt_RZS);
    EMat3 ZS (cholesky.matrixL().transpose());
    EVec3 Z (ZS.diagonal());
    EMat3 R (RZS * ZS.inverse());

    // Detect and fix if the rotation flips parity
    if (R.determinant() < 0.0f) {
        Z(0) = -Z(0);
        ZS.row(0) = -ZS.row(0);
        R = RZS * ZS.inverse();
    }

    // Extract the result into StandardCyborg data types
    transform.scale = toVec3(Z);

    transform.shear = math::Vec3{
        ZS(0, 1) / Z(0),
        ZS(0, 2) / Z(0),
        ZS(1, 2) / Z(1)
    };

    transform.rotation = math::Quaternion::fromMat3x3(toMat3x3(R));

    return transform;
}

Transform Transform::fromMat3x4(const math::Mat3x4& A, std::string srcFrame, std::string destFrame) {
  Transform t (Transform::fromMat3x4(A));
  t.srcFrame = srcFrame;
  t.destFrame = destFrame;
  return t;
}

Transform Transform::inverse() const {
  Mat3x4 m (Mat3x4::fromTransform(*this));
  return Transform::fromMat3x4(m.invert(), destFrame, srcFrame);
}

} // namespace math
} // namespace standard_cyborg


#include <iostream>
#include <armadillo>
#include<complex>
#include<cstdlib>
#include <ctime>
#include <string>
#include "Particle.h"
#include "Spherical.h"
#include "Cubic.h"
#include "Rhombohedral.h"
#include "Shell.h"
using namespace std;
using namespace arma;

int main(){
    // Input
    int sizeCells;  //sizeCells of lattice in +/-ve x/y/z direction
    int pSizeCells;  // size of the particle (radius, edge) in unit cells - carved out of bulk lattice
    double disorderStrength; // Magnitude of disorder potential
    double disorderCoverage; // fraction of disordered states above orderRadius
    char pShape; // Spherical, Cubic or Rhombohedral

    // Parameters and data structures (Don't need to change those)
    const double tHopping = 1;
    const double deltatHopping = 0.4;
    const complex<double> spinOrbitCoupling(0,1);

    // Geometry of the model;
    const rowvec latVec1 ("0 0.5 0.5");
    const rowvec latVec2 ("0.5 0 0.5");
    const rowvec latVec3 ("0.5 0.5 0");
    const rowvec sublatVec ("0.25 0.25 0.25");
    const rowvec comShift ("-0.5 -0.5 -0.5"); // to have centre of mass of sublatOne at the origin
    const double latVecNorm = 1/sqrt(2); // cubic cell size a = 1
    const double sublatVecNorm = sqrt(3)/4;

    const double delta = 0.01; // small number for numerical comparisons

    // Read input from file
    ifstream inputfile;
    inputfile.open("input.txt");
    char buffer [512];
    string line;
    while (getline(inputfile, line)) {
        sscanf(line.c_str(), "sizeCells = %d %s", &sizeCells, buffer);
        sscanf(line.c_str(), "particleSizeCells = %d %s", &pSizeCells, buffer);
        sscanf(line.c_str(), "disorderStrength = %lf %s", &disorderStrength, buffer);
        sscanf(line.c_str(), "disorderCoverage = %lf %s", &disorderCoverage, buffer);
        sscanf(line.c_str(), "particleShape = %c %s", &pShape, buffer);
    }

    Particle * myParticle;

    if (pShape == 'S' || pShape == 's') {
        myParticle = new Spherical(pSizeCells*latVecNorm, pSizeCells*latVecNorm - sublatVecNorm);
    } else if (pShape == 'C' ||  pShape == 'c') {
        myParticle = new Cubic (pSizeCells*latVecNorm, pSizeCells*latVecNorm - sublatVecNorm);
    } else if (pShape == 'R' || pShape == 'r') {
        myParticle = new Rhombohedral(pSizeCells*latVecNorm, pSizeCells*latVecNorm - sublatVecNorm);
	} else if (pShape == 'H' || pShape == 'h') {
        myParticle = new Shell(pSizeCells*latVecNorm, pSizeCells*latVecNorm - 4*sublatVecNorm);
    } else { cout << "Wrong particle type - exiting" << endl; return 1;}
    cout << "Input: "<< sizeCells << " " << pSizeCells << " " <<  disorderStrength << " " << disorderCoverage << " " << myParticle->shape << endl;

    int pCells = 0; // # unit cells belonging to the particle
    int dummy = 0;
    rowvec dummyVec(3);
    cx_mat dummyMat(2,2);
    rowvec pcomShift(3);

    cx_mat unity(2,2), sigma_x(2,2), sigma_y(2,2), sigma_z(2,2); // Pauli matrices
    unity.fill(0), sigma_x.fill(0), sigma_y.fill(0), sigma_z.fill(0);
    mat dummy_unity={{1,0},{0,1}}, dummy_sigma_x={{0,1},{1,0}};
    mat dummy_sigma_y={{0,-1},{1,0}}, dummy_sigma_z={{1,0},{0,-1}};
    unity.set_real(dummy_unity);
    sigma_x.set_real(dummy_sigma_x);
    sigma_y.set_imag(dummy_sigma_y);
    sigma_z.set_real(dummy_sigma_z);

    // Rhombohedral has the shape of the unit cell, so don't need to carve out
    if (myParticle->shape == "Rhombohedral") {
        sizeCells = pSizeCells;
    }

    // Set up lattice and calculate number of cells within particle
    for (int i=-sizeCells; i<sizeCells; i++) {
        for (int j=-sizeCells;  j<sizeCells; j++) {
            for (int k=-sizeCells; k<sizeCells; k++) {
                dummyVec=i*latVec1+j*latVec2+k*latVec3-comShift;
                if (myParticle->WithinParticle(dummyVec)) {
                    pcomShift=pcomShift+2*dummyVec+sublatVec;
                    pCells++;
                }
            }
        }
    }

    // Finding particle's COM
    pcomShift=0.5*pcomShift/pCells;

    // Set up data structures for the particle
    myParticle->SetDataStructures(pCells);

    // Carve out the unit sphere out of lattice
    for (int i=-sizeCells; i<sizeCells; i++) {
        for (int j=-sizeCells;  j<sizeCells; j++) {
            for (int k=-sizeCells; k<sizeCells; k++) {
                dummyVec=i*latVec1+j*latVec2+k*latVec3-comShift;
                if (myParticle->WithinParticle(dummyVec)) {
                    myParticle->sublatOne(dummy,0)=dummy;
                    myParticle->sublatOne(dummy,span(1,3))=dummyVec-pcomShift;
                    myParticle->sublatTwo(dummy,0)=dummy;
                    myParticle->sublatTwo(dummy,span(1,3))=dummyVec+sublatVec-pcomShift;
                    dummy++;
                }
            }
        }
    }

    // Set up the Hamiltonian
    for (int i=0; i<pCells; i++) {
        for (int j=0; j<pCells; j++) {
            // Set up nearest neighbour hoppings (between sublattices)
            dummyVec=myParticle->sublatOne(i,span(1,3))-myParticle->sublatTwo(j,span(1,3));
            if (dot(dummyVec,dummyVec) < sublatVecNorm*sublatVecNorm+delta) {
                if (i==j) {
                    // hoppings within a cell i
                    myParticle->Hamiltonian(span(4*i+0,4*i+1),span(4*i+2,4*i+3))+=(tHopping+deltatHopping)*unity;
                    myParticle->Hamiltonian(span(4*i+2,4*i+3),span(4*i+0,4*i+1))+=(tHopping+deltatHopping)*unity;
                } else {
                    // hoppings from sublatTwo of cell j to sublatOne of cell i
                    myParticle->Hamiltonian(span(4*i+0,4*i+1),span(4*j+2,4*j+3))+=(tHopping)*unity;
                    // hoppings from sublatOne of cell i to sublatTwo of cell j
                    myParticle->Hamiltonian(span(4*j+2,4*j+3),span(4*i+0,4*i+1))+=(tHopping)*unity;
                }
            }

            // Set up next nearest neighbour hoppings (within each sublattice)
            dummyVec=myParticle->sublatOne(i,span(1,3))-myParticle->sublatOne(j,span(1,3));
            if ( (dot(dummyVec,dummyVec) < latVecNorm*latVecNorm+delta)  &&
                    (dot(dummyVec,dummyVec) > sublatVecNorm*sublatVecNorm+delta) ) {

                // Matrix elements are direction-dependent
                if ( (dummyVec(0) > 0 && dummyVec(1) > 0) || (dummyVec(0) > 0 &&
                            dummyVec(2) > 0) || (dummyVec(1) > 0 && dummyVec(2) > 0) ) {
                    dummyVec = cross(sublatVec, dummyVec-sublatVec);
                } else if ( (dummyVec(1) < 0 && dummyVec(2) < 0) || (dummyVec(0) > 0 &&
                            dummyVec(1) < 0) || (dummyVec(0) > 0 && dummyVec(2) < 0) ) {
                    dummyVec = cross(-latVec1+sublatVec, dummyVec+latVec1-sublatVec);
                } else if ( (dummyVec(0) < 0 && dummyVec(1) > 0) || (dummyVec(0) < 0 &&
                            dummyVec(2) < 0) || (dummyVec(1) > 0 && dummyVec(2) < 0) ) {
                    dummyVec = cross(-latVec2+sublatVec, dummyVec+latVec2-sublatVec);
                } else if ( (dummyVec(0) < 0 && dummyVec(2) > 0) || (dummyVec(1) < 0 &&
                            dummyVec(2) > 0) || (dummyVec(0) < 0 && dummyVec(1) < 0) ) {
                    dummyVec = cross(-latVec3+sublatVec, dummyVec+latVec3-sublatVec);
                } else {}
                dummyMat=dummyVec(0)*sigma_x+dummyVec(1)*sigma_y+dummyVec(2)*sigma_z;
                // See if both atoms are connected to the same linking atom
                // hoppings from cell j to cell i within sublattice One
                myParticle->Hamiltonian(span(4*i,4*i+1),span(4*j,4*j+1))=spinOrbitCoupling*dummyMat;
                // hoppings from cell i to cell j within sublattice One
                // Minus sign for hopping in opposite direction
                myParticle->Hamiltonian(span(4*j,4*j+1),span(4*i,4*i+1))=-spinOrbitCoupling*dummyMat;
                // hoppings from cell j to cell i within sublattice Two
                // Minus sign for sublattice
                myParticle->Hamiltonian(span(4*i+2,4*i+3),span(4*j+2,4*j+3))=-spinOrbitCoupling*dummyMat;
                // hoppings from cell i to cell j within sublattice Two
                myParticle->Hamiltonian(span(4*j+2,4*j+3),span(4*i+2,4*i+3))=spinOrbitCoupling*dummyMat;
            }
        }
    }

    myParticle->AddDisorder(disorderStrength, disorderCoverage, delta);

    // abs returns a matrix, max returns a vector, 2nd max - largest value
    if (max(max(abs(myParticle->Hamiltonian-myParticle->Hamiltonian.t()))) > delta ) {
        cout << "Error, Hamiltonian is not Hermitian by at least " << delta << endl;
    }

    // Get the eigenvectors and eigevalues
    eig_sym(myParticle->eigvals, myParticle->eigvecs, myParticle->Hamiltonian);

    myParticle->PrintInfo(disorderStrength);

    // Find the middle of the spectrum (Dirac point, E=0, for clean spectrum)
    for (int i=0; i<4*pCells-1;i++) {
        if (myParticle->eigvals(i) > 0 && myParticle->eigvals(i-1) < 0) {
            dummy = i ;
        }
    }
    cout << "First state above E=0 is #: " << dummy << " with E= " << myParticle->eigvals(dummy) << endl;
    dummy=2*pCells;
    cout << "Picking state #: " << dummy << " with E= " << myParticle->eigvals(dummy) << endl;

    mat probDensity(2*pCells,4+100);
    probDensity(span(0,pCells-1),span(0,2))=myParticle->sublatOne.cols(1,3);
    probDensity(span(pCells,2*pCells-1),span(0,2))=myParticle->sublatTwo.cols(1,3);

    // Calculate probability density of probDensity labelled dummy at each lattice site
    for (int i=0; i<pCells; i++) {
		for (int j=0; j<100; j++) {
        probDensity(i,3+j)=norm(myParticle->eigvecs(span(4*i,4*i+1),dummy+j))*norm(myParticle->eigvecs(span(4*i,4*i+1),dummy+j));
        probDensity(pCells+i,3+j)=norm(myParticle->eigvecs(span(4*i+2,4*i+3),dummy+j))*norm(myParticle->eigvecs(span(4*i+2,4*i+3),dummy+j));
		}
    }
    // Save all eigenvalues and a particular eigenvector
    myParticle->eigvals.save("output_eigvals.txt",raw_ascii);
    probDensity.save("output_state.txt", raw_ascii);
    return 0;
}