#ifndef INCLUDE_SWIFT_VIO_IMU_ERROR_MODELS_HPP_
#define INCLUDE_SWIFT_VIO_IMU_ERROR_MODELS_HPP_

// Generic methods specific to each IMU model is encapsulated in the following classes.
// These models are to be used in constructing ceres::SizedCostFunction or
// ceres::AutoDiffCostFunction. Both require constant parameter block sizes at compile time.
// This is why we cannot use polymorphism for representing IMU models.

// The model parameter data are kept in the ImuModel class which
// is initialized and updated in the estimator.

// The IMU input reference frame or sensor frame denoted by S is affixed to
// the accelerometer triad, and its x-axis aligned to the accelerometer in the x direction.
// Its origin is at the intersection of the three accelerometers.
// Tts y-axis in the plane spanned by the two accelerometers at x and y
// direction while being close to the accelerometer at y-direction.
// The sensor rig body frame denoted by B is used to express the motion of the rig.
// It varies depending on the IMU model.

#include <vector>

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

#include <okvis/ModelSwitch.hpp>

namespace swift_vio {
static const int kBgBaDim = 6; // bg ba

template <typename T>
void vectorToLowerTriangularMatrix(const T* data, int startIndex, Eigen::Matrix<T, 3, 3>* mat33) {
  (*mat33)(0, 0) = data[startIndex];
  (*mat33)(0, 1) = 0;
  (*mat33)(0, 2) = 0;
  (*mat33)(1, 0) = data[startIndex + 1];
  (*mat33)(1, 1) = data[startIndex + 2];
  (*mat33)(1, 2) = 0;
  (*mat33)(2, 0) = data[startIndex + 3];
  (*mat33)(2, 1) = data[startIndex + 4];
  (*mat33)(2, 2) = data[startIndex + 5];
}

template <typename T>
void vectorToMatrix(const T* data, int startIndex, Eigen::Matrix<T, 3, 3>* mat33) {
  (*mat33)(0, 0) = data[startIndex];
  (*mat33)(0, 1) = data[startIndex + 1];
  (*mat33)(0, 2) = data[startIndex + 2];
  (*mat33)(1, 0) = data[startIndex + 3];
  (*mat33)(1, 1) = data[startIndex + 4];
  (*mat33)(1, 2) = data[startIndex + 5];
  (*mat33)(2, 0) = data[startIndex + 6];
  (*mat33)(2, 1) = data[startIndex + 7];
  (*mat33)(2, 2) = data[startIndex + 8];
}

template <typename T>
void invertLowerTriangularMatrix(const T* data, int startIndex, Eigen::Matrix<T, 3, 3>* mat33) {
  //  syms a b c d e f positive
  //  g = [a, 0, 0, b, c, 0, d, e, f]
  //  [ a, 0, 0]
  //  [ b, c, 0]
  //  [ d, e, f]
  //  inv(g)
  //  [                 1/a,        0,   0]
  //  [            -b/(a*c),      1/c,   0]
  //  [ (b*e - c*d)/(a*c*f), -e/(c*f), 1/f]
  (*mat33)(0, 0) = 1 / data[startIndex];
  (*mat33)(0, 1) = 0;
  (*mat33)(0, 2) = 0;
  (*mat33)(1, 0) = - data[startIndex + 1] / (data[startIndex] * data[startIndex + 2]);
  (*mat33)(1, 1) = 1 / data[startIndex + 2];
  (*mat33)(1, 2) = 0;
  (*mat33)(2, 0) = (data[startIndex + 1] * data[startIndex + 4] -
      data[startIndex + 2] * data[startIndex + 3]) /
      (data[startIndex] * data[startIndex + 2] * data[startIndex + 5]);
  (*mat33)(2, 1) = - data[startIndex + 4] / (data[startIndex + 2] * data[startIndex + 5]);
  (*mat33)(2, 2) = 1 / data[startIndex + 5];
}

/**
 * @brief The Imu_BG_BA class
 * The body frame is identical to the classic IMU sensor frame which has origin
 * at the accelerometer intersection, and x along x-accelrometer and y in the
 * plane spanned by the x- and y-accelerometer.
 * The accelerometer triad and the gyroscope triad are free of scaling error and misalignment.
 */
class Imu_BG_BA {
 public:
  static const int kModelId = 0;
  static const size_t kGlobalDim = kBgBaDim;
  static const size_t kAugmentedDim = 0;

  /**
   * @brief getAugmentedDim
   * @return dim of all the augmented params.
   */
  static inline int getAugmentedDim() { return kAugmentedDim; }
  /**
   * @brief getMinimalDim
   * @return minimal dim of all the params.
   */
  static inline int getMinimalDim() { return kGlobalDim; }
  /**
   * @brief getAugmentedMinimalDim
   * @return minimal dim of all augmented params.
   */
  static inline int getAugmentedMinimalDim() { return kAugmentedDim; }
  /**
   * get nominal values for augmented params.
   */
  template <typename T>
  static Eigen::Matrix<T, kAugmentedDim, 1> getNominalAugmentedParams() {
    return Eigen::Matrix<T, kAugmentedDim, 1>::Zero();
  }
  /**
   * predict IMU measurement from values in the body frame.
   * This function is used for testing purposes.
   */
  template <typename T>
  static void
  predict(const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> &ba,
          const Eigen::Matrix<T, Eigen::Dynamic, 1> & /*extraParams*/,
          const Eigen::Matrix<T, 3, 1> &w_b, const Eigen::Matrix<T, 3, 1> &a_b,
          Eigen::Matrix<T, 3, 1> *w, Eigen::Matrix<T, 3, 1> *a) {
    *a = a_b + ba;
    *w = w_b + bg;
  }
  /**
   * correct IMU measurement to the body frame.
   * This function is used by the ceres::CostFunction.
   * @param[in] params bg ba and augmented Euclidean params.
   * @param[in] q_gyro_i orientation from the accelerometer triad input reference frame,
   *     i.e., the IMU sensor frame to the gyro triad input reference frame.
   * @param[in] w, a angular velocity and linear acceleration measured by the IMU.
   * @param[out] w_b, a_b angular velocity and linear acceleration in the body frame.
   */
  template <typename T>
  static void
  correct(const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> &ba,
          const Eigen::Matrix<T, Eigen::Dynamic, 1> & /*params*/,
          const Eigen::Matrix<T, 3, 1> &w, const Eigen::Matrix<T, 3, 1> &a,
          Eigen::Matrix<T, 3, 1> *w_b, Eigen::Matrix<T, 3, 1> *a_b) {
    *a_b = a - ba;
    *w_b = w - bg;
  }

  static Eigen::VectorXd computeAugmentedParamsError(
      const Eigen::VectorXd& /*params*/) {
      return Eigen::VectorXd(0);
  }
};

/**
 * @brief The Imu_BG_BA_TG_TS_TA class
 * The body frame is the same as the IMU sensor frame which is
 * defined relative to an external sensor, e.g., the camera. Its
 * orientation is fixed to the nominal value of R_SC0 and its origin is at
 * the accelerometer intersection. Thus both accelerometer triad and
 * gyroscope triad need to account for scaling effect (3), misalignment (3),
 * relative orientation (4, minimal 3) to the body frame.
 * This model also considers the g-sensitivity (9) of the gyroscope triad.
 * In other words, the remaining misalignment between the orthogonal
 *  accelerometer input reference frame (A) and the C frame is
 * absorbed into T_a, the IMU accelerometer misalignment matrix.
 *
 * IMU model
 * w_m = T_g * w_B + T_s * a_B + b_w + n_w
 * a_m = T_a * a_B + b_a + n_a = S * M * R_AB * a_B + b_a + n_a
 *
 * The A frame has origin at the accelerometers intersection and x-axis aligned
 * with accelerometer x.
 */
class Imu_BG_BA_TG_TS_TA {
 public:
  static const int kModelId = 1;
  static const size_t kAugmentedDim = 27;
  static const size_t kGlobalDim = kAugmentedDim + kBgBaDim;

  static inline int getAugmentedDim() { return kAugmentedDim; }
  static inline int getMinimalDim() { return kGlobalDim; }
  static inline int getAugmentedMinimalDim() { return kAugmentedDim; }
  template <typename T>
  static Eigen::Matrix<T, kAugmentedDim, 1> getNominalAugmentedParams() {
    Eigen::Matrix<T, 9, 1> eye;
    eye << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    Eigen::Matrix<T, kAugmentedDim, 1> augmentedParams;
    augmentedParams.template head<9>() = eye;
    augmentedParams.template tail<9>() = eye;
    return augmentedParams;
  }

  template <typename T>
  static void
  predict(const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> &ba,
          const Eigen::Matrix<T, Eigen::Dynamic, 1> &params,
          const Eigen::Matrix<T, 3, 1> &w_b, const Eigen::Matrix<T, 3, 1> &a_b,
          Eigen::Matrix<T, 3, 1> *w, Eigen::Matrix<T, 3, 1> *a) {
    Eigen::Matrix<T, 3, 3> T_g;
    vectorToMatrix<T>(params.data(), 0, &T_g);
    Eigen::Matrix<T, 3, 3> T_s;
    vectorToMatrix<T>(params.data(), 9, &T_s);
    Eigen::Matrix<T, 3, 3> T_a;
    vectorToMatrix<T>(params.data(), 18, &T_a);
    *a = T_a * a_b + ba;
    *w = T_g * w_b + T_s * a_b + bg;
  }

  template <typename T>
  static void
  correct(const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> &ba,
          const Eigen::Matrix<T, Eigen::Dynamic, 1> &params,
          const Eigen::Matrix<T, 3, 1> &w, const Eigen::Matrix<T, 3, 1> &a,
          Eigen::Matrix<T, 3, 1> *w_b, Eigen::Matrix<T, 3, 1> *a_b) {
    Eigen::Matrix<T, 3, 3> T_g;
    vectorToMatrix<T>(params.data(), 0, &T_g);
    Eigen::Matrix<T, 3, 3> T_s;
    vectorToMatrix<T>(params.data(), 9, &T_s);
    Eigen::Matrix<T, 3, 3> T_a;
    vectorToMatrix<T>(params.data(), 18, &T_a);
    Eigen::Matrix<T, 3, 3> inv_T_g = T_g.inverse();
    Eigen::Matrix<T, 3, 3> inv_T_a = T_a.inverse();
    *a_b = inv_T_a * (a - ba);
    *w_b = inv_T_g * (w - bg - T_s * (*a_b));
  }

  static Eigen::VectorXd computeAugmentedParamsError(
      const Eigen::VectorXd& params) {
      Eigen::VectorXd residual = params;
      Eigen::Matrix<double, 9, 1> eye;
      eye << 1, 0, 0, 0, 1, 0, 0, 0, 1;
      residual.head<9>() -= eye;
      residual.tail<9>() -= eye;
      return residual;
  }
};

/**
 * @brief The ScaledMisalignedImu class
 * The body frame is the same as the classic IMU sensor frame. So the gyroscope triad
 * needs to consider scaling effect(3), misalignment(3), and relative
 * orientation(4, minimal 3) to the IMU sensor frame, and g-sensitivity (9)
 * whereas the accelerometer triad needs to consider scaling effect (3) and
 * misalignment (3). The lever arm(size) effects are ignored.
 * Implemented according to "Extending Kalibr".
 */
class ScaledMisalignedImu {
 public:
  static const int kModelId = 2;
  static const size_t kSMDim = 6;
  static const size_t kSensitivityDim = 9;
  static const size_t kAugmentedDim = kSMDim + kSensitivityDim + kSMDim + 4;
  static const size_t kGlobalDim = kAugmentedDim + kBgBaDim;
  static inline int getAugmentedDim() { return kAugmentedDim; }
  static inline int getMinimalDim() { return kGlobalDim - 1; }
  static inline int getAugmentedMinimalDim() { return kAugmentedDim - 1; }
  template <typename T>
  static Eigen::Matrix<T, kAugmentedDim, 1> getNominalAugmentedParams() {
    Eigen::Matrix<T, kAugmentedDim, 1> nominalValues = Eigen::Matrix<T, kAugmentedDim, 1>::Zero();
    nominalValues[0] = T(1.0);
    nominalValues[2] = T(1.0);
    nominalValues[5] = T(1.0);
    nominalValues[6 + 9] = T(1.0);
    nominalValues[6 + 9 + 2] = T(1.0);
    nominalValues[6 + 9 + 5] = T(1.0);
    nominalValues[kAugmentedDim - 1] = T(1.0);  // quaternion in xyzw format for R_gyro_i.
    return nominalValues;
  }

  /**
   * nearly 1:1 implementation of
   * https://github.com/ethz-asl/kalibr/blob/master/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py#L1033-L1049
   * w_b angular velocity in body frame at time tk.
   * w_dot_b angular acceleration in body frame at time tk.
   * a_w linear acceleration at tk.
   * r_b acceleration triad origin, i.e., the sensor frame origin, coordinates expressed in the body frame.
   * params gyro bias, accelerometer bias, gyro Scaling*Misalignment, gyro g-sensitivity, accelerometer Scaling*Misalignment.
   * C_gyro_i the relative orientation from the accelerometer triad frame, i.e., the IMU sensor frame to the gyro triad frame.
   */
  template <typename T>
  static void predictAngularVelocity(
      const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> & /*ba*/,
      const Eigen::Matrix<T, Eigen::Dynamic, 1> &params,
      const Eigen::Quaternion<T> &q_w_b, const Eigen::Matrix<T, 3, 1> &w_b,
      const Eigen::Matrix<T, 3, 1> &a_w, const Eigen::Matrix<T, 3, 1> &g_w,
      Eigen::Matrix<T, 3, 1> *w) {
    Eigen::Matrix<T, 3, 1> w_dot_b = Eigen::Matrix<T, 3, 1>::Zero();
    Eigen::Matrix<T, 3, 3> C_b_w =
        q_w_b.template toRotationMatrix().transpose();
    Eigen::Matrix<T, 3, 1> r_b =
        Eigen::Matrix<T, 3, 1>::Zero(); // Assume the 3 accelerometers are at
                                        // the origin of the body frame.
    Eigen::Matrix<T, 3, 1> a_b =
        C_b_w * (a_w - g_w) + w_dot_b.cross(r_b) + w_b.cross(w_b.cross(r_b));

    Eigen::Matrix<T, 3, 3> C_i_b =
        Eigen::Matrix<T, 3, 3>::Identity(); // The IMU sensor frame coincides
                                            // the accelerometer triad frame.
    Eigen::Map<const Eigen::Quaternion<T>> q_gyro_i(params.data() +
                                                    kAugmentedDim - 4);
    Eigen::Matrix<T, 3, 3> C_gyro_i = q_gyro_i.template toRotationMatrix();
    Eigen::Matrix<T, 3, 3> C_gyro_b = C_gyro_i * C_i_b;

    Eigen::Matrix<T, 3, 3> M_gyro;
    vectorToLowerTriangularMatrix<T>(params.data(), 0, &M_gyro);
    Eigen::Matrix<T, 3, 3> M_accel_gyro;
    vectorToMatrix<T>(params.data(), kSMDim, &M_accel_gyro);
    *w = M_gyro * (C_gyro_b * w_b) + M_accel_gyro * (C_gyro_b * a_b) + bg;
  }

  /**
   * nearly 1:1 implementation of
   * https://github.com/ethz-asl/kalibr/blob/master/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py#L989-L1000
   */
  template <typename T>
  static void predictLinearAcceleration(
      const Eigen::Matrix<T, 3, 1> &/*bg*/, const Eigen::Matrix<T, 3, 1> &ba,
      const Eigen::Matrix<T, Eigen::Dynamic, 1> &params,
      const Eigen::Quaternion<T> &q_w_b, const Eigen::Matrix<T, 3, 1> &w_b,
      const Eigen::Matrix<T, 3, 1> &a_w, const Eigen::Matrix<T, 3, 1> &g_w,
      Eigen::Matrix<T, 3, 1> *a) {
    Eigen::Matrix<T, 3, 1> w_dot_b = Eigen::Matrix<T, 3, 1>::Zero();
    Eigen::Matrix<T, 3, 3> C_b_w =
        q_w_b.template toRotationMatrix().transpose();

    Eigen::Matrix<T, 3, 3> M_accel;
    vectorToLowerTriangularMatrix<T>(params.data(), kSMDim + kSensitivityDim,
                                     &M_accel);

    Eigen::Matrix<T, 3, 1> r_b =
        Eigen::Matrix<T, 3, 1>::Zero(); // Assume the 3 accelerometers are at
                                        // the origin of the body frame.
    Eigen::Matrix<T, 3, 1> a_b =
        C_b_w * (a_w - g_w) + w_dot_b.cross(r_b) + w_b.cross(w_b.cross(r_b));

    Eigen::Matrix<T, 3, 3> C_i_b =
        Eigen::Matrix<T, 3, 3>::Identity(); // The IMU sensor frame coincides
                                            // the accelerometer triad frame.
    *a = M_accel * (C_i_b * a_b) + ba;
  }

  template <typename T>
  static void
  predict(const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> &ba,
          const Eigen::Matrix<T, Eigen::Dynamic, 1> &params,
          const Eigen::Matrix<T, 3, 1> &w_b, const Eigen::Matrix<T, 3, 1> &a_b,
          Eigen::Matrix<T, 3, 1> *w, Eigen::Matrix<T, 3, 1> *a) {
    Eigen::Matrix<T, 3, 3> M_accel;
    vectorToLowerTriangularMatrix<T>(params.data(), kSMDim + kSensitivityDim,
                                     &M_accel);
    Eigen::Matrix<T, 3, 3> C_i_b =
        Eigen::Matrix<T, 3, 3>::Identity(); // The IMU sensor frame coincides
                                            // the accelerometer triad frame.
    *a = M_accel * (C_i_b * a_b) + ba;

    Eigen::Map<const Eigen::Quaternion<T>> q_gyro_i(params.data() +
                                                    kAugmentedDim - 4);
    Eigen::Matrix<T, 3, 3> C_gyro_i = q_gyro_i.toRotationMatrix();
    Eigen::Matrix<T, 3, 3> C_gyro_b = C_gyro_i * C_i_b;

    Eigen::Matrix<T, 3, 3> M_gyro;
    vectorToLowerTriangularMatrix<T>(params.data(), 0, &M_gyro);
    Eigen::Matrix<T, 3, 3> M_accel_gyro;
    vectorToMatrix<T>(params.data(), kSMDim, &M_accel_gyro);
    *w = M_gyro * (C_gyro_b * w_b) + M_accel_gyro * (C_gyro_b * a_b) + bg;
  }

  template <typename T>
  static void
  correct(const Eigen::Matrix<T, 3, 1> &bg, const Eigen::Matrix<T, 3, 1> &ba,
          const Eigen::Matrix<T, Eigen::Dynamic, 1> &params,
          const Eigen::Matrix<T, 3, 1> &w, const Eigen::Matrix<T, 3, 1> &a,
          Eigen::Matrix<T, 3, 1> *w_b, Eigen::Matrix<T, 3, 1> *a_b) {
    Eigen::Matrix<T, 3, 3> M_accel_inv;
    invertLowerTriangularMatrix<T>(params.data(), kSMDim + kSensitivityDim,
                                   &M_accel_inv);
    Eigen::Matrix<T, 3, 3> C_b_i =
        Eigen::Matrix<T, 3, 3>::Identity(); // The IMU sensor frame coincides
                                            // the accelerometer triad frame.
    *a_b = C_b_i * M_accel_inv * (a - ba);

    Eigen::Matrix<T, 3, 3> C_i_b = C_b_i.transpose();
    Eigen::Map<const Eigen::Quaternion<T>> q_gyro_i(params.data() +
                                                    kAugmentedDim - 4);
    Eigen::Matrix<T, 3, 3> C_gyro_i = q_gyro_i.toRotationMatrix();
    Eigen::Matrix<T, 3, 3> C_gyro_b = C_gyro_i * C_i_b;

    Eigen::Matrix<T, 3, 3> M_gyro_inv;
    invertLowerTriangularMatrix<T>(params.data(), 0, &M_gyro_inv);
    Eigen::Matrix<T, 3, 3> M_accel_gyro;
    vectorToMatrix<T>(params.data(), kSMDim, &M_accel_gyro);
    *w_b = C_gyro_b.transpose() *
           (M_gyro_inv * (w - bg - M_accel_gyro * (C_gyro_b * (*a_b))));
  }

  static Eigen::VectorXd
  computeAugmentedParamsError(const Eigen::VectorXd &params) {
    Eigen::VectorXd residual(getAugmentedMinimalDim());
    Eigen::VectorXd nominalValues = getNominalAugmentedParams<double>();
    constexpr int kAugmentedEuclideanDim = kAugmentedDim - 4;
    Eigen::Map<const Eigen::Quaterniond> q_g_i(nominalValues.data() +
                                               kAugmentedEuclideanDim);
    residual.head<kAugmentedEuclideanDim>() =
        params.head<kAugmentedEuclideanDim>() -
        nominalValues.head<kAugmentedEuclideanDim>();
    Eigen::Map<const Eigen::Quaterniond> q_g_i_hat(params.data() + kAugmentedEuclideanDim);
    residual.tail<3>() = (q_g_i * q_g_i_hat.conjugate()).coeffs().head<3>() * 2;
    return residual;
  }
};

#ifndef IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASES                                                  \
  IMU_ERROR_MODEL_CASE(Imu_BG_BA)                                              \
  IMU_ERROR_MODEL_CASE(Imu_BG_BA_TG_TS_TA)                                     \
  IMU_ERROR_MODEL_CASE(ScaledMisalignedImu)
#endif

inline int ImuModelGetMinimalDim(int model_id) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
  case ImuModel::kModelId:             \
    return ImuModel::getMinimalDim();

    MODEL_SWITCH_CASES

#undef IMU_ERROR_MODEL_CASE
#undef MODEL_CASES
  }
  return 0;
}

inline int ImuModelGetAugmentedDim(int model_id) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
  case ImuModel::kModelId:             \
    return ImuModel::getAugmentedDim();

    MODEL_SWITCH_CASES

#undef IMU_ERROR_MODEL_CASE
#undef MODEL_CASES
  }
  return 0;
}

inline int ImuModelGetAugmentedMinimalDim(int model_id) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
  case ImuModel::kModelId:             \
    return ImuModel::getAugmentedMinimalDim();

    MODEL_SWITCH_CASES

#undef IMU_ERROR_MODEL_CASE
#undef MODEL_CASES
  }
  return 0;
}

inline void ImuModelToAugmentedDesiredStdevs(const int imu_model,
                                             Eigen::VectorXd *stdevs) {
  int index = 0;
  switch (imu_model) {
  case Imu_BG_BA_TG_TS_TA::kModelId:
    stdevs->resize(27);
    for (int i = 0; i < 9; ++i) {
      (*stdevs)[i] = 4e-3;
    }
    index = 9;
    for (int i = 0; i < 9; ++i) {
      (*stdevs)[i + index] = 1e-3;
    }
    index += 9;
    for (int i = 0; i < 9; ++i) {
      (*stdevs)[i + index] = 5e-3;
    }
    break;
  case ScaledMisalignedImu::kModelId:
    stdevs->resize(24);
    for (int i = 0; i < 6; ++i) {
      (*stdevs)[i] = 4e-3;
    }
    index += 6;
    for (int i = 0; i < 9; ++i) {
      (*stdevs)[i + index] = 1e-3;
    }
    index += 9;
    for (int i = 0; i < 6; ++i) {
      (*stdevs)[i + index] = 5e-3;
    }
    index += 6;
    for (int i = 0; i < 3; ++i) {
      (*stdevs)[i + index] = 5e-3;
    }
    index += 3;
    break;
  case Imu_BG_BA::kModelId:
  default:
    stdevs->resize(0);
    break;
  }
}

inline void
ImuModelToMinimalAugmentedDimensionLabels(const int imu_model,
                                          std::vector<std::string> *labels) {
  std::vector<std::string> extraLabels;
  switch (imu_model) {
  case Imu_BG_BA_TG_TS_TA::kModelId:
    extraLabels = {"Tg_1", "Tg_2", "Tg_3", "Tg_4", "Tg_5", "Tg_6", "Tg_7",
                   "Tg_8", "Tg_9", "Ts_1", "Ts_2", "Ts_3", "Ts_4", "Ts_5",
                   "Ts_6", "Ts_7", "Ts_8", "Ts_9", "Ta_1", "Ta_2", "Ta_3",
                   "Ta_4", "Ta_5", "Ta_6", "Ta_7", "Ta_8", "Ta_9"};
    break;
  case ScaledMisalignedImu::kModelId:
    extraLabels = {"Mg_11", "Mg_21",       "Mg_22",       "Mg_31",      "Mg_32",
                   "Mg_33", "A_11",        "A_12",        "A_13",       "A_21",
                   "A_22",  "A_23",        "A_31",        "A_32",       "A_33",
                   "Ma_11", "Ma_21",       "Ma_22",       "Ma_31",      "Ma_32",
                   "Ma_33", "theta_g_a_x", "theta_g_a_y", "theta_g_a_z"};
    break;
  case Imu_BG_BA::kModelId:
  default:
    break;
  }
  *labels = extraLabels;
}

inline void
ImuModelToAugmentedDimensionLabels(const int imu_model,
                                   std::vector<std::string> *labels) {
  std::vector<std::string> extraLabels;
  switch (imu_model) {
  case Imu_BG_BA_TG_TS_TA::kModelId:
    extraLabels = {"Tg_1", "Tg_2", "Tg_3", "Tg_4", "Tg_5", "Tg_6", "Tg_7",
                   "Tg_8", "Tg_9", "Ts_1", "Ts_2", "Ts_3", "Ts_4", "Ts_5",
                   "Ts_6", "Ts_7", "Ts_8", "Ts_9", "Ta_1", "Ta_2", "Ta_3",
                   "Ta_4", "Ta_5", "Ta_6", "Ta_7", "Ta_8", "Ta_9"};
    break;
  case ScaledMisalignedImu::kModelId:
    extraLabels = {"Mg_11", "Mg_21",   "Mg_22",   "Mg_31",   "Mg_32",
                   "Mg_33", "A_11",    "A_12",    "A_13",    "A_21",
                   "A_22",  "A_23",    "A_31",    "A_32",    "A_33",
                   "Ma_11", "Ma_21",   "Ma_22",   "Ma_31",   "Ma_32",
                   "Ma_33", "q_g_a_x", "q_g_a_y", "q_g_a_z", "q_g_a_w"};
    break;
  case Imu_BG_BA::kModelId:
  default:
    break;
  }
  *labels = extraLabels;
}

inline void ImuModelToDimensionLabels(const int imu_model,
                                      std::vector<std::string> *labels) {
  *labels = {"b_g_x[rad/s]", "b_g_y", "b_g_z",
             "b_a_x[m/s^2]", "b_a_y", "b_a_z"};
  std::vector<std::string> extraLabels;
  ImuModelToAugmentedDimensionLabels(imu_model, &extraLabels);
  labels->insert(labels->end(), extraLabels.begin(), extraLabels.end());
}

inline int ImuModelNameToId(std::string imu_error_model_descrip) {
  std::transform(imu_error_model_descrip.begin(), imu_error_model_descrip.end(),
                 imu_error_model_descrip.begin(),
                 [](unsigned char c) { return std::toupper(c); });
  if (imu_error_model_descrip.compare("SCALEDMISALIGNED") == 0) {
    return ScaledMisalignedImu::kModelId;
  } else if (imu_error_model_descrip.compare("BG_BA_TG_TS_TA") == 0) {
    return Imu_BG_BA_TG_TS_TA::kModelId;
  } else if (imu_error_model_descrip.compare("BG_BA") == 0) {
    return Imu_BG_BA::kModelId;
  } else {
    return Imu_BG_BA_TG_TS_TA::kModelId;
  }
}

inline Eigen::Matrix<double, Eigen::Dynamic, 1> ImuModelNominalAugmentedParams(int model_id) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
    case ImuModel::kModelId:             \
  return ImuModel::getNominalAugmentedParams<double>();

    MODEL_SWITCH_CASES

    #undef IMU_ERROR_MODEL_CASE
    #undef MODEL_CASES
    }
}

inline void ImuModelPredict(int model_id, const Eigen::Vector3d& bg, const Eigen::Vector3d& ba,
                            const Eigen::Matrix<double, Eigen::Dynamic, 1>& params,
                            const Eigen::Matrix<double, 3, 1>& w_b, const Eigen::Matrix<double, 3, 1>& a_b,
                            Eigen::Matrix<double, 3, 1>* w, Eigen::Matrix<double, 3, 1>* a) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
    case ImuModel::kModelId:             \
  return ImuModel::predict<double>(bg, ba, params, w_b, a_b, w, a);

    MODEL_SWITCH_CASES

    #undef IMU_ERROR_MODEL_CASE
    #undef MODEL_CASES
    }
}

inline void ImuModelCorrect(int model_id,
                            const Eigen::Vector3d& bg, const Eigen::Vector3d& ba,
                            const Eigen::Matrix<double, Eigen::Dynamic, 1>& params,
                            const Eigen::Matrix<double, 3, 1>& w, const Eigen::Matrix<double, 3, 1>& a,
                            Eigen::Matrix<double, 3, 1>* w_b, Eigen::Matrix<double, 3, 1>* a_b) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
    case ImuModel::kModelId:             \
  return ImuModel::correct<double>(bg, ba, params, w, a, w_b, a_b);

    MODEL_SWITCH_CASES

    #undef IMU_ERROR_MODEL_CASE
    #undef MODEL_CASES
    }
}

inline Eigen::VectorXd ImuModelComputeAugmentedParamsError(
    int model_id, const Eigen::VectorXd& parameters) {
  switch (model_id) {
#define MODEL_CASES IMU_ERROR_MODEL_CASES
#define IMU_ERROR_MODEL_CASE(ImuModel) \
    case ImuModel::kModelId:             \
  return ImuModel::computeAugmentedParamsError(parameters);

    MODEL_SWITCH_CASES

    #undef IMU_ERROR_MODEL_CASE
    #undef MODEL_CASES
  }
}
}  // namespace swift_vio
#endif  // INCLUDE_SWIFT_VIO_IMU_ERROR_MODELS_HPP_


#include <Eigen/Dense>
#include <Utils.hpp>

#include "FermionOperator.hpp"
#include "ObservableTransform.hpp"
#include "PauliOperator.hpp"
#include "cppmicroservices/BundleActivator.h"
#include "cppmicroservices/BundleContext.h"
#include "cppmicroservices/ServiceProperties.h"
#include "qrt.hpp"
#include "xacc.hpp"
#include "xacc_internal_compiler.hpp"
#include "xacc_observable.hpp"
#include "xacc_service.hpp"
#include "CommonGates.hpp"

using namespace cppmicroservices;
using namespace xacc;

namespace qcor {
template <typename T>
bool ptr_is_a(std::shared_ptr<Observable> ptr) {
  return std::dynamic_pointer_cast<T>(ptr) != nullptr;
}
class NISQ : public ::quantum::QuantumRuntime {
 protected:
  std::shared_ptr<xacc::CompositeInstruction> program;
  std::shared_ptr<xacc::IRProvider> provider;

  void one_qubit_inst(const std::string &name, const qubit &qidx,
                      std::vector<double> parameters = {}) {
    auto inst = provider->createInstruction(
        name, std::vector<std::size_t>{qidx.second});
    inst->setBufferNames({qidx.first});
    for (int i = 0; i < parameters.size(); i++) {
      inst->setParameter(i, parameters[i]);
    }
    // Not in a controlled-block
    // if (xacc::internal_compiler::__controlledIdx.empty()) {
    // Add the instruction
    program->addInstruction(inst);
    // } else {
    //   // In a controlled block:
    //   add_controlled_inst(inst, __controlledIdx[0]);
    // }
  }

  void two_qubit_inst(const std::string &name, const qubit &qidx1,
                      const qubit &qidx2, std::vector<double> parameters = {}) {
    auto inst = provider->createInstruction(
        name, std::vector<std::size_t>{qidx1.second, qidx2.second});
    inst->setBufferNames({qidx1.first, qidx2.first});
    for (int i = 0; i < parameters.size(); i++) {
      inst->setParameter(i, parameters[i]);
    }
    // Not in a controlled-block
    // if (xacc::internal_compiler::__controlledIdx.empty()) {
    program->addInstruction(inst);
    // } else {
    //   // In a controlled block:
    //   add_controlled_inst(inst, __controlledIdx[0]);
    // }
  }

 public:
  void initialize(const std::string kernel_name) override {
    provider = xacc::getIRProvider("quantum");
    program = provider->createComposite(kernel_name);
  }

  void h(const qubit &qidx) override { one_qubit_inst("H", qidx); }
  void x(const qubit &qidx) override { one_qubit_inst("X", qidx); }
  void y(const qubit &qidx) override { one_qubit_inst("Y", qidx); }
  void z(const qubit &qidx) override { one_qubit_inst("Z", qidx); }

  void s(const qubit &qidx) override { one_qubit_inst("S", qidx); }
  void sdg(const qubit &qidx) override { one_qubit_inst("Sdg", qidx); }

  void t(const qubit &qidx) override { one_qubit_inst("T", qidx); }
  void tdg(const qubit &qidx) override { one_qubit_inst("Tdg", qidx); }

  void rx(const qubit &qidx, const double theta) override {
    one_qubit_inst("Rx", qidx, {theta});
  }

  void ry(const qubit &qidx, const double theta) override {
    one_qubit_inst("Ry", qidx, {theta});
  }

  void rz(const qubit &qidx, const double theta) override {
    one_qubit_inst("Rz", qidx, {theta});
  }

  void u1(const qubit &qidx, const double theta) override {
    one_qubit_inst("U1", qidx, {theta});
  }

  void u3(const qubit &qidx, const double theta, const double phi,
          const double lambda) override {
    one_qubit_inst("U", qidx, {theta, phi, lambda});
  }

  void reset(const qubit &qidx) override { one_qubit_inst("Reset", qidx); }

  bool mz(const qubit &qidx) override {
    one_qubit_inst("Measure", qidx);
    return false;
  }

  void cnot(const qubit &src_idx, const qubit &tgt_idx) override {
    two_qubit_inst("CNOT", src_idx, tgt_idx);
  }

  void cy(const qubit &src_idx, const qubit &tgt_idx) override {
    two_qubit_inst("CY", src_idx, tgt_idx);
  }

  void cz(const qubit &src_idx, const qubit &tgt_idx) override {
    two_qubit_inst("CZ", src_idx, tgt_idx);
  }

  void ch(const qubit &src_idx, const qubit &tgt_idx) override {
    two_qubit_inst("CH", src_idx, tgt_idx);
  }

  void swap(const qubit &src_idx, const qubit &tgt_idx) override {
    two_qubit_inst("Swap", src_idx, tgt_idx);
  }

  void cphase(const qubit &src_idx, const qubit &tgt_idx,
              const double theta) override {
    two_qubit_inst("CPhase", src_idx, tgt_idx, {theta});
  }

  void crz(const qubit &src_idx, const qubit &tgt_idx,
           const double theta) override {
    two_qubit_inst("CRZ", src_idx, tgt_idx, {theta});
  }

  void exp(qreg q, const double theta, xacc::Observable &H) override {
    exp(q, theta, xacc::as_shared_ptr(&H));
  }

  void exp(qreg q, const double theta, xacc::Observable *H) override {
    exp(q, theta, xacc::as_shared_ptr(H));
  }

  void exp(qreg q, const double theta,
           std::shared_ptr<xacc::Observable> Hptr_input) override {
    std::map<std::string, xacc::quantum::Term> terms;

    auto obs_str = Hptr_input->toString();
    auto fermi_to_pauli = xacc::getService<xacc::ObservableTransform>("jw");
    std::shared_ptr<xacc::Observable> Hptr;
    if (ptr_is_a<xacc::quantum::FermionOperator>(Hptr_input)) {
      Hptr = fermi_to_pauli->transform(Hptr_input);
    } else if (obs_str.find("^") != std::string::npos) {
      auto fermionObservable = xacc::quantum::getObservable("fermion", obs_str);
      Hptr = fermi_to_pauli->transform(fermionObservable);
    } else if (ptr_is_a<xacc::quantum::PauliOperator>(Hptr_input)) {
      Hptr = Hptr_input;
    } else if (obs_str.find("X") != std::string::npos ||
               obs_str.find("Y") != std::string::npos ||
               obs_str.find("Z") != std::string::npos) {
      Hptr = xacc::quantum::getObservable("pauli", obs_str);
    } else {
      xacc::error(
          "[qcor::exp()] Error, cannot cast incoming Observable ptr to "
          "something we can process.");
    }

    // Convert the IR into a Hamiltonian
    xacc::quantum::PauliOperator &H =
        dynamic_cast<xacc::quantum::PauliOperator &>(*Hptr.get());
    terms = H.getTerms();

    double pi = xacc::constants::pi;
    auto gateRegistry = xacc::getIRProvider("quantum");
    std::vector<xacc::InstPtr> exp_insts;

    auto q_name = q.name();
    for (auto inst : terms) {
      auto spinInst = inst.second;
      if (spinInst.isIdentity()) {
        continue;
      }
      // Get the individual pauli terms
      auto termsMap = std::get<2>(spinInst);

      std::vector<std::pair<int, std::string>> terms;
      for (auto &kv : termsMap) {
        if (kv.second != "I" && !kv.second.empty()) {
          terms.push_back({kv.first, kv.second});
        }
      }
      // The largest qubit index is on the last term
      int largestQbitIdx = terms[terms.size() - 1].first;

      std::vector<std::size_t> qidxs;
      std::vector<xacc::InstPtr> basis_front, basis_back;

      for (auto &term : terms) {
        auto qid = term.first;
        auto pop = term.second;

        qidxs.push_back(qid);

        if (pop == "X") {
          basis_front.emplace_back(std::make_shared<xacc::quantum::Hadamard>(qid));
          basis_back.emplace_back(std::make_shared<xacc::quantum::Hadamard>(qid));
        } else if (pop == "Y") {
          basis_front.emplace_back(std::make_shared<xacc::quantum::Rx>(qid, 1.57079362679));
          basis_back.emplace_back(std::make_shared<xacc::quantum::Rx>(qid, -1.57079362679));
        }
      }

      // std::cout << "QIDS:  " << qidxs << "\n";

      Eigen::MatrixXi cnot_pairs(2, qidxs.size() - 1);
      for (int i = 0; i < qidxs.size() - 1; i++) {
        cnot_pairs(0, i) = qidxs[i];
      }
      for (int i = 0; i < qidxs.size() - 1; i++) {
        cnot_pairs(1, i) = qidxs[i + 1];
      }

      // std::cout << "HOWDY: \n" << cnot_pairs << "\n";
      std::vector<xacc::InstPtr> cnot_front, cnot_back;
      for (int i = 0; i < qidxs.size() - 1; i++) {
        Eigen::VectorXi pairs = cnot_pairs.col(i);
        auto c = pairs(0);
        auto t = pairs(1);
        cnot_front.emplace_back(std::make_shared<xacc::quantum::CNOT>(c, t));
      }

      for (int i = qidxs.size() - 2; i >= 0; i--) {
        Eigen::VectorXi pairs = cnot_pairs.col(i);
        auto c = pairs(0);
        auto t = pairs(1);
        cnot_back.emplace_back(std::make_shared<xacc::quantum::CNOT>(c, t));
      }
      exp_insts.insert(exp_insts.end(),
                       std::make_move_iterator(basis_front.begin()),
                       std::make_move_iterator(basis_front.end()));
      exp_insts.insert(exp_insts.end(),
                       std::make_move_iterator(cnot_front.begin()),
                       std::make_move_iterator(cnot_front.end()));

      // FIXME, we assume real coefficients, if its zero,
      // check that the imag part is not zero and use it
      if (std::fabs(std::real(spinInst.coeff())) > 1e-12) {
        exp_insts.emplace_back(std::make_shared<xacc::quantum::Rz>(
            qidxs[qidxs.size() - 1], std::real(spinInst.coeff()) * theta));
      } else if (std::fabs(std::imag(spinInst.coeff())) > 1e-12) {
        exp_insts.emplace_back(std::make_shared<xacc::quantum::Rz>(
            qidxs[qidxs.size() - 1], std::imag(spinInst.coeff()) * theta));
      }
      exp_insts.insert(exp_insts.end(),
                       std::make_move_iterator(cnot_back.begin()),
                       std::make_move_iterator(cnot_back.end()));
      exp_insts.insert(exp_insts.end(),
                       std::make_move_iterator(basis_back.begin()),
                       std::make_move_iterator(basis_back.end()));
    }
    for (auto& inst : exp_insts) {
      inst->setBufferNames(std::vector<std::string>(inst->bits().size(), q_name));
      program->addInstruction(inst);
    }
  }

  void submit(xacc::AcceleratorBuffer *buffer) override {
    // xacc::internal_compiler::execute_pass_manager();
    xacc::internal_compiler::execute(buffer, program);
    clearProgram();
  }

  void submit(xacc::AcceleratorBuffer **buffers, const int nBuffers) override {
    xacc::internal_compiler::execute(buffers, nBuffers, program);
  }

  void set_current_program(
      std::shared_ptr<xacc::CompositeInstruction> p) override {
    program = p;
  }
  std::shared_ptr<xacc::CompositeInstruction> get_current_program() override {
    return program;
  }
  void clearProgram() {
    if (program && provider)
      program = provider->createComposite(program->name());
  }

  void set_current_buffer(xacc::AcceleratorBuffer *buffer) override {
    // Nothing to do: the NISQ runtime doesn't keep track of runtime buffer
    // info.
  }

  const std::string name() const override { return "nisq"; }
  const std::string description() const override { return ""; }
};
}  // namespace qcor

namespace {

/**
 */
class US_ABI_LOCAL NisqQRTActivator : public BundleActivator {
 public:
  NisqQRTActivator() {}

  /**
   */
  void Start(BundleContext context) {
    auto xt = std::make_shared<qcor::NISQ>();
    context.RegisterService<::quantum::QuantumRuntime>(xt);
  }

  /**
   */
  void Stop(BundleContext /*context*/) {}
};

}  // namespace

CPPMICROSERVICES_EXPORT_BUNDLE_ACTIVATOR(NisqQRTActivator)


#ifdef STAN_OPENCL

#include <stan/math/prim/mat.hpp>
#include <stan/math/opencl/multiply_transpose.hpp>
#include <stan/math/opencl/copy.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <algorithm>
boost::random::mt19937 rng;

#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(A(i), B(i), DELTA);

TEST(MathMatrix, multiply_transpose_exception_fail_zero) {
  stan::math::row_vector_d rv(0);
  stan::math::matrix_d m(0, 3);
  stan::math::matrix_d m1(2, 0);
  stan::math::matrix_cl mm(m);
  stan::math::matrix_cl mm2(m1);
  stan::math::matrix_cl rvv(rv);
  stan::math::matrix_cl ans_mm1(1, 1);
  stan::math::matrix_cl ans_mm2(0, 0);
  stan::math::matrix_cl ans_mm3(2, 2);
  EXPECT_NO_THROW(ans_mm1 = stan::math::multiply_transpose(rvv));
  EXPECT_NO_THROW(ans_mm2 = stan::math::multiply_transpose(mm));
  EXPECT_NO_THROW(ans_mm3 = stan::math::multiply_transpose(mm2));
}

TEST(MathMatrix, multiply_m_m_exception_pass_dim) {
  stan::math::matrix_d m1(1, 3);
  stan::math::matrix_d m2(3, 5);
  stan::math::matrix_cl mm1(m1);
  stan::math::matrix_cl mm2(m2);
  stan::math::matrix_cl mm3a(1, 1);
  stan::math::matrix_cl mm3b(3, 3);
  EXPECT_NO_THROW(mm3a = stan::math::multiply_transpose(mm1));
  EXPECT_NO_THROW(mm3b = stan::math::multiply_transpose(mm2));
}

TEST(MathMatrix, multiply_zero_size) {
  stan::math::vector_d v0;
  stan::math::row_vector_d rv0;
  stan::math::matrix_d m0;
  stan::math::matrix_cl v00(v0);
  stan::math::matrix_cl rv00(rv0);
  stan::math::matrix_cl m00(m0);
  EXPECT_NO_THROW(stan::math::multiply_transpose(v00));
  EXPECT_NO_THROW(stan::math::multiply_transpose(rv00));
  EXPECT_NO_THROW(stan::math::multiply_transpose(m00));
}

TEST(AgradRevMatrix, multiply_transposed_small) {
  using stan::math::multiply;
  auto m1 = stan::math::matrix_d::Random(3, 3).eval();
  stan::math::matrix_d m2(3, 3);
  stan::math::matrix_d m2_cl(3, 3);

  m2 = m1 * m1.transpose();

  stan::math::matrix_cl m11(m1);
  stan::math::matrix_cl m22(3, 3);
  m22 = stan::math::multiply_transpose(m11);
  m2_cl = stan::math::from_matrix_cl(m22);

  EXPECT_MATRIX_NEAR(m2, m2_cl, 1e-10);
}

TEST(AgradRevMatrix, multiply_transposed_big) {
  using stan::math::multiply;
  stan::math::matrix_d m2, m2_cl;

  int size = 500;
  auto m1 = stan::math::matrix_d::Random(size, size).eval();
  m2.resize(size, size);
  m2_cl.resize(size, size);
  m2 = m1 * m1.transpose();

  stan::math::matrix_cl m11(m1);
  stan::math::matrix_cl m22(size, size);
  m22 = stan::math::multiply_transpose(m11);
  m2_cl = stan::math::from_matrix_cl(m22);
  EXPECT_MATRIX_NEAR(m2, m2_cl, 1e-10);
}

TEST(AgradRevMatrix, multiply_transposed_big_non_square) {
  using stan::math::multiply;
  stan::math::matrix_d m2, m2_cl;

  int size_x = 200;
  int size_y = 500;
  auto m1 = stan::math::matrix_d::Random(size_x, size_y).eval();
  m2.resize(size_x, size_x);
  m2_cl.resize(size_x, size_x);

  m2 = m1 * m1.transpose();

  stan::math::matrix_cl m11(m1);
  stan::math::matrix_cl m22(size_x, size_x);
  m22 = stan::math::multiply_transpose(m11);
  m2_cl = stan::math::from_matrix_cl(m22);

  EXPECT_MATRIX_NEAR(m2, m2_cl, 1e-10);
}

TEST(AgradRevMatrix, multiply_transposed_big_x) {
  using stan::math::multiply;
  stan::math::matrix_d m2, m2_cl;

  int size_x = 200;
  int size_y = 50;
  auto m1 = stan::math::matrix_d::Random(size_x, size_y).eval();
  m2.resize(size_x, size_x);
  m2_cl.resize(size_x, size_x);

  m2 = m1 * m1.transpose();

  stan::math::matrix_cl m11(m1);
  stan::math::matrix_cl m22(size_x, size_x);
  m22 = stan::math::multiply_transpose(m11);
  m2_cl = stan::math::from_matrix_cl(m22);

  EXPECT_MATRIX_NEAR(m2, m2_cl, 1e-10);
}

#endif


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_MEMORY_DETAILS_LEAD_PADDING_HPP_INCLUDED
#define NT2_SDK_MEMORY_DETAILS_LEAD_PADDING_HPP_INCLUDED

////////////////////////////////////////////////////////////////////////////////
// Implementation of the no_padding strategy for memory allocation
////////////////////////////////////////////////////////////////////////////////
#include <nt2/sdk/meta/fusion.hpp>
#include <boost/typeof/typeof.hpp>
#include <nt2/sdk/memory/slice.hpp>
#include <nt2/sdk/memory/stride.hpp>
#include <nt2/sdk/memory/align_on.hpp>
#include <nt2/sdk/memory/details/no_padding.hpp>
#include <nt2/sdk/functor/preprocessor/call.hpp>

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct  call< tag::slice_ ( tag::fusion_sequence_
                            , tag::padding_<memory::lead_padding>
                            , tag::mpl_integral_<tag::integer_>
                            )
              , tag::cpu_, Dummy  >
        : callable
  {
    ////////////////////////////////////////////////////////////////////////////
    // Computes the actual result type depending on A0 size and A2 value
    ////////////////////////////////////////////////////////////////////////////
    template<class Sig> struct result;

    template<class This,class A0, class A1, class A2>
    struct  result<This(A0,A1,A2)>
    {
      typedef typename meta::strip<A2>::type        arg0;
      typedef typename meta::strip<A2>::type        arg2;
      typedef boost::fusion::result_of::size<arg0>  size_;
      static  typename meta::strip<A0>::type const& s;

      BOOST_TYPEOF_NESTED_TYPEDEF_TPL
      ( true_case
      , nt2::mul( slice<2>(s,memory::no_padding())
                , memory::align_on( boost::fusion::at_c<0>(s) )
                )
      );

      BOOST_TYPEOF_NESTED_TYPEDEF_TPL
      ( false_case, slice<arg2::value>(s,memory::no_padding())
      );

      typedef typename boost::mpl::eval_if_c< (arg2::value == 1)
                                            , true_case
                                            , false_case
                                            >::type type;
    };

    ////////////////////////////////////////////////////////////////////////////
    // Implementation when A2 == 1
    ////////////////////////////////////////////////////////////////////////////
    template<class A0, class A1, class A2> inline
    typename boost::lazy_enable_if_c< (A2::value==1), NT2_RETURN_TYPE(3)>::type
    operator()( A0 const& a0, A1 const&, A2 const& ) const
    {
      return nt2::mul ( slice<2>(a0,memory::no_padding())
                      , memory::align_on( boost::fusion::at_c<0>(a0) )
                      );
    }

    ////////////////////////////////////////////////////////////////////////////
    // Implementation when A2 >= 1
    ////////////////////////////////////////////////////////////////////////////
    template<class A0, class A1, class A2> inline
    typename boost::lazy_enable_if_c< (A2::value!=1), NT2_RETURN_TYPE(3)>::type
    operator()( A0 const& a0, A1 const&, A2 const& ) const
    {
      return slice<A2::value>(a0,memory::no_padding());
    }
  };
} }

////////////////////////////////////////////////////////////////////////////////
// stride Functor implementation - Do nothing except on inner dimension
////////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  template<class Dummy>
  struct  call< tag::stride_( tag::fusion_sequence_
                            , tag::padding_<memory::lead_padding>
                            , tag::mpl_integral_<tag::integer_>
                            )
              , tag::cpu_, Dummy  >
        : callable
  {
    ////////////////////////////////////////////////////////////////////////////
    // Computes the actual result type depending on A0 size and A2 value
    ////////////////////////////////////////////////////////////////////////////
    template<class Sig> struct result;

    template<class This,class A0, class A1, class A2>
    struct  result<This(A0,A1,A2)>
    {
      typedef typename meta::strip<A2>::type        arg0;
      typedef typename meta::strip<A2>::type        arg2;
      static  typename meta::strip<A0>::type const& s;

      BOOST_TYPEOF_NESTED_TYPEDEF_TPL
      ( true_case
      , memory::align_on( boost::fusion::at_c<0>(s) )
      );

      BOOST_TYPEOF_NESTED_TYPEDEF_TPL
      ( false_case, boost::fusion::at_c<arg2::value-1>(s) );

      typedef typename boost::mpl::eval_if_c< (arg2::value == 1)
                                            , true_case
                                            , false_case
                                            >::type type;
    };

    ////////////////////////////////////////////////////////////////////////////
    // Implementation when A2 == 1
    ////////////////////////////////////////////////////////////////////////////
    template<class A0, class A1, class A2> inline
    typename boost::lazy_enable_if_c< (A2::value==1), NT2_RETURN_TYPE(3)>::type
    operator()( A0 const& a0, A1 const&, A2 const& ) const
    {
      return memory::align_on( boost::fusion::at_c<0>(a0) );
    }

    ////////////////////////////////////////////////////////////////////////////
    // Implementation when A2 >= 1
    ////////////////////////////////////////////////////////////////////////////
    template<class A0, class A1, class A2> inline
    typename boost::lazy_enable_if_c< (A2::value!=1), NT2_RETURN_TYPE(3)>::type
    operator()( A0 const& a0, A1 const&, A2 const& ) const
    {
      return boost::fusion::at_c<A2::value-1>(a0);
    }
  };
} }

#endif


#define BOOST_TEST_MODULE contigs_graph_builder test
#include <boost/test/unit_test.hpp>

#include <string>
#include <cmath>

#include "../dna/simple_immutable_dna.h"
#include "contigs_graph_builder.h"

using namespace std;

const double precision = 1e-3;

BOOST_AUTO_TEST_CASE( build_test )
{
    /*
     *            1->         2->       <-3
     * ATGCGATCGAAGCACC TTA ATGGCTT AT CACGAA GCG
     *             0           2          5
     *             1           3          4
     * TACGCTAGCTTCGTGG AAT TACCGAA TA GTGCTT CGC
     *             <-11       12->
     *                         <-22     21->
     *           <-31        32->
     *            <-42                   41->
     *
     */

    SimpleImmutableDna contig1("ATGCGATCGAAGCACC");
    SimpleImmutableDna contig2("ATGGCTT");
    SimpleImmutableDna contig3("TTCGTG");

    SimpleImmutableDna mp11("GGTG");
    SimpleImmutableDna mp12("GGCT");
    size_t d1 = 12; // real 13

    SimpleImmutableDna mp21("ACGA");
    SimpleImmutableDna mp22("AAGC");
    size_t d2 = 12; // real 11

    SimpleImmutableDna mp31("TGCT");
    SimpleImmutableDna mp32("TGGC");
    size_t d3 = 12; // real 14

    SimpleImmutableDna mp41("CGAA");
    SimpleImmutableDna mp42("GTGC");
    size_t d4 = 23; // real 23

    ContigsGraphBuilder builder(4, 15, 6);
    try
    {
        builder.add_contig(contig1);
        builder.add_contig(contig2);
        builder.add_contig(contig3);
    }
    catch (HashSetFullExcpetion const & e)
    {
        BOOST_FAIL("index capacity too small");
    }

    builder.add_connection(mp11, mp12, d1);
    builder.add_connection(mp21, mp22, d2);
    builder.add_connection(mp31, mp32, d3);
    builder.add_connection(mp41, mp42, d4);

    ContigsGraph graph = builder.build();

    BOOST_CHECK_EQUAL(graph.vertices.size(), 3u * 2u);
    BOOST_CHECK_EQUAL(graph.edges.size(), 3u * 2u);

    BOOST_CHECK_CLOSE(graph.edges[0][2].mean, 1.5, precision);
    BOOST_CHECK_CLOSE(graph.edges[0][2].standard_deviation, sqrt(2)/2., precision);
    BOOST_CHECK_EQUAL(graph.edges[0][2].weight, 2);

    BOOST_CHECK_CLOSE(graph.edges[0][5].mean, 12., precision);
    BOOST_CHECK_CLOSE(graph.edges[0][5].standard_deviation, 0, precision);
    BOOST_CHECK_EQUAL(graph.edges[0][5].weight, 1);

    BOOST_CHECK_CLOSE(graph.edges[2][5].mean, 3., precision);
    BOOST_CHECK_CLOSE(graph.edges[2][5].standard_deviation, 0, precision);
    BOOST_CHECK_EQUAL(graph.edges[2][5].weight, 1);

    BOOST_CHECK_CLOSE(graph.edges[3][1].mean, 1.5, precision);
    BOOST_CHECK_CLOSE(graph.edges[3][1].standard_deviation, sqrt(2)/2., precision);
    BOOST_CHECK_EQUAL(graph.edges[3][1].weight, 2);

    BOOST_CHECK_CLOSE(graph.edges[4][1].mean, 12., precision);
    BOOST_CHECK_CLOSE(graph.edges[4][1].standard_deviation, 0, precision);
    BOOST_CHECK_EQUAL(graph.edges[4][1].weight, 1);

    BOOST_CHECK_CLOSE(graph.edges[4][3].mean, 3., precision);
    BOOST_CHECK_CLOSE(graph.edges[4][3].standard_deviation, 0, precision);
    BOOST_CHECK_EQUAL(graph.edges[4][3].weight, 1);

}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::common::generator.hpp                 	 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_COMMON_GENERATOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_COMMON_GENERATOR_HPP_ER_2009
#include <algorithm>
#include <vector>
#include <iterator>
#include <boost/fusion/include/make_map.hpp>
#include <boost/statistics/detail/fusion/joint_view/bind_iterator.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace response{

    // e.g. Factory = factory2<event<T> >
    template<typename Factory>
	class generator{
    	public:

        typedef Factory factory_type;
        typedef typename factory_type::tag tag_y_;
        typedef typename factory_type::result_type y_;
        typedef record::tag::entry_time tag_et_;

        // Side effect : writes to out the responses
        // Treats failure times in [b_ft,e_ft) as measured from entry_time
        template<typename ItY,typename T,typename It,typename T1>
        static ItY call(
        	ItY out,
        	const T& entry_time,
            It b_ft,
            It e_ft,
            const T1& time_analysis
        ){
            typedef typename
            	boost::fusion::result_of::make_map<tag_et_,T1>::type et_;
            et_ et = boost::fusion::make_map<tag_et_>(entry_time);
        	BOOST_ASSERT(entry_time <time_analysis);

			namespace jv = statistics::detail::fusion::joint_view;
            return std::transform(
                jv::bind_iterator(et,b_ft),
                jv::bind_iterator(et,e_ft),
                out,
                factory_type( time_analysis )
            );
		}

    };

}// response
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef NT2_INTERPOL_FUNCTIONS_IDX_LINEAR_HPP_INCLUDED
#define NT2_INTERPOL_FUNCTIONS_IDX_LINEAR_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/core/container/dsl/value_type.hpp>
#include <boost/mpl/bool.hpp>
#include <nt2/sdk/meta/adapted_traits.hpp>
/*!
 * \ingroup interpol
 * \defgroupinterpol_idx_linear idx_linear
 *
 * \par Description
 * one dimensional idx_linear interpolation
 * given an array a and a vector idx of "real" indices
 * provides the interpolated values along the a rows
 * by linear formula.
 * calls can be idx_linear(a, idx)
 *              idx_linear(a, idx, true) allowing extrapolation
 *              idx_linear(a, idx, val1) putting val1 outside the bounds
 *              idx_linear(a, idx, val1, val2)}, putting val1 under the index bounds val2 ovr
 *              idx_linear(a, idx, _, dim) dim is the dimension of interpolation firstnonsingletopn of xi by default
 *              idx_linear(a, idx, val1, dim)
 *              idx_linear(a, idx, val1, val2, dim) can also be used
 * \par
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/idx_linear.hpp>
 * \endcode
 *
**/

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag idx_linear_ of functor idx_linear
     *        in namespace nt2::tag for toolbox statistics
    **/
    struct idx_linear_ : ext::unspecified_<idx_linear_> { typedef ext::unspecified_<idx_linear_> parent; };
  }
  NT2_FUNCTION_IMPLEMENTATION(tag::idx_linear_, idx_linear, 2)
  NT2_FUNCTION_IMPLEMENTATION(tag::idx_linear_, idx_linear, 3)
  NT2_FUNCTION_IMPLEMENTATION(tag::idx_linear_, idx_linear, 4)
  NT2_FUNCTION_IMPLEMENTATION(tag::idx_linear_, idx_linear, 5)
}

namespace nt2 { namespace ext
{
  template<class Domain, class Expr,  int N>
  struct size_of<tag::idx_linear_, Domain, N, Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr,N-1>::value_type v_t;
    typedef typename nt2::meta::is_integral<v_t>::type                      c_t;

    typedef typename  boost::proto::result_of
                      ::child_c<Expr&,0>::value_type::extent_type   result_type;

    result_type operator()(Expr& e) const
    {
      std::size_t dim = nt2::firstnonsingleton(boost::proto::child_c<1>(e));
      result_type sizee = boost::proto::child_c<0>(e).extent();
      sizee[dim-1] = numel(boost::proto::child_c<1>(e));
      return sizee;
    }
  };

  template <class Domain, class Expr,  int N>
  struct  value_type < tag::idx_linear_, Domain,N,Expr>
        : meta::value_as<Expr,0>
  {};
} }

#endif


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

  Copyright 2015 NumScale SAS

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

STF_CASE_TPL (" clz real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::clz;
  using r_t = decltype(clz(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(clz(bs::Nan<T>()), bs::Zero<r_t>());
#endif
  STF_EQUAL(clz(bs::Mone<T>()), bs::Zero<r_t>());
  STF_EQUAL(clz(bs::Signmask<T>()),bs::Zero<r_t>());
} // end of test for real_

STF_CASE_TPL (" clz signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::clz;
  using r_t = decltype(clz(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
  STF_EQUAL(clz(bs::One<T>()), r_t(sizeof(T)*8 -1));
  STF_EQUAL(clz(bs::Signmask<T>()), bs::Zero<r_t>());
} // end of test for signed_int_

 STF_CASE_TPL (" clz unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
 {
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::clz;
  using r_t = decltype(clz(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  // specific values tests
  STF_EQUAL(clz(bs::One<T>()),  r_t(sizeof(T)*8-bs::One<r_t>()));

  T j = 1;
  for(int i=1; i < bs::Valmax<char>(); i*= 2)
    {
      STF_EQUAL(clz(T(i)),T(sizeof(T)*8- T(j)));
      ++j;
    }
 } // end of test for unsigned_int_


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ExplicitRecomputationTest

#include <memory>
#include <vector>

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <onnxutil.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/nll.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

using namespace popart;

// Detailed test description:
//
// Compute the forward pass which is made of MatMuls and Adds
// Normal Ops or Ops marked to not be recomputed do not perform recompute
// In the backward pass, we have MatMul and Add that are in fact from the
// forward pass. They have therefore marked as recomputed
// Finally, we have gradient computations and var updates as normal

TensorId gemm(Builder *builder, TensorId ip, TensorId w0, TensorId b0) {
  return builder->customOp(Onnx::AiOnnx::OpSet10::Gemm,
                           10,
                           {ip, w0, b0},
                           1,
                           {{"__recompute_output_in_backward_pass",
                             static_cast<int>(RecomputeType::Recompute)}},
                           "CustomGEMM")[0];
}

BOOST_AUTO_TEST_CASE(ExplicitRecomputation_Case) {

  // Build the following three layered NN:
  // (in0) -> [GEMM0] -> [GEMM1] -> [GEMM2] -> (out)
  // turn on recomputation and check if the IR contains recomputation
  // The purpose of any recomputation is to trade computation for memory

  auto test = [](bool recomputation) {
    auto builder = Builder::create();
    auto aiOnnx  = builder->aiOnnxOpset10();

    TensorInfo wshape{"FLOAT", std::vector<int64_t>{10, 10}};
    TensorInfo bshape{"FLOAT", std::vector<int64_t>{10, 1}};

    std::vector<float> ones(1.0f, 1.0f);
    ConstVoidData ones_data = {ones.data(), wshape};

    std::vector<float> zeros(0.0f, 1);
    ConstVoidData zeros_data = {ones.data(), bshape};

    auto ip0 = builder->addInputTensor(wshape);

    auto w0 = builder->addInitializedInputTensor(ones_data);
    auto b0 = builder->addInitializedInputTensor(zeros_data);

    auto m1 = gemm(builder.get(), ip0, w0, b0);
    auto m2 = gemm(builder.get(), m1, w0, b0);
    auto m3 = gemm(builder.get(), m2, w0, b0);

    auto out      = m3;
    auto l1       = builder->aiGraphcoreOpset1().l1loss({out}, 0.1);
    auto nMatMuls = 3;

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);

    auto dataFlow = DataFlow(1, {out}, AnchorReturnType("All"));

    auto optimizer = ConstSGD(0.01);

    auto device = createTestDevice(TEST_TARGET);
    SessionOptions opts;
    opts.enableOutlining = false;

    if (recomputation) {
      opts.explicitRecomputation = true;
    } else {
      opts.explicitRecomputation = false;
    }

    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                l1,
                &optimizer,
                *device,
                opts,
                Patterns(PatternsLevel::All)});

    auto nRecompute = 0;
    for (auto op : ir.getOpSchedule({}, RequireOptimalSchedule::Yes)) {
      if (recomputation) {
        // All Ops except those that have their type set to Recomputed
        // must have their recompute type set to Checkpoint
        if (op->settings.recomputeType != RecomputeType::Recomputed) {
          BOOST_CHECK(op->settings.recomputeType == RecomputeType::Checkpoint);
        }

        // Count the number of MatMuls with a recompute flag
        if ((op->settings.recomputeType == RecomputeType::Recomputed) &&
            (!op->opid.type.compare("MatMul"))) {
          nRecompute++;
        }
      } else {
        // All Forward pass Ops must have recomputeType set to Recompute
        // if explicit recomputation is turned off and path is not from loss
        if (op->fromLoss == PathFromLoss::No) {
          BOOST_CHECK(op->settings.recomputeType == RecomputeType::Recompute);
        }

        // All Backwards pass Ops must have recomputeType set to Checkpoint
        // if explicit recomputation is turned off and path is from Loss
        if (op->fromLoss == PathFromLoss::Yes) {
          BOOST_CHECK(op->settings.recomputeType == RecomputeType::Checkpoint);
        }
      }
    }
    // Verify that the number of recomputation equals to the num ops
    if (recomputation) {
      BOOST_CHECK(nRecompute == nMatMuls);
    }
  };

  // test(true);
  // test(false);
}

BOOST_AUTO_TEST_CASE(ExplicitRecomputation_Case1) {
  // Build the graph:
  //
  // in0 -- Add -- Relu -- L1Loss -- out
  //       /     (recomp)
  // in1 -
  //
  // and verify that the recomputed Relu tensor is scheduled as expected:
  // after the loss gradient has been computed
  auto builder = Builder::create();
  auto ip0     = builder->addInputTensor("FLOAT", std::vector<int64_t>{10});
  auto ip1     = builder->addInputTensor("FLOAT", std::vector<int64_t>{10, 10});

  auto rel = builder->aiOnnxOpset10().relu({ip0});
  auto out = builder->aiOnnxOpset10().add({rel, ip1});
  auto l1  = builder->aiGraphcoreOpset1().l1loss({out}, 0.1);
  auto id  = builder->aiGraphcoreOpset1().identityloss({l1});

  builder->recomputeOutputInBackwardPass(rel);
  auto modelProto = io::getModelFromString(builder->getModelProto());

  auto device    = createTestDevice(TEST_TARGET);
  auto optimizer = ConstSGD(0.01);
  SessionOptions opts;
  opts.explicitRecomputation      = true;
  std::vector<TensorId> anchorIds = {reservedGradientPrefix() + ip0, out};
  auto patterns                   = Patterns(PatternsLevel::All);
  // So that the identity loss is not pruned
  patterns.enableInPlace(false);
  patterns.enableOpToIdentity(false);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              DataFlow(1, anchorIds),
              id,
              &optimizer,
              *device,
              opts,
              patterns});

  auto opSchedule = ir.getOpSchedule({}, RequireOptimalSchedule::Yes);
  std::vector<size_t> recomputedIdx;
  std::vector<size_t> lossGradIdx;

  for (size_t idx = 0; idx < opSchedule.size(); idx++) {
    auto op = opSchedule.at(idx);

    // Register the position in the schedule of the recomputed Relu op
    if ((op->settings.recomputeType == RecomputeType::Recomputed) &&
        (!op->opid.type.compare("Relu"))) {
      recomputedIdx.push_back(idx);
    }

    // Register the position in the schedule of the loss gradient op
    if (!op->opid.type.compare("L1Grad")) {
      lossGradIdx.push_back(idx);
    }
  }
  // Verify that the recomputed Relu and the loss grad have been
  // found in the schedule
  BOOST_CHECK(recomputedIdx.size() == 1);
  BOOST_CHECK(lossGradIdx.size() == 1);

  // Verify that the recomputed Relu appears after the loss grad op
  // in the schedule
  BOOST_CHECK(recomputedIdx[0] > lossGradIdx[0]);
}


#include "MultiplicationExpression.h"
#include "exceptions/IllegalMultiplicationOperationException.h"
#include "ParenthesesExpression.h"
#include <boost/format.hpp>
#include <typeinfo>

using namespace thewizardplusplus::wizard_basic_2::compiler::translator;
using namespace thewizardplusplus::wizard_basic_2::compiler::translator::
	exceptions;
using namespace boost;

MultiplicationExpression::MultiplicationExpression(const Expression::Pointer&
	operand1, MultiplicationType::Types multiplication_type, const Expression::
	Pointer& operand2)
:
	Expression(ValueType::NUMBER),
	operand1(operand1),
	multiplication_type(multiplication_type),
	operand2(operand2)
{
	if (operand1->getType() != operand2->getType() || operand1->getType() !=
		ValueType::NUMBER)
	{
		throw IllegalMultiplicationOperationException(multiplication_type,
			operand1->getType(), operand2->getType());
	}

	this->operand1 = Expression::Pointer(new ParenthesesExpression(operand1));
	this->operand2 = Expression::Pointer(new ParenthesesExpression(operand2));
}

std::string MultiplicationExpression::getCppCode(void) const {
	return (format("%1% %2% %3%") % operand1->getCppCode() % MultiplicationType
		::convertToCppCode(multiplication_type) % operand2->getCppCode()).str();
}


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <exception>
#include <ctime>
#include <boost/program_options.hpp>
#include <boost/random.hpp>

#include "scene.h"

#include "../../src/parameters/ParamParser_getopt.hpp"
#include "../../src/pointsets/Pointset.hpp"
#include "../../src/io/fileIO.hpp"

double random01()
{
	static boost::mt19937 rng(time(NULL));
	static boost::uniform_01<boost::mt19937&> zeroone(rng);
	return zeroone();
}

int main(int argc, char** argv)
{
	/* ARG PARSER *****************************************************/

	bool help=false;
	int nReal=1;
	int nPts=1024;
	std::string fn_output;

	utk::ParamParser_getopt parser;

	parser.addShortOption('h', &help, 0, utk::assignBoolTrue, utk::displayBool, "\tDisplays this help message", "");
	parser.addShortOption('n', &nPts, 1, utk::assignInt, utk::displayInt, "[int=1024] \tThe number of samples", "");
	parser.addShortOption('o', &fn_output, 1, utk::assignString, utk::displayString, "[string]\tThe output file", "");
	parser.addShortOption('m', &nReal, 1, utk::assignInt, utk::displayInt, "[int=1]\tThe number of realisations", "Realisations:");

	parser.parse(argc, argv);

	if(fn_output.empty())
	{
		ERROR("Parameter -o mandatory");
		std::cout << parser.getHelp() << std::endl;
		return 0;
	}
	if(help)
	{
		std::cout << parser.getHelp() << std::endl;
		return 0;
	}

	int current_real=0;
	while(current_real<nReal)
	{

		/* PROG ***********************************************************/
		try
		{
			Scene bnot_scene;
			std::vector<Point> in_points;

			/* GENERATE ***************************************************/
			std::cout << "Intializing w  " << nPts << " random points" << std::endl;
			for(int i=0; i<nPts; i++)
				in_points.push_back(Point(random01(), random01()));
			std::cout << "Done" << std::endl;

			/* OPTIMIZE ***************************************************/

			std::vector<FT> noise(in_points.size(), 0.0);
			std::vector<FT> weights(in_points.size(), 0.0);
			std::cout << "Construct Tglation" << std::endl;
			bnot_scene.construct_triangulation(in_points, weights, noise);
			std::cout << "Done" << std::endl;

			std::cout << "Optimizing" << std::endl;
			//max newton iter, epsilon, maxiter, ...

			//high number of points
			if(nPts > 150)
				bnot_scene.optimize_all(0.0, 0.0, 500, 0.2, 500, std::cout, true);
			else
			//low number of points
				bnot_scene.optimize_all(0.0, 0.0, 20, 0.2, 20, std::cout, true);

			std::cout << "Done" << std::endl;

			std::vector<Point> out_points;
			bnot_scene.collect_visible_points(out_points);

			/* WRITE ******************************************************/
			{
				utk::Pointset<2, double, utk::Point<2, double> > pts;
				pts.resize(out_points.size());

				for(unsigned int i=0; i<out_points.size(); i++)
				{
					pts[i].pos()[0] = out_points.at(i).x();
					pts[i].pos()[1] = out_points.at(i).y();
				}

				utk::PointsetWriter<2, double, utk::Point<2, double> > writer;
				writer.open(fn_output);
				writer.writePointset(pts);
				writer.close();
			}

		}
		catch(const std::exception& e)
		{
			std::cerr << "Error : " << e.what() << std::endl;
			exit(EXIT_FAILURE);
		}

		current_real++;
	}

	exit(EXIT_SUCCESS);
}


//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <boost/tree/balance.hpp>
#include <boost/tree/binary_tree.hpp>
#include <boost/tree/detail/balancers/treap.hpp>
#include <boost/tree/detail/augmentors/unaugmented.hpp>

#include <vector>
#include <algorithm>

#define BOOST_TEST_MODULE treap_tree test
//#define BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>

//TODO: Use a mock_binary_tree
BOOST_AUTO_TEST_CASE( treap_test )
{
    using namespace boost::tree;

    std::vector<int> my_vector;
    typedef binary_tree<int> tree_t;
    //typedef test_searcher<false, tree_t> searcher_t;
    typedef balance<binary_tree<int>, balancers::treap> treap_t;

    //searcher_t my_searcher;
    treap_t my_balance;
    //tree_t my_tree;

    //create_test_data_searcher(my_searcher);
    //create_test_data_sequence(my_balancer);
    //create_test_data_sequence(my_vector);

//    Segfaults:
//    BOOST_CHECK(std::equal(my_balancer.begin(), my_balancer.end(), my_vector.begin()));

    //TODO: More tests?
    for (treap_t::iterator ci = my_balance.begin(); ci != my_balance.end(); ++ci) {
        treap_t::hierarchy_type::cursor c = ci.base().base().base();
//        int priority = c->metadata().get_priority(); // FIXME: Segfaults!
//        if (!c.is_leaf()) {
//            BOOST_CHECK(priority
//                      > c.begin()->metadata().get_priority());
//        }
    }

    //treap_t::iterator ci = my_balancer.begin();
    //treap_t::hierarchy_type::cursor c;
    //c = ci.base().base();
    //c = c.parent();
    //treap_t::metadata_type m = c->metadata();
    //int i = ci.base().base().parent()->metadata().get_priority();//m.get_priority();
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/core.hpp>

#include <boost/mpl/bool.hpp>
#include <boost/mpl/char.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/long.hpp>
#include <boost/mpl/size_t.hpp>
#include <type_traits>
using namespace boost::hana;

static_assert(std::is_same<
    datatype_t<boost::mpl::bool_<true>>,
    MplIntegralConstant
>::value, "");

static_assert(std::is_same<
    datatype_t<boost::mpl::int_<0>>,
    MplIntegralConstant
>::value, "");

static_assert(std::is_same<
    datatype_t<boost::mpl::long_<0>>,
    MplIntegralConstant
>::value, "");

static_assert(std::is_same<
    datatype_t<boost::mpl::size_t<0>>,
    MplIntegralConstant
>::value, "");

static_assert(std::is_same<
    datatype_t<boost::mpl::integral_c<int, 0>>,
    MplIntegralConstant
>::value, "");

static_assert(std::is_same<
    datatype_t<boost::mpl::char_<0>>,
    MplIntegralConstant
>::value, "");

int main() { }


#pragma once

#include <tudocomp/Algorithm.hpp>
#include <tudocomp/ds/TextDS.hpp>
#include <tudocomp/ds/ArrayMaxHeap.hpp>

#include <tudocomp/compressors/lzss/LZSSFactors.hpp>
#include <boost/heap/pairing_heap.hpp>

#include <tudocomp_stat/StatPhase.hpp>

namespace tdc {
namespace lcpcomp {

/// Implements the original "Max LCP" selection strategy for LCPComp.
///
/// This strategy constructs a deque containing suffix array entries sorted
/// by their LCP length. The entry with the maximum LCP is selected and
/// overlapping suffices are removed from the deque.
///
/// This was the original naive approach in "Textkompression mithilfe von
/// Enhanced Suffix Arrays" (BA thesis, Patrick Dinklage, 2015).
class BoostHeap : public Algorithm {
public:
    inline static Meta meta() {
        Meta m("lcpcomp_comp", "bheap", "boost heaps");
        return m;
    }

    inline static ds::dsflags_t textds_flags() {
        return ds::SA | ds::ISA | ds::LCP;
    }

    using Algorithm::Algorithm; //import constructor

    template<typename text_t>
    inline void factorize(text_t& text, const size_t threshold, lzss::FactorBuffer& factors) {

		// Construct SA, ISA and LCP
        StatPhase::wrap("Construct text ds", [&]{
            text.require(text_t::SA | text_t::ISA | text_t::LCP);
        });

        auto& sa = text.require_sa();
        auto& isa = text.require_isa();

        text.require_lcp();
        auto lcp = text.release_lcp();

		struct LCPCompare {
			using lcp_t = decltype(lcp);
			using sa_t = decltype(sa); //`text_t::isa_type;
			lcp_t& m_lcp;
			sa_t& m_sa;
			LCPCompare(lcp_t& lcp_, sa_t& sa_) : m_lcp(lcp_), m_sa(sa_) {}

			bool operator()(const len_t i, const len_t j) const {
				if(m_lcp[i] == m_lcp[j]) return m_sa[i] > m_sa[j];
				return m_lcp[i] < m_lcp[j];
			}

		};

		LCPCompare comp(lcp,sa);

        StatPhase phase("Construct MaxLCPHeap");

		boost::heap::pairing_heap<len_compact_t,boost::heap::compare<LCPCompare>> heap(comp);
		std::vector<typename decltype(heap)::handle_type> handles(lcp.size());

		handles[0].node_ = nullptr;
        for(size_t i = 1; i < lcp.size(); ++i) {
            if(lcp[i] >= threshold) handles[i] = heap.emplace(i);
			else handles[i].node_ = nullptr;
        }

        //Factorize
        phase.split("Process MaxLCPHeap");

        while(heap.size() > 0) {
            //get suffix with longest LCP
            const len_compact_t& m = heap.top();

            //generate factor
            const len_t fpos = sa[m];
            const len_t fsrc = sa[m-1];
            const len_t flen = lcp[m];

            factors.emplace_back(fpos, fsrc, flen);

            //remove overlapped entries
            for(size_t k = 0; k < flen; k++) {
                const len_t pos = isa[fpos + k];
			    if(handles[pos].node_ == nullptr) continue;
                heap.erase(handles[pos]);
			    handles[pos].node_ = nullptr;
            }

            //correct intersecting entries
            for(size_t k = 0; k < flen && fpos > k; k++) {
                size_t s = fpos - k - 1;
                size_t i = isa[s];
			    if(handles[i].node_ != nullptr) {
                    if(s + lcp[i] > fpos) {
                        size_t l = fpos - s;
                        if(l >= threshold) {
						    lcp[i] = l;
						    heap.decrease(handles[i]);
                        } else {
						    heap.erase(handles[i]);
						    handles[i].node_ = nullptr;
                        }
                    }
                }
            }
        }
    }
};

}}


/**
 * @file tests/main_tests/kmeans_test.cpp
 * @author Prabhat Sharma
 *
 * Test mlpackMain() of kmeans_main.cpp
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <string>

#define BINDING_TYPE BINDING_TYPE_TEST
static const std::string testName = "Kmeans";

#include <mlpack/core.hpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"
#include <mlpack/methods/kmeans/kmeans_main.cpp>

#include <boost/test/unit_test.hpp>
#include "../test_tools.hpp"

using namespace mlpack;

struct KmTestFixture
{
 public:
  KmTestFixture()
  {
      // Cache in the options for this program.
      CLI::RestoreSettings(testName);
  }

  ~KmTestFixture()
  {
      // Clear the settings.
      CLI::ClearSettings();
  }
};

void ResetKmSettings()
{
  CLI::ClearSettings();
  CLI::RestoreSettings(testName);
}

BOOST_FIXTURE_TEST_SUITE(KmeansMainTest, KmTestFixture);

/**
 * Checking that number of Clusters are non negative
 */
BOOST_AUTO_TEST_CASE(NonNegativeClustersTest)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  SetInputParam("input", std::move(inputData));
  SetInputParam("clusters", (int) -1); // Invalid

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Checking that initial centroids are provided if clusters are to be auto detected
 */
BOOST_AUTO_TEST_CASE(AutoDetectClusterTest)
{
  constexpr int N = 10;
  constexpr int D = 4;

  arma::mat inputData = arma::randu<arma::mat>(N, D);

  SetInputParam("input", std::move(inputData));
  SetInputParam("clusters", (int) 0); // Invalid

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Checking that percentage is between 0 and 1 when --refined_start is specified
*/
BOOST_AUTO_TEST_CASE(RefinedStartPercentageTest)
{
  int c = 2;
  double P = 2.0;
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  SetInputParam("input", std::move(inputData));
  SetInputParam("refined_start", true);
  SetInputParam("clusters", c);
  SetInputParam("percentage", std::move(P));     // Invalid

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Checking percentage is non-negative when --refined_start is specified
 */
BOOST_AUTO_TEST_CASE(NonNegativePercentageTest)
{
  int c = 2;
  double P = -1.0;
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  SetInputParam("input", std::move(inputData));
  SetInputParam("refined_start", true);
  SetInputParam("clusters", c);
  SetInputParam("percentage", P);     // Invalid

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Checking that size and dimensionality of prediction is correct.
 */
BOOST_AUTO_TEST_CASE(KmClusteringSizeCheck)
{
  int c = 2;
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  size_t col = inputData.n_cols;
  size_t row = inputData.n_rows;

  SetInputParam("input", std::move(inputData));
  SetInputParam("clusters", c);

  mlpackMain();

  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("output").n_rows, row+1);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("output").n_cols, col);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("centroid").n_rows, row);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("centroid").n_cols, c);
}

/**
 * Checking that size and dimensionality of prediction is correct when --labels_only is specified
 */
BOOST_AUTO_TEST_CASE(KmClusteringSizeCheckLabelOnly)
{
  int c = 2;

  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");
  size_t col = inputData.n_cols;
  size_t row = inputData.n_rows;

  SetInputParam("input", std::move(inputData));
  SetInputParam("clusters", c);
  SetInputParam("labels_only", true);

  mlpackMain();

  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("output").n_rows, 1);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("output").n_cols, col);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("centroid").n_rows, row);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("centroid").n_cols, c);
}

/**
 * Checking that predictions are not same when --allow_empty_clusters or kill_empty_clusters are specified
 */
BOOST_AUTO_TEST_CASE(KmClusteringEmptyClustersCheck)
{
  int c = 400;
  int iterations = 100;

  arma::mat inputData;
  if (!data::Load("test_data_3_1000.csv", inputData))
    BOOST_FAIL("Unable to load train dataset test_data_3_1000.csv!");
  arma::mat initCentroid = arma::randu<arma::mat>(inputData.n_rows, c);

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("labels_only", true);
  SetInputParam("max_iterations", iterations);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat normalOutput;
  normalOutput = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("labels_only", true);
  SetInputParam("allow_empty_clusters", true);
  SetInputParam("max_iterations", iterations);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat allowEmptyOutput;
  allowEmptyOutput = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("labels_only", true);
  SetInputParam("kill_empty_clusters", true);
  SetInputParam("max_iterations", iterations);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat killEmptyOutput;
  killEmptyOutput = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  if (killEmptyOutput.n_elem == allowEmptyOutput.n_elem)
  {
    BOOST_REQUIRE_GT(arma::accu(killEmptyOutput != allowEmptyOutput), 1);
    BOOST_REQUIRE_GT(arma::accu(killEmptyOutput != normalOutput), 1);
  }
  BOOST_REQUIRE_GT(arma::accu(normalOutput != allowEmptyOutput), 1);
}

/**
 * Checking that that size and dimensionality of Final Input File is correct
 * when flag --in_place is specified
 */
BOOST_AUTO_TEST_CASE(KmClusteringResultSizeCheck)
{
  int c = 2;

  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  size_t row = inputData.n_rows;
  size_t col = inputData.n_cols;

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("in_place", true);

  mlpackMain();
  arma::mat processedInput = CLI::GetParam<arma::mat>("output");
  // here input is actually accessed through output
  // due to a little trick in kmeans_main

  BOOST_REQUIRE_EQUAL(processedInput.n_cols, col);
  BOOST_REQUIRE_EQUAL(processedInput.n_rows, row+1);
}

/**
 * Ensuring that absence of Number of Clusters is checked.
 */
BOOST_AUTO_TEST_CASE(KmClustersNotDefined)
{
  arma::mat inputData;
  if (!data::Load("vc2.csv", inputData))
    BOOST_FAIL("Unable to load train dataset vc2.csv!");

  SetInputParam("input", std::move(inputData));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Checking that all the algorithms yield same results
 */
BOOST_AUTO_TEST_CASE(AlgorithmsSimilarTest)
{
  int c = 5;
  arma::mat inputData(10, 1000);
  inputData.randu();

  arma::mat initCentroids(10, 5);
  initCentroids.randu();

  arma::mat initCentroid = arma::randu<arma::mat>(inputData.n_rows, c);
  std::string algo = "naive";

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("algorithm", std::move(algo));
  SetInputParam("labels_only", true);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat naiveOutput;
  arma::mat naiveCentroid;
  naiveOutput = std::move(CLI::GetParam<arma::mat>("output"));
  naiveCentroid = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  algo = "elkan";

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("algorithm", std::move(algo));
  SetInputParam("labels_only", true);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat elkanOutput;
  arma::mat elkanCentroid;
  elkanOutput = std::move(CLI::GetParam<arma::mat>("output"));
  elkanCentroid = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  algo = "hamerly";

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("algorithm", std::move(algo));
  SetInputParam("labels_only", true);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat hamerlyOutput;
  arma::mat hamerlyCentroid;
  hamerlyOutput = std::move(CLI::GetParam<arma::mat>("output"));
  hamerlyCentroid = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  algo = "dualtree";

  SetInputParam("input", inputData);
  SetInputParam("clusters", c);
  SetInputParam("algorithm", std::move(algo));
  SetInputParam("labels_only", true);
  SetInputParam("initial_centroids", initCentroid);

  mlpackMain();

  arma::mat dualTreeOutput;
  arma::mat dualTreeCentroid;
  dualTreeOutput = std::move(CLI::GetParam<arma::mat>("output"));
  dualTreeCentroid = std::move(CLI::GetParam<arma::mat>("centroid"));

  ResetKmSettings();

  algo = "dualtree-covertree";

  SetInputParam("input", std::move(inputData));
  SetInputParam("clusters", c);
  SetInputParam("algorithm", std::move(algo));
  SetInputParam("labels_only", true);
  SetInputParam("initial_centroids", std::move(initCentroid));

  mlpackMain();

  arma::mat dualCoverTreeOutput;
  arma::mat dualCoverTreeCentroid;
  dualCoverTreeOutput = std::move(CLI::GetParam<arma::mat>("output"));
  dualCoverTreeCentroid = std::move(CLI::GetParam<arma::mat>("centroid"));

  // Checking all the algorithms return same assignments
  CheckMatrices(naiveOutput, hamerlyOutput);
  CheckMatrices(naiveOutput, elkanOutput);
  CheckMatrices(naiveOutput, dualTreeOutput);
  CheckMatrices(naiveOutput, dualCoverTreeOutput);

  // Checking all the algorithms return almost same centroid
  CheckMatrices(naiveCentroid, hamerlyCentroid);
  CheckMatrices(naiveCentroid, elkanCentroid);
  CheckMatrices(naiveCentroid, dualTreeCentroid);
  CheckMatrices(naiveCentroid, dualCoverTreeCentroid);
}

BOOST_AUTO_TEST_SUITE_END();


/* btcStructs.cxx */
/* ====================================================================
 * INCLUDE ALL BITCOIN RELATED CLASSES FOR THIS PROJECT.
 * DO NOT USE DIRECTLY; ONLY USE BY INCLUDING.
 * ====================================================================
 *
 * CLASSES EXPLAINED:
 *
 * #1 UTXO:
 *    A UTXO object will store all information about an Unspent Transaction
 *    Output which can be used as an input(source of funds) for a new
 *    transaction. The class has various member functions to automatically
 *    generate serialized data parts related to inputs inside the raw
 *    transaction data recorded on the Blockchain.
 *
 * #
 *
 *
 */

#include "btcStructs.h"
#include "btcTools.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <openssl/bn.h>
#include <openssl/obj_mac.h>
#include <openssl/ec.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <sstream>
#include <cmath>

using namespace std;
using namespace boost::multiprecision;

typedef vector<unsigned char>::iterator vbit;

UTXO::UTXO(string id, string add, string sp, int ix, double amt) {

	txid = id;
	address = add;
	spendable = sp;
	index = ix;
	amount = amt * 100000000;

	}

bool UTXO::good() {

	if (spendable == "YES")
	{return true;}
	else {return false;}

	}

double UTXO::get_balance() {

	double balance = amount;
	balance = balance / 100000000;
	return balance;

	}

void UTXO::print_info() {

	cout << "\nTXID: " << txid << "\nIndex: " << index;
	cout << "\nPay to Address: " << address;
	cout << "\nAmount: " << amount << " Satoshis";
	cout << "\nSpendable: " << spendable << "\n" << endl;

	}

void UTXO::export_to_file(ofstream& t_ufile) {

	t_ufile << txid << " " << address << " " << spendable << " ";
	double dmt = amount;
	dmt /= 100000000;
	t_ufile << index << " " << dmt << "\n";

	}

void UTXO::pkhash_decode(unsigned char* pkhout) {

	const string decoder = "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";
	size_t adlen = address.length();
	uint256_t bigN, chunk, cbyte;
	int value;
	for (size_t i = 0; i < adlen; ++i) {
		value = decoder.find(address[i]);
		chunk = 58;
		chunk = pow(chunk, adlen - 1 - i) * value;
		bigN += chunk;
		}

	unsigned char ibyte;
	for (int j = 0; j < 20; ++j) {
		cbyte = (bigN >> (23 - j) * 8) & 0xFF;
		while (cbyte > 0) {++ibyte; --cbyte;}
		pkhout[j] = ibyte;
		ibyte = 0;
		}

	}

void UTXO::fetch_private(vector<string>& t_wif_set, unsigned char* keyout) {

	uint256_t real_key, chunk;

	for (vector<string>::iterator si = t_wif_set.begin(); si != t_wif_set.end(); ++si) {
		if (wif_comp_to_pkh(*si) == address) {
			real_key = wif_to_priv(*si);
			break;
			}
		}

	unsigned char ibyte;
	for (int i = 0; i < 32; ++i) {
		chunk = (real_key >> ((31 - i) * 8)) & 0xFF;
		while (chunk > 0) {++ibyte; --chunk;}
		keyout[i] = ibyte;
		ibyte = 0;
		}

	}

void UTXO::bc_partial_serial(unsigned char* icout) {
	// partial size = 32 + 4 = 36.

	uint64_t id1, id2, id3, id4;
	string ids1 = txid.substr(0, 16);
	string ids2 = txid.substr(16, 16);
	string ids3 = txid.substr(32, 16);
	string ids4 = txid.substr(48, 16);
	stringstream(ids1) >> hex >> id1;
	stringstream(ids2) >> hex >> id2;
	stringstream(ids3) >> hex >> id3;
	stringstream(ids4) >> hex >> id4;
	for (int i = 0; i < 8; ++i) {
		icout[i] = (id4 >> i * 8) & 0xFF;
		icout[i + 8] = (id3 >> i * 8) & 0xFF;
		icout[i + 16] = (id2 >> i * 8) & 0xFF;
		icout[i + 24] = (id1 >> i * 8) & 0xFF;
		}

	uint64_t ix = index;
	for (int i = 0; i < 4; ++i) {icout[i + 32] = (ix >> i * 8) & 0xFF;}

	}

void UTXO::ac_partial_serial(unsigned char* t_secret, unsigned char* acout) {
	// partial size = 1 + 33(1 + 32) + 4 = 38.

	unsigned char* pubkey = new unsigned char[33];

	EC_GROUP* ecgroup = EC_GROUP_new_by_curve_name(NID_secp256k1);
	EC_POINT* ecpoint = EC_POINT_new(ecgroup);
	BN_CTX* bnctx = BN_CTX_new();
	BIGNUM* priv_bn = BN_bin2bn(t_secret, 32, NULL);
	EC_POINT_mul(ecgroup, ecpoint, priv_bn, NULL, NULL, bnctx);
	EC_POINT_point2oct(ecgroup, ecpoint, POINT_CONVERSION_COMPRESSED,
					   pubkey, 33, bnctx);

	acout[0] = 0x21;
	for (int i = 0; i < 33; ++i)
	{acout[1 + i] = pubkey[i];}
	acout[34] = 0xFF;
	acout[35] = 0xFF;
	acout[36] = 0xFF;
	acout[37] = 0xFF;

	}

void UTXO::input_serial(unsigned char* sout) {
	// input size = 32 + 4 + 1 + 25 + 4 = 66.

	uint64_t id1, id2, id3, id4;
	string ids1 = txid.substr(0, 16);
	string ids2 = txid.substr(16, 16);
	string ids3 = txid.substr(32, 16);
	string ids4 = txid.substr(48, 16);
	stringstream(ids1) >> hex >> id1;
	stringstream(ids2) >> hex >> id2;
	stringstream(ids3) >> hex >> id3;
	stringstream(ids4) >> hex >> id4;
	for (int i = 0; i < 8; ++i) {
		sout[i] = (id4 >> i * 8) & 0xFF;
		sout[i + 8] = (id3 >> i * 8) & 0xFF;
		sout[i + 16] = (id2 >> i * 8) & 0xFF;
		sout[i + 24] = (id1 >> i * 8) & 0xFF;
		}

	uint64_t ix = index;
	for (int i = 0; i < 4; ++i) {sout[i + 32] = (ix >> i * 8) & 0xFF;}

	sout[36] = 0x19;
	sout[37] = 0x76;
	sout[38] = 0xa9;
	sout[39] = 0x14;
	unsigned char* pkh = new unsigned char[20];
	this->pkhash_decode(pkh);
	for (int i = 0; i < 20; ++i) {sout[i + 40] = pkh[i];}

	sout[60] = 0x88;
	sout[61] = 0xac;
	sout[62] = 0xff;
	sout[63] = 0xff;
	sout[64] = 0xff;
	sout[65] = 0xff;

	}