#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_


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Ediis.h"
#include "EdiisCoefficientOptimizer.h"
#include <Eigen/QR>
#include <iostream>

namespace Scine {
namespace Utils {

Ediis::Ediis() {
  setSubspaceSize(6);
}

void Ediis::setUnrestricted(bool b) {
  unrestricted_ = b;
}

void Ediis::setSubspaceSize(int n) {
  bool resizeNeeded = n != subspaceSize_;

  subspaceSize_ = n;

  if (resizeNeeded)
    resizeMembers();
}

void Ediis::setNAOs(int n) {
  bool resizeNeeded = n != nAOs_;

  nAOs_ = n;

  if (resizeNeeded)
    resizeMembers();
}

void Ediis::resizeMembers() {
  fockMatrices.resize(subspaceSize_);
  densityMatrices.resize(subspaceSize_);
  energies.resize(subspaceSize_);

  B = Eigen::MatrixXd::Zero(subspaceSize_, subspaceSize_);

  restart();
}

void Ediis::restart() {
  iterationNo_ = 0;
  index_ = 0;
}

void Ediis::addMatrices(double energy, const SpinAdaptedMatrix& F, const DensityMatrix& P) {
  iterationNo_++;
  lastAdded_ = index_;

  fockMatrices[index_] = F;
  densityMatrices[index_] = P;
  energies[index_] = energy;

  updateBMatrix();

  index_ = (index_ + 1) % subspaceSize_;
}

void Ediis::updateBMatrix() {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;

  // Bii element
  B(lastAdded_, lastAdded_) = 0;

  // Bij elements
  for (int i = 0; i < activeSize; i++) {
    if (i == lastAdded_)
      continue;
    double v = getBMatrixElement(lastAdded_, i);
    B(lastAdded_, i) = v;
    B(i, lastAdded_) = B(lastAdded_, i);
  }
}

double Ediis::getBMatrixElement(int i, int j) const {
  if (unrestricted_) {
    double va = ((fockMatrices[i].alphaMatrix() - fockMatrices[j].alphaMatrix()).selfadjointView<Eigen::Lower>() *
                 (densityMatrices[i].alphaMatrix() - densityMatrices[j].alphaMatrix()))
                    .trace();
    double vb = ((fockMatrices[i].betaMatrix() - fockMatrices[j].betaMatrix()).selfadjointView<Eigen::Lower>() *
                 (densityMatrices[i].betaMatrix() - densityMatrices[j].betaMatrix()))
                    .trace();
    return (va + vb) / 2;
  }
  else {
    double v = ((fockMatrices[i].restrictedMatrix() - fockMatrices[j].restrictedMatrix()).selfadjointView<Eigen::Lower>() *
                (densityMatrices[i].restrictedMatrix() - densityMatrices[j].restrictedMatrix()))
                   .trace();
    // Divide v by two because density matrix is RHF
    return v / 2;
  }
}

SpinAdaptedMatrix Ediis::getMixedFockMatrix() {
  if (iterationNo_ > subspaceSize_)
    iterationNo_ = subspaceSize_;

  // If we have only one Fock matrix
  if (iterationNo_ < 2) {
    return fockMatrices[0];
  }
  else {
    EdiisCoefficientOptimizer opt(energies, B.block(0, 0, iterationNo_, iterationNo_));
    auto coefs = opt.getCoefficients();

    return calculateLinearCombination(coefs);
  }
}

SpinAdaptedMatrix Ediis::calculateLinearCombination(const Eigen::VectorXd& coefs) {
  if (unrestricted_) {
    Eigen::MatrixXd FAlpha = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    Eigen::MatrixXd FBeta = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    for (int i = 0; i < iterationNo_; i++) {
      FAlpha += coefs[i] * fockMatrices[i].alphaMatrix();
      FBeta += coefs[i] * fockMatrices[i].betaMatrix();
    }
    return SpinAdaptedMatrix::createUnrestricted(std::move(FAlpha), std::move(FBeta));
  }
  else {
    Eigen::MatrixXd Fsol = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    for (int i = 0; i < iterationNo_; i++)
      Fsol += coefs[i] * fockMatrices[i].restrictedMatrix();
    return SpinAdaptedMatrix::createRestricted(std::move(Fsol));
  }
}
} // namespace Utils
} // namespace Scine


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef GRAPH_CONFIGURATION_HPP
#define GRAPH_CONFIGURATION_HPP

#include <boost/graph/adjacency_list.hpp>
#include "configuration.hpp"
#include "rjmcmc/util/variant.hpp" // apply_visitor

namespace marked_point_process {

    template<typename T, typename UnaryEnergy, typename BinaryEnergy, typename Accelerator=trivial_accelerator, typename OutEdgeList = boost::listS, typename VertexList = boost::listS  >
    class graph_configuration
    {
    public:
	typedef graph_configuration<T,UnaryEnergy, BinaryEnergy, Accelerator, OutEdgeList, VertexList> self;
        typedef T	value_type;
    private:
	class edge {
	public:
            edge() : m_energy(0) {}
            inline double energy() const { return m_energy; }
            inline void energy(double e) { m_energy = e;    }

	private:
            double m_energy;
	};

	class node {
	public:
            node(const value_type& obj, double e) : m_value(obj), m_energy(e) { }
            inline const value_type& value() const { return m_value; }
            inline double energy() const { return m_energy; }

	private:
            value_type	m_value;
            double	m_energy;
	};
	typedef boost::adjacency_list<OutEdgeList, VertexList, boost::undirectedS, node, edge> graph_type;
	typedef typename graph_type::out_edge_iterator	out_edge_iterator;
	typedef	typename graph_type::vertex_descriptor	vertex_descriptor;
	typedef std::pair< typename graph_type::edge_descriptor , bool > edge_descriptor_bool;

    public:
	typedef	typename graph_type::vertex_iterator	iterator;
	typedef	typename graph_type::vertex_iterator	const_iterator;
	typedef typename graph_type::edge_iterator	edge_iterator;
	typedef typename graph_type::edge_iterator	const_edge_iterator;
        typedef internal::modification<self>            modification;
    public:

	// configuration constructors/destructors
	graph_configuration(UnaryEnergy unary_energy, BinaryEnergy binary_energy, Accelerator accelerator=Accelerator()) : m_unary(0.), m_binary(0.), m_unary_energy(unary_energy), m_binary_energy(binary_energy), m_accelerator(accelerator)
	{}
	~graph_configuration()
	{}

	// configuration accessors
	inline double unary_energy () const { return m_unary;}
	inline double binary_energy() const { return m_binary;}
	inline double energy       () const {
            return unary_energy()+binary_energy();
	}

	// values
	inline size_t size() const { return num_vertices(m_graph); }
	inline bool empty() const {	return (num_vertices(m_graph)==0); }
	inline iterator begin() { return vertices(m_graph).first; }
	inline iterator end  () { return vertices(m_graph).second; }
	inline const_iterator begin() const { return vertices(m_graph).first; }
        inline const_iterator end  () const { return vertices(m_graph).second; }
	inline const value_type& value( const_iterator v ) const { return m_graph[ *v ].value(); }
	inline double energy( const_iterator v ) const { return m_graph[ *v ].energy(); }

	// interactions
	inline size_t size_of_interactions   () const { return num_edges(m_graph);    }
	inline edge_iterator interactions_begin() { return edges(m_graph).first; }
	inline edge_iterator interactions_end  () { return edges(m_graph).second; }
	inline const_edge_iterator interactions_begin() const { return edges(m_graph).first; }
	inline const_edge_iterator interactions_end  () const { return edges(m_graph).second; }
	inline double energy( edge_iterator e ) const { return m_graph[ *e ].energy(); }

	// evaluators

	template <typename Modification> double delta_energy(const Modification &modif) const
	{
            return delta_birth(modif)+delta_death(modif);
	}

	template <typename Modification> double delta_birth(const Modification &modif) const
	{
            double delta = 0;
            typedef typename Modification::birth_type::const_iterator bci;
            typedef typename Modification::death_type::const_iterator dci;
            bci bbeg = modif.birth().begin();
            bci bend = modif.birth().end();
            dci dbeg = modif.death().begin();
            dci dend = modif.death().end();
            for(bci it=bbeg; it!=bend; ++it) {
                delta += rjmcmc::apply_visitor(m_unary_energy,*it);
                const_iterator   it2, end2;
                boost::tie(it2,end2)=m_accelerator(*this,*it);
                for (; it2 != end2; ++it2)
                    if (std::find(dbeg,dend,it2)==dend)
                        delta += rjmcmc::apply_visitor(m_binary_energy, *it, value(it2) );
                for (bci it2=bbeg; it2 != it; ++it2)
                    delta += rjmcmc::apply_visitor(m_binary_energy, *it, *it2);
            }
            return delta;
	}

	template <typename Modification> double delta_death(const Modification &modif) const
	{
            double delta = 0;
            typedef typename Modification::death_type::const_iterator dci;
            dci dbeg = modif.death().begin();
            dci dend = modif.death().end();
            for(dci it=dbeg; it!=dend; ++it) {
                iterator v = *it;
                delta -= energy(v);
                out_edge_iterator it2, end;
                for(boost::tie(it2,end) = out_edges( *v, m_graph ); it2!=end; ++it2) {
                    vertex_descriptor dtarget = target(*it2, m_graph);
                    bool found = false;
                    for(dci it3=dbeg; it3!=it && !found; ++it3)
                        found = (**it3 == dtarget);
                    if (!found)
                        delta -= m_graph[ *it2 ].energy();
                }
            }
            return delta;
        }

	// manipulators
	void insert(const value_type& obj)
	{
            node n(obj, rjmcmc::apply_visitor(m_unary_energy,obj));
            m_unary += n.energy();
            vertex_descriptor d = add_vertex(n, m_graph);
            iterator   it, end;

            for (boost::tie(it,end)=m_accelerator(*this,obj); it != end; ++it) {
                if ( *it == d ) continue;
                double e = rjmcmc::apply_visitor(m_binary_energy, obj, value(it) );
                if (   e == 0 ) continue;
                edge_descriptor_bool new_edge = add_edge(d, *it, m_graph );
                m_graph[ new_edge.first ].energy( e );
                m_binary += e;
            }
	}

        template<typename F>
        struct DerefAdapter
        {
            graph_type const & g_;
            F f_;
            DerefAdapter(graph_type const & g, F f) : g_(g), f_(f) {}
            template<typename U> typename F::result_type operator()(U u) { return rjmcmc::apply_visitor(f_,g_[u].value()); }
        };

        template<typename F> inline void for_each(F f)       {
            iterator it, end;
            boost::tie(it,end) = vertices(m_graph);
            DerefAdapter<F> da(m_graph,f);
            std::for_each(it,end,da);
        }
        template<typename F> inline void for_each(F f) const {
            iterator it, end;
            boost::tie(it,end) = vertices(m_graph);
            DerefAdapter<F> da(m_graph,f);
            std::for_each(it,end,da);
        }

	void remove( iterator v )
	{
            out_edge_iterator it, end;
            for(boost::tie(it,end) = out_edges( *v, m_graph ); it!=end; ++it)
                m_binary -= m_graph[ *it ].energy();
            m_unary -= m_graph[*v].energy();
            clear_vertex ( *v , m_graph);
            remove_vertex( *v , m_graph);
	}

	inline void clear() { m_graph.clear(); m_unary=m_binary=0; }

	// audit
	double audit_unary_energy() const
	{
            double e = 0.;
            for (const_iterator i=begin(); i != end(); ++i)
                e += rjmcmc::apply_visitor(m_unary_energy, value(i) );
            return e;
	}

	double audit_binary_energy() const
	{
            double e = 0.;
            const_edge_iterator it, end;
            for(boost::tie(it,end) = edges( m_graph ); it!=end; ++it)
                e += rjmcmc::apply_visitor(m_binary_energy,	m_graph[source(*it,m_graph)].value() ,
                                           m_graph[target(*it,m_graph)].value() );
            return e;
	}

	unsigned int audit_structure() const
	{
            unsigned int err = 0;
            for (const_iterator i=begin(); i != end(); ++i)
            {
                const_iterator j = i;
                for (++j; j != end(); ++j)
                {
                    bool computed = (0!= rjmcmc::apply_visitor(m_binary_energy,value(i), value(j)));
                    bool stored = boost::edge(*i, *j, m_graph).second;
                    if (computed != stored)	++err;
                }
            }
            return err;
	}

    private:
        double m_unary;
        double m_binary;
	graph_type m_graph;
	UnaryEnergy	m_unary_energy;
	BinaryEnergy	m_binary_energy;
        Accelerator	m_accelerator;
    };

}; // namespace marked_point_process

#endif // GRAPH_CONFIGURATION_HPP


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#include "camera.h"

#include "transforms.h"
#include "util.h"

#include <Eigen/Geometry>
#include <Eigen/LU>
#include <cmath>
#include <iostream>
#include <limits>

namespace scenepic
{
  void check_valid_rotation(const Transform& rotation, float tolerance = 1e-6f)
  {
    auto ortho_diff = (rotation * rotation.transpose() - Transform::Identity())
                        .array()
                        .abs()
                        .maxCoeff();
    auto det_diff = std::abs(rotation.determinant() - 1.0f);
    if (ortho_diff > tolerance)
    {
      std::cerr << "camera rotation: not orthogonal within tolerance of "
                << tolerance << ": " << ortho_diff << std::endl;
    }

    if (det_diff > tolerance)
    {
      std::cerr << "camera rotation: |det " << rotation.determinant()
                << " - 1| > " << tolerance << std::endl;
    }
  }

  Camera::Camera(
    const Vector& center,
    const Vector& look_at,
    const Vector& up_dir,
    double fov_y_degrees,
    double near_crop_distance,
    double far_crop_distance,
    double aspect_ratio)
  {
    auto rotation = Transforms::look_at_rotation(center, look_at, up_dir);
    auto translation = Transforms::translate(-center);
    m_world_to_camera = rotation * translation;
    m_camera_to_world = m_world_to_camera.inverse();
    m_projection = Transforms::gl_projection(
      fov_y_degrees, aspect_ratio, near_crop_distance, far_crop_distance);
  }

  Camera::Camera(
    const Vector& center,
    const Transform& rotation,
    double fov_y_degrees,
    double near_crop_distance,
    double far_crop_distance,
    double aspect_ratio)
  {
    check_valid_rotation(rotation);
    auto translation = Transforms::translate(-center);
    m_world_to_camera = rotation * translation;
    m_camera_to_world = m_world_to_camera.inverse();
    m_projection = Transforms::gl_projection(
      fov_y_degrees, aspect_ratio, near_crop_distance, far_crop_distance);
  }

  Camera::Camera(
    const Transform& world_to_camera,
    double fov_y_degrees,
    double near_crop_distance,
    double far_crop_distance,
    double aspect_ratio)
  : m_world_to_camera(world_to_camera)
  {
    if (!m_world_to_camera.isZero())
    {
      check_valid_rotation(this->rotation());
      m_camera_to_world = m_world_to_camera.inverse();
    }

    m_projection = Transforms::gl_projection(
      fov_y_degrees, aspect_ratio, near_crop_distance, far_crop_distance);
  }

  Camera::Camera(const Transform& world_to_camera, const Transform& projection)
  : m_world_to_camera(world_to_camera), m_projection(projection)
  {
    if (!m_world_to_camera.isZero())
    {
      check_valid_rotation(this->rotation());
      m_camera_to_world = m_world_to_camera.inverse();
    }
  }

  Vector Camera::center() const
  {
    Vector center = m_camera_to_world.topRightCorner(3, 1).transpose();
    return center;
  }

  Vector Camera::look_at() const
  {
    Vector look_at(0, 0, -1);
    look_at =
      (look_at.homogeneous() * m_camera_to_world.transpose()).hnormalized();
    return look_at;
  }

  Vector Camera::up_dir() const
  {
    Vector up_dir(0, 1, 0);
    up_dir = (up_dir.homogeneous() * this->rotation()).hnormalized();
    return up_dir;
  }

  Transform Camera::rotation() const
  {
    Transform rotation = m_world_to_camera;
    rotation.topRightCorner(3, 1).fill(0);
    return rotation;
  }

  JsonValue Camera::to_json() const
  {
    JsonValue obj;
    obj["CommandType"] = "SetCamera";
    obj["Value"]["WorldToCamera"] = matrix_to_json(m_world_to_camera);
    obj["Value"]["Projection"] = matrix_to_json(m_projection);
    return obj;
  }

  bool Camera::is_none() const
  {
    return m_projection.isZero() && m_world_to_camera.isZero();
  }

  Camera Camera::None()
  {
    Transform world_to_camera = Transform::Zero();
    Transform projection = Transform::Zero();
    return Camera(world_to_camera, projection);
  }

  std::string Camera::to_string() const
  {
    return this->to_json().to_string();
  }

  const Transform& Camera::world_to_camera() const
  {
    return m_world_to_camera;
  }

  const Transform& Camera::camera_to_world() const
  {
    return m_camera_to_world;
  }

  const Transform& Camera::projection() const
  {
    return m_projection;
  }

  float Camera::aspect_ratio() const
  {
    return m_projection(1, 1) / m_projection(0, 0);
  }

  Camera& Camera::aspect_ratio(float aspect_ratio)
  {
    m_projection(0, 0) = m_projection(1, 1) / aspect_ratio;
    return *this;
  }

  std::vector<Camera> Camera::orbit(
    int num_frames,
    float distance,
    int num_times,
    float min_altitude,
    float max_altitude,
    Vector up_dir,
    Vector forward_dir,
    double fov_y_degrees,
    double aspect_ratio,
    double near_crop_distance,
    double far_crop_distance)
  {
    Vector right_dir = up_dir.cross(forward_dir);
    Eigen::VectorXf azimuth(num_frames);
    Eigen::VectorXf altitude(num_frames);
    azimuth.setLinSpaced(0, num_times * 2 * EIGEN_PI);
    int half_frames = num_frames / 2;
    altitude.topRows(half_frames + 1).setLinSpaced(min_altitude, max_altitude);
    altitude.bottomRows(half_frames).setLinSpaced(min_altitude, max_altitude);
    altitude.bottomRows(half_frames).reverseInPlace();

    Transform projection = Transforms::gl_projection(
      fov_y_degrees, aspect_ratio, near_crop_distance, far_crop_distance);

    Transform init_ext = Transforms::look_at_rotation(
      -forward_dir * distance, Vector::Zero(), up_dir);
    init_ext = Transforms::translate(-forward_dir * distance) * init_ext;
    std::vector<Camera> cameras;
    for (int i = 0; i < num_frames; ++i)
    {
      Transform elevate =
        Transforms::rotation_matrix_from_axis_angle(right_dir, altitude[i]);
      Transform rotate =
        Transforms::rotation_matrix_from_axis_angle(up_dir, azimuth[i]);
      Transform camera_to_world = rotate * elevate * init_ext;
      Transform world_to_camera = camera_to_world.inverse();
      cameras.push_back(Camera(world_to_camera, projection));
    }

    return cameras;
  }

} // namespace scenepic


#include <polyfem/RhsAssembler.hpp>
#include <polyfem/par_for.hpp>

#include <polyfem/BoundarySampler.hpp>
#include <polysolve/LinearSolver.hpp>

#include <polyfem/Logger.hpp>

#include <Eigen/Sparse>

#ifdef POLYFEM_WITH_TBB
#include <tbb/parallel_for.h>
#include <tbb/enumerable_thread_specific.h>
#endif

#include <iostream>
#include <map>
#include <memory>

namespace polyfem
{
	using namespace polysolve;
	namespace
	{
		class LocalThreadScalarStorage
		{
		public:
			double val;
			ElementAssemblyValues vals;

			LocalThreadScalarStorage()
			{
				val = 0;
			}
		};
	} // namespace

	RhsAssembler::RhsAssembler(const AssemblerUtils &assembler, const Mesh &mesh,
							   const int n_basis, const int size,
							   const std::vector<ElementBases> &bases, const std::vector<ElementBases> &gbases, const AssemblyValsCache &ass_vals_cache,
							   const std::string &formulation, const Problem &problem,
							   const std::string bc_method,
							   const std::string &solver, const std::string &preconditioner, const json &solver_params)
		: assembler_(assembler), mesh_(mesh),
		  n_basis_(n_basis), size_(size),
		  bases_(bases), gbases_(gbases), ass_vals_cache_(ass_vals_cache),
		  formulation_(formulation), problem_(problem),
		  bc_method_(bc_method),
		  solver_(solver), preconditioner_(preconditioner), solver_params_(solver_params)
	{
	}

	void RhsAssembler::assemble(const Density &density, Eigen::MatrixXd &rhs, const double t) const
	{
		rhs = Eigen::MatrixXd::Zero(n_basis_ * size_, 1);
		if (!problem_.is_rhs_zero())
		{
			Eigen::MatrixXd rhs_fun;

			const int n_elements = int(bases_.size());
			ElementAssemblyValues vals;
			for (int e = 0; e < n_elements; ++e)
			{
				vals.compute(e, mesh_.is_volume(), bases_[e], gbases_[e]);

				const Quadrature &quadrature = vals.quadrature;

				problem_.rhs(assembler_, formulation_, vals.val, t, rhs_fun);

				for (int d = 0; d < size_; ++d)
				{
					//rhs_fun.col(d) = rhs_fun.col(d).array() * vals.det.array() * quadrature.weights.array();
					for (int q = 0; q < quadrature.weights.size(); ++q)
					{
						const double rho = density(vals.val(q, 0), vals.val(q, 1), vals.val.cols() == 2 ? 0. : vals.val(q, 2), vals.element_id);
						rhs_fun(q, d) *= vals.det(q) * quadrature.weights(q) * rho;
					}
				}

				const int n_loc_bases_ = int(vals.basis_values.size());
				for (int i = 0; i < n_loc_bases_; ++i)
				{
					const AssemblyValues &v = vals.basis_values[i];

					for (int d = 0; d < size_; ++d)
					{
						const double rhs_value = (rhs_fun.col(d).array() * v.val.array()).sum();
						for (std::size_t ii = 0; ii < v.global.size(); ++ii)
							rhs(v.global[ii].index * size_ + d) += rhs_value * v.global[ii].val;
					}
				}
			}
		}
	}

	void RhsAssembler::initial_solution(Eigen::MatrixXd &sol) const
	{
		time_bc([&](const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val)
				{ problem_.initial_solution(mesh, global_ids, pts, val); },
				sol);
	}

	void RhsAssembler::initial_velocity(Eigen::MatrixXd &sol) const
	{
		time_bc([&](const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val)
				{ problem_.initial_velocity(mesh, global_ids, pts, val); },
				sol);
	}

	void RhsAssembler::initial_acceleration(Eigen::MatrixXd &sol) const
	{
		time_bc([&](const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val)
				{ problem_.initial_acceleration(mesh, global_ids, pts, val); },
				sol);
	}

	void RhsAssembler::time_bc(const std::function<void(const Mesh &, const Eigen::MatrixXi &, const Eigen::MatrixXd &, Eigen::MatrixXd &)> &fun, Eigen::MatrixXd &sol) const
	{
		sol = Eigen::MatrixXd::Zero(n_basis_ * size_, 1);
		Eigen::MatrixXd loc_sol;

		const int n_elements = int(bases_.size());
		ElementAssemblyValues vals;
		Eigen::MatrixXi ids;
		for (int e = 0; e < n_elements; ++e)
		{
			vals.compute(e, mesh_.is_volume(), bases_[e], gbases_[e]);
			ids.resize(vals.val.rows(), 1);
			ids.setConstant(e);

			const Quadrature &quadrature = vals.quadrature;
			//problem_.initial_solution(vals.val, loc_sol);
			fun(mesh_, ids, vals.val, loc_sol);

			for (int d = 0; d < size_; ++d)
				loc_sol.col(d) = loc_sol.col(d).array() * vals.det.array() * quadrature.weights.array();

			const int n_loc_bases_ = int(vals.basis_values.size());
			for (int i = 0; i < n_loc_bases_; ++i)
			{
				const AssemblyValues &v = vals.basis_values[i];

				for (int d = 0; d < size_; ++d)
				{
					const double sol_value = (loc_sol.col(d).array() * v.val.array()).sum();
					for (std::size_t ii = 0; ii < v.global.size(); ++ii)
						sol(v.global[ii].index * size_ + d) += sol_value * v.global[ii].val;
				}
			}
		}

		Eigen::MatrixXd b = sol;
		sol.setZero();

		const double mmin = b.minCoeff();
		const double mmax = b.maxCoeff();

		if (fabs(mmin) > 1e-8 || fabs(mmax) > 1e-8)
		{
			StiffnessMatrix mass;
			Density d;
			assembler_.assemble_mass_matrix(formulation_, size_ == 3, n_basis_, d, bases_, gbases_, ass_vals_cache_, mass);
			auto solver = LinearSolver::create(solver_, preconditioner_);
			solver->setParameters(solver_params_);
			solver->analyzePattern(mass, mass.rows());
			solver->factorize(mass);

			for (long i = 0; i < b.cols(); ++i)
			{
				solver->solve(b.col(i), sol.col(i));
			}
			logger().trace("mass matrix error {}", (mass * sol - b).norm());
		}
	}

	void RhsAssembler::lsq_bc(const std::function<void(const Eigen::MatrixXi &, const Eigen::MatrixXd &, const Eigen::MatrixXd &, Eigen::MatrixXd &)> &df,
							  const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, Eigen::MatrixXd &rhs) const
	{
		const int n_el = int(bases_.size());

		Eigen::MatrixXd uv, samples, gtmp, rhs_fun;
		Eigen::VectorXi global_primitive_ids;

		int index = 0;
		std::vector<int> indices;
		indices.reserve(n_el * 10);
		// std::map<int, int> global_index_to_col;

		long total_size = 0;

		Eigen::Matrix<bool, Eigen::Dynamic, 1> is_boundary(n_basis_);
		is_boundary.setConstant(false);
		Eigen::VectorXi global_index_to_col(n_basis_);
		global_index_to_col.setConstant(-1);

		const int actual_dim = problem_.is_scalar() ? 1 : mesh_.dimension();

		// assert((bounday_nodes.size()/actual_dim)*actual_dim == bounday_nodes.size());

		int skipped_count = 0;
		for (int b : bounday_nodes)
		{
			int bindex = b / actual_dim;

			if (bindex < is_boundary.size())
				is_boundary[bindex] = true;
			else
				skipped_count++;
		}
		assert(skipped_count <= 1);

		for (const auto &lb : local_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = sample_boundary(lb, resolution, true, uv, samples, global_primitive_ids);

			if (!has_samples)
				continue;

			const ElementBases &bs = bases_[e];
			const int n_local_bases = int(bs.bases.size());

			total_size += samples.rows();

			for (int j = 0; j < n_local_bases; ++j)
			{
				const Basis &b = bs.bases[j];

				for (std::size_t ii = 0; ii < b.global().size(); ++ii)
				{
					//pt found
					// if(std::find(bounday_nodes.begin(), bounday_nodes.end(), size_ * b.global()[ii].index) != bounday_nodes.end())
					if (is_boundary[b.global()[ii].index])
					{
						// if(!global_index_to_col.contains(b.global()[ii].index))
						if (global_index_to_col(b.global()[ii].index) == -1)
						{
							// global_index_to_col[b.global()[ii].index] = index++;
							global_index_to_col(b.global()[ii].index) = index++;
							indices.push_back(b.global()[ii].index);
							assert(indices.size() == size_t(index));
						}
					}
				}
			}
		}

		// Eigen::MatrixXd global_mat = Eigen::MatrixXd::Zero(total_size, indices.size());
		Eigen::MatrixXd global_rhs = Eigen::MatrixXd::Zero(total_size, size_);

		const long buffer_size = total_size * long(indices.size());
		std::vector<Eigen::Triplet<double>> entries, entries_t;
		// entries.reserve(buffer_size);
		// entries_t.reserve(buffer_size);

		index = 0;

		int global_counter = 0;
		Eigen::MatrixXd mapped;

		std::vector<AssemblyValues> tmp_val;

		for (const auto &lb : local_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = sample_boundary(lb, resolution, false, uv, samples, global_primitive_ids);

			if (!has_samples)
				continue;

			const ElementBases &bs = bases_[e];
			const ElementBases &gbs = gbases_[e];
			const int n_local_bases = int(bs.bases.size());

			gbs.eval_geom_mapping(samples, mapped);

			bs.evaluate_bases(samples, tmp_val);
			for (int j = 0; j < n_local_bases; ++j)
			{
				const Basis &b = bs.bases[j];
				const auto &tmp = tmp_val[j].val;

				for (std::size_t ii = 0; ii < b.global().size(); ++ii)
				{
					// auto item = global_index_to_col.find(b.global()[ii].index);
					// if(item != global_index_to_col.end()){
					auto item = global_index_to_col(b.global()[ii].index);
					if (item != -1)
					{
						for (int k = 0; k < int(tmp.size()); ++k)
						{
							// entries.push_back(Eigen::Triplet<double>(global_counter+k, item->second, tmp(k, j) * b.global()[ii].val));
							// entries_t.push_back(Eigen::Triplet<double>(item->second, global_counter+k, tmp(k, j) * b.global()[ii].val));
							entries.push_back(Eigen::Triplet<double>(global_counter + k, item, tmp(k) * b.global()[ii].val));
							entries_t.push_back(Eigen::Triplet<double>(item, global_counter + k, tmp(k) * b.global()[ii].val));
						}
						// global_mat.block(global_counter, item->second, tmp.size(), 1) = tmp;
					}
				}
			}

			// problem_.bc(mesh_, global_primitive_ids, mapped, t, rhs_fun);
			df(global_primitive_ids, uv, mapped, rhs_fun);
			global_rhs.block(global_counter, 0, rhs_fun.rows(), rhs_fun.cols()) = rhs_fun;
			global_counter += rhs_fun.rows();

			//UIState::ui_state().debug_data().add_points(mapped, Eigen::MatrixXd::Constant(1, 3, 0));

			//Eigen::MatrixXd asd(mapped.rows(), 3);
			//asd.col(0)=mapped.col(0);
			//asd.col(1)=mapped.col(1);
			//asd.col(2)=rhs_fun;
			//UIState::ui_state().debug_data().add_points(asd, Eigen::MatrixXd::Constant(1, 3, 0));
		}

		assert(global_counter == total_size);

		if (total_size > 0)
		{
			const double mmin = global_rhs.minCoeff();
			const double mmax = global_rhs.maxCoeff();

			if (fabs(mmin) < 1e-8 && fabs(mmax) < 1e-8)
			{
				// std::cout<<"is all zero, skipping"<<std::endl;
				for (size_t i = 0; i < indices.size(); ++i)
				{
					for (int d = 0; d < size_; ++d)
					{
						if (problem_.all_dimensions_dirichlet() || std::find(bounday_nodes.begin(), bounday_nodes.end(), indices[i] * size_ + d) != bounday_nodes.end())
							rhs(indices[i] * size_ + d) = 0;
					}
				}
			}
			else
			{
				StiffnessMatrix mat(int(total_size), int(indices.size()));
				mat.setFromTriplets(entries.begin(), entries.end());

				StiffnessMatrix mat_t(int(indices.size()), int(total_size));
				mat_t.setFromTriplets(entries_t.begin(), entries_t.end());

				StiffnessMatrix A = mat_t * mat;
				Eigen::MatrixXd b = mat_t * global_rhs;

				Eigen::MatrixXd coeffs(b.rows(), b.cols());
				auto solver = LinearSolver::create(solver_, preconditioner_);
				solver->setParameters(solver_params_);
				solver->analyzePattern(A, A.rows());
				solver->factorize(A);
				coeffs.setZero();
				for (long i = 0; i < b.cols(); ++i)
				{
					solver->solve(b.col(i), coeffs.col(i));
				}
				logger().trace("RHS solve error {}", (A * coeffs - b).norm());

				for (long i = 0; i < coeffs.rows(); ++i)
				{
					for (int d = 0; d < size_; ++d)
					{
						if (problem_.all_dimensions_dirichlet() || std::find(bounday_nodes.begin(), bounday_nodes.end(), indices[i] * size_ + d) != bounday_nodes.end())
							rhs(indices[i] * size_ + d) = coeffs(i, d);
					}
				}
			}
		}
	}

	void RhsAssembler::sample_bc(const std::function<void(const Eigen::MatrixXi &, const Eigen::MatrixXd &, const Eigen::MatrixXd &, Eigen::MatrixXd &)> &df,
								 const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, Eigen::MatrixXd &rhs) const
	{
		const int n_el = int(bases_.size());

		Eigen::MatrixXd rhs_fun;
		Eigen::VectorXi global_primitive_ids(1);
		Eigen::MatrixXd nans(1, 1);
		nans(0) = std::nan("");

#ifndef NDEBUG
		Eigen::Matrix<bool, Eigen::Dynamic, 1> is_boundary(n_basis_);
		is_boundary.setConstant(false);

		const int actual_dim = problem_.is_scalar() ? 1 : mesh_.dimension();

		int skipped_count = 0;
		for (int b : bounday_nodes)
		{
			int bindex = b / actual_dim;

			if (bindex < is_boundary.size())
				is_boundary[bindex] = true;
			else
				skipped_count++;
		}
		assert(skipped_count <= 1);
#endif

		for (const auto &lb : local_boundary)
		{
			const int e = lb.element_id();
			const ElementBases &bs = bases_[e];

			for (int i = 0; i < lb.size(); ++i)
			{
				global_primitive_ids(0) = lb.global_primitive_id(i);
				const auto nodes = bs.local_nodes_for_primitive(global_primitive_ids(0), mesh_);

				for (long n = 0; n < nodes.size(); ++n)
				{
					const auto &b = bs.bases[nodes(n)];
					const auto &glob = b.global();

					for (size_t ii = 0; ii < glob.size(); ++ii)
					{
						assert(is_boundary[glob[ii].index]);

						//TODO, missing UV!!!!
						df(global_primitive_ids, nans, glob[ii].node, rhs_fun);

						for (int d = 0; d < size_; ++d)
						{
							if (problem_.all_dimensions_dirichlet() || std::find(bounday_nodes.begin(), bounday_nodes.end(), glob[ii].index * size_ + d) != bounday_nodes.end())
								rhs(glob[ii].index * size_ + d) = rhs_fun(0, d);
						}
					}
				}
			}
		}
	}

	void RhsAssembler::integrate_bc(const std::function<void(const Eigen::MatrixXi &, const Eigen::MatrixXd &, const Eigen::MatrixXd &, Eigen::MatrixXd &)> &df,
									const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, Eigen::MatrixXd &rhs) const
	{
		Eigen::MatrixXd uv, samples, rhs_fun, normals, mapped;
		Eigen::VectorXd weights;

		Eigen::VectorXi global_primitive_ids;
		std::vector<AssemblyValues> tmp_val;

		Eigen::Matrix<bool, Eigen::Dynamic, 1> is_boundary(n_basis_);
		is_boundary.setConstant(false);

		Eigen::MatrixXd areas(rhs.rows(), 1);
		areas.setZero();

		const int actual_dim = problem_.is_scalar() ? 1 : mesh_.dimension();

		int skipped_count = 0;
		for (int b : bounday_nodes)
		{
			rhs(b) = 0;
			int bindex = b / actual_dim;

			if (bindex < is_boundary.size())
				is_boundary[bindex] = true;
			else
				skipped_count++;
		}
		assert(skipped_count <= 1);
		ElementAssemblyValues vals;

		for (const auto &lb : local_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = boundary_quadrature(lb, resolution, false, uv, samples, normals, weights, global_primitive_ids);

			if (!has_samples)
				continue;

			const ElementBases &bs = bases_[e];
			const ElementBases &gbs = gbases_[e];

			vals.compute(e, mesh_.is_volume(), samples, bs, gbs);

			df(global_primitive_ids, uv, vals.val, rhs_fun);

			for (int d = 0; d < size_; ++d)
				rhs_fun.col(d) = rhs_fun.col(d).array() * weights.array();

			for (int i = 0; i < lb.size(); ++i)
			{
				const int primitive_global_id = lb.global_primitive_id(i);
				const auto nodes = bs.local_nodes_for_primitive(primitive_global_id, mesh_);

				for (long n = 0; n < nodes.size(); ++n)
				{
					// const auto &b = bs.bases[nodes(n)];
					const AssemblyValues &v = vals.basis_values[nodes(n)];
					const double area = (weights.array() * v.val.array()).sum();
					for (int d = 0; d < size_; ++d)
					{
						const double rhs_value = (rhs_fun.col(d).array() * v.val.array()).sum();

						for (size_t g = 0; g < v.global.size(); ++g)
						{
							const int g_index = v.global[g].index * size_ + d;
							if (problem_.all_dimensions_dirichlet() || std::find(bounday_nodes.begin(), bounday_nodes.end(), g_index) != bounday_nodes.end())
							{
								rhs(g_index) += rhs_value * v.global[g].val;
								areas(g_index) += area * v.global[g].val;
							}
						}
					}
				}
			}
		}

		for (int b : bounday_nodes)
		{
			assert(areas(b) != 0);
			rhs(b) /= areas(b);
		}
	}

	void RhsAssembler::set_bc(
		const std::function<void(const Eigen::MatrixXi &, const Eigen::MatrixXd &, const Eigen::MatrixXd &, Eigen::MatrixXd &)> &df,
		const std::function<void(const Eigen::MatrixXi &, const Eigen::MatrixXd &, const Eigen::MatrixXd &, const Eigen::MatrixXd &, Eigen::MatrixXd &)> &nf,
		const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector<LocalBoundary> &local_neumann_boundary, Eigen::MatrixXd &rhs) const
	{
		if (bc_method_ == "sample")
			sample_bc(df, local_boundary, bounday_nodes, rhs);
		else if (bc_method_ == "integrate")
			integrate_bc(df, local_boundary, bounday_nodes, resolution, rhs);
		else
			lsq_bc(df, local_boundary, bounday_nodes, resolution, rhs);

		//Neumann
		Eigen::MatrixXd uv, samples, gtmp, rhs_fun;
		Eigen::VectorXi global_primitive_ids;
		Eigen::MatrixXd points, normals;
		Eigen::VectorXd weights;

		ElementAssemblyValues vals;

		for (const auto &lb : local_neumann_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = boundary_quadrature(lb, resolution, false, uv, points, normals, weights, global_primitive_ids);

			if (!has_samples)
				continue;

			const ElementBases &gbs = gbases_[e];
			const ElementBases &bs = bases_[e];

			vals.compute(e, mesh_.is_volume(), points, bs, gbs);

			for (int n = 0; n < vals.jac_it.size(); ++n)
			{
				normals.row(n) = normals.row(n) * vals.jac_it[n];
				normals.row(n).normalize();
			}
			// problem_.neumann_bc(mesh_, global_primitive_ids, vals.val, t, rhs_fun);
			nf(global_primitive_ids, uv, vals.val, normals, rhs_fun);

			// UIState::ui_state().debug_data().add_points(vals.val, Eigen::RowVector3d(0,1,0));

			for (int d = 0; d < size_; ++d)
				rhs_fun.col(d) = rhs_fun.col(d).array() * weights.array();

			for (int i = 0; i < lb.size(); ++i)
			{
				const int primitive_global_id = lb.global_primitive_id(i);
				const auto nodes = bs.local_nodes_for_primitive(primitive_global_id, mesh_);

				for (long n = 0; n < nodes.size(); ++n)
				{
					// const auto &b = bs.bases[nodes(n)];
					const AssemblyValues &v = vals.basis_values[nodes(n)];
					for (int d = 0; d < size_; ++d)
					{
						const double rhs_value = (rhs_fun.col(d).array() * v.val.array()).sum();

						for (size_t g = 0; g < v.global.size(); ++g)
						{
							const int g_index = v.global[g].index * size_ + d;
							const bool is_neumann = std::find(bounday_nodes.begin(), bounday_nodes.end(), g_index) == bounday_nodes.end();

							if (is_neumann)
							{
								rhs(g_index) += rhs_value * v.global[g].val;
							}
						}
					}
				}
			}
		}
	}

	void RhsAssembler::set_bc(const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector<LocalBoundary> &local_neumann_boundary, Eigen::MatrixXd &rhs, const double t) const
	{
		set_bc(
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val)
			{ problem_.bc(mesh_, global_ids, uv, pts, t, val); },
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, Eigen::MatrixXd &val)
			{ problem_.neumann_bc(mesh_, global_ids, uv, pts, normals, t, val); },
			local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs);
	}

	void RhsAssembler::set_velocity_bc(const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector<LocalBoundary> &local_neumann_boundary, Eigen::MatrixXd &rhs, const double t) const
	{
		set_bc(
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val)
			{ problem_.velocity_bc(mesh_, global_ids, uv, pts, t, val); },
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, Eigen::MatrixXd &val)
			{ problem_.neumann_velocity_bc(mesh_, global_ids, uv, pts, normals, t, val); },
			local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs);
	}

	void RhsAssembler::set_acceleration_bc(const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const int resolution, const std::vector<LocalBoundary> &local_neumann_boundary, Eigen::MatrixXd &rhs, const double t) const
	{
		set_bc(
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, Eigen::MatrixXd &val)
			{ problem_.acceleration_bc(mesh_, global_ids, uv, pts, t, val); },
			[&](const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const Eigen::MatrixXd &normals, Eigen::MatrixXd &val)
			{ problem_.neumann_acceleration_bc(mesh_, global_ids, uv, pts, normals, t, val); },
			local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs);
	}

	void RhsAssembler::compute_energy_grad(const std::vector<LocalBoundary> &local_boundary, const std::vector<int> &bounday_nodes, const Density &density, const int resolution, const std::vector<LocalBoundary> &local_neumann_boundary, const Eigen::MatrixXd &final_rhs, const double t, Eigen::MatrixXd &rhs) const
	{
		if (problem_.is_constant_in_time())
		{
			rhs = final_rhs;
		}
		else
		{
			assemble(density, rhs, t);
			rhs *= -1;
			// set_bc(local_boundary, bounday_nodes, resolution, local_neumann_boundary, rhs, t);

			if (rhs.size() != final_rhs.size())
			{
				const int prev_size = rhs.size();
				rhs.conservativeResize(final_rhs.size(), rhs.cols());
				//Zero initial pressure
				rhs.block(prev_size, 0, final_rhs.size() - prev_size, rhs.cols()).setZero();
				rhs(rhs.size() - 1) = 0;
			}

			assert(rhs.size() == final_rhs.size());
		}
	}

	double RhsAssembler::compute_energy(const Eigen::MatrixXd &displacement, const std::vector<LocalBoundary> &local_neumann_boundary, const Density &density, const int resolution, const double t) const
	{

		double res = 0;

		if (!problem_.is_rhs_zero())
		{
#if defined(POLYFEM_WITH_CPP_THREADS)
			std::vector<LocalThreadScalarStorage> storages(polyfem::get_n_threads());
#elif defined(POLYFEM_WITH_TBB)
			typedef tbb::enumerable_thread_specific<LocalThreadScalarStorage> LocalStorage;
			LocalStorage storages((LocalThreadScalarStorage()));
#else
			LocalThreadScalarStorage loc_storage;
			Eigen::MatrixXd forces;
#endif

			const int n_bases = int(bases_.size());

#if defined(POLYFEM_WITH_CPP_THREADS)
			polyfem::par_for(n_bases, [&](int start, int end, int t)
							 {
								 auto &loc_storage = storages[t];
								 Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> local_displacement(size_);
								 Eigen::MatrixXd forces;
								 for (int e = start; e < end; ++e)
								 {
#elif defined(POLYFEM_WITH_TBB)
			tbb::parallel_for(tbb::blocked_range<int>(0, n_bases), [&](const tbb::blocked_range<int> &r) {
				LocalStorage::reference loc_storage = storages.local();
				Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> local_displacement(size_);
				Eigen::MatrixXd forces;
				for (int e = r.begin(); e != r.end(); ++e)
				{
#else
			Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> local_displacement(size_);

			for (int e = 0; e < n_bases; ++e)
			{
#endif
									 ElementAssemblyValues &vals = loc_storage.vals;
									 vals.compute(e, mesh_.is_volume(), bases_[e], gbases_[e]);

									 const Quadrature &quadrature = vals.quadrature;
									 const Eigen::VectorXd da = vals.det.array() * quadrature.weights.array();

									 problem_.rhs(assembler_, formulation_, vals.val, t, forces);
									 assert(forces.rows() == da.size());
									 assert(forces.cols() == size_);

									 for (long p = 0; p < da.size(); ++p)
									 {
										 local_displacement.setZero();

										 for (size_t i = 0; i < vals.basis_values.size(); ++i)
										 {
											 const auto &bs = vals.basis_values[i];
											 assert(bs.val.size() == da.size());
											 const double b_val = bs.val(p);

											 for (int d = 0; d < size_; ++d)
											 {
												 for (std::size_t ii = 0; ii < bs.global.size(); ++ii)
												 {
													 local_displacement(d) += (bs.global[ii].val * b_val) * displacement(bs.global[ii].index * size_ + d);
												 }
											 }
										 }
										 const double rho = density(vals.val(p, 0), vals.val(p, 1), vals.val.cols() == 2 ? 0. : vals.val(p, 2), vals.element_id);

										 for (int d = 0; d < size_; ++d)
										 {
											 loc_storage.val += forces(p, d) * local_displacement(d) * da(p) * rho;
											 // res += forces(p, d) * local_displacement(d) * da(p);
										 }
									 }
#if defined(POLYFEM_WITH_CPP_THREADS) || defined(POLYFEM_WITH_TBB)
								 }
							 });
#else
				}
#endif

#if defined(POLYFEM_WITH_CPP_THREADS)
			for (const auto &t : storages)
			{
				res += t.val;
			}
#elif defined(POLYFEM_WITH_TBB)
				for (LocalStorage::iterator i = storages.begin(); i != storages.end(); ++i)
				{
					res += i->val;
				}
#else
				res = loc_storage.val;
#endif
		}

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1> local_displacement(size_);
		Eigen::MatrixXd forces;

		ElementAssemblyValues vals;
		//Neumann
		Eigen::MatrixXd points, uv, normals;
		Eigen::VectorXd weights;
		Eigen::VectorXi global_primitive_ids;
		for (const auto &lb : local_neumann_boundary)
		{
			const int e = lb.element_id();
			bool has_samples = boundary_quadrature(lb, resolution, false, uv, points, normals, weights, global_primitive_ids);

			if (!has_samples)
				continue;

			const ElementBases &gbs = gbases_[e];
			const ElementBases &bs = bases_[e];

			vals.compute(e, mesh_.is_volume(), points, bs, gbs);

			for (int n = 0; n < vals.jac_it.size(); ++n)
			{
				normals.row(n) = normals.row(n) * vals.jac_it[n];
				normals.row(n).normalize();
			}
			problem_.neumann_bc(mesh_, global_primitive_ids, uv, vals.val, normals, t, forces);

			// UIState::ui_state().debug_data().add_points(vals.val, Eigen::RowVector3d(1,0,0));

			for (long p = 0; p < weights.size(); ++p)
			{
				local_displacement.setZero();

				for (size_t i = 0; i < vals.basis_values.size(); ++i)
				{
					const auto &vv = vals.basis_values[i];
					assert(vv.val.size() == weights.size());
					const double b_val = vv.val(p);

					for (int d = 0; d < size_; ++d)
					{
						for (std::size_t ii = 0; ii < vv.global.size(); ++ii)
						{
							local_displacement(d) += (vv.global[ii].val * b_val) * displacement(vv.global[ii].index * size_ + d);
						}
					}
				}

				for (int d = 0; d < size_; ++d)
					res -= forces(p, d) * local_displacement(d) * weights(p);
			}
		}

		return res;
	}

	bool RhsAssembler::boundary_quadrature(const LocalBoundary &local_boundary, const int order, const bool skip_computation, Eigen::MatrixXd &uv, Eigen::MatrixXd &points, Eigen::MatrixXd &normals, Eigen::VectorXd &weights, Eigen::VectorXi &global_primitive_ids) const
	{
		uv.resize(0, 0);
		points.resize(0, 0);
		normals.resize(0, 0);
		weights.resize(0);
		global_primitive_ids.resize(0);

		for (int i = 0; i < local_boundary.size(); ++i)
		{
			const int gid = local_boundary.global_primitive_id(i);
			Eigen::MatrixXd tmp_p, tmp_uv, tmp_n;
			Eigen::VectorXd tmp_w;
			switch (local_boundary.type())
			{
			case BoundaryType::TriLine:
				BoundarySampler::quadrature_for_tri_edge(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w);
				BoundarySampler::normal_for_tri_edge(local_boundary[i], tmp_n);
				break;
			case BoundaryType::QuadLine:
				BoundarySampler::quadrature_for_quad_edge(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w);
				BoundarySampler::normal_for_quad_edge(local_boundary[i], tmp_n);
				break;
			case BoundaryType::Quad:
				BoundarySampler::quadrature_for_quad_face(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w);
				BoundarySampler::normal_for_quad_face(local_boundary[i], tmp_n);
				break;
			case BoundaryType::Tri:
				BoundarySampler::quadrature_for_tri_face(local_boundary[i], order, gid, mesh_, tmp_uv, tmp_p, tmp_w);
				BoundarySampler::normal_for_tri_face(local_boundary[i], tmp_n);
				break;
			case BoundaryType::Polygon:
				BoundarySampler::quadrature_for_polygon_edge(local_boundary.element_id(), local_boundary.global_primitive_id(i), order, mesh_, tmp_uv, tmp_p, tmp_w);
				BoundarySampler::normal_for_polygon_edge(local_boundary.element_id(), local_boundary.global_primitive_id(i), mesh_, tmp_n);
				break;
			case BoundaryType::Invalid:
				assert(false);
				break;
			default:
				assert(false);
			}

			uv.conservativeResize(uv.rows() + tmp_uv.rows(), tmp_uv.cols());
			uv.bottomRows(tmp_uv.rows()) = tmp_uv;

			points.conservativeResize(points.rows() + tmp_p.rows(), tmp_p.cols());
			points.bottomRows(tmp_p.rows()) = tmp_p;

			normals.conservativeResize(normals.rows() + tmp_p.rows(), tmp_p.cols());
			for (int k = normals.rows() - tmp_p.rows(); k < normals.rows(); ++k)
				normals.row(k) = tmp_n;

			weights.conservativeResize(weights.rows() + tmp_w.rows(), tmp_w.cols());
			weights.bottomRows(tmp_w.rows()) = tmp_w;

			global_primitive_ids.conservativeResize(global_primitive_ids.rows() + tmp_p.rows());
			global_primitive_ids.bottomRows(tmp_p.rows()).setConstant(gid);
		}

		assert(uv.rows() == global_primitive_ids.size());
		assert(points.rows() == global_primitive_ids.size());
		assert(normals.rows() == global_primitive_ids.size());
		assert(weights.size() == global_primitive_ids.size());

		return true;
	}

	bool RhsAssembler::sample_boundary(const LocalBoundary &local_boundary, const int n_samples, const bool skip_computation, Eigen::MatrixXd &uv, Eigen::MatrixXd &samples, Eigen::VectorXi &global_primitive_ids) const
	{
		uv.resize(0, 0);
		samples.resize(0, 0);
		global_primitive_ids.resize(0);

		for (int i = 0; i < local_boundary.size(); ++i)
		{
			Eigen::MatrixXd tmp, tmp_uv;
			switch (local_boundary.type())
			{
			case BoundaryType::TriLine:
				BoundarySampler::sample_parametric_tri_edge(local_boundary[i], n_samples, tmp_uv, tmp);
				break;
			case BoundaryType::QuadLine:
				BoundarySampler::sample_parametric_quad_edge(local_boundary[i], n_samples, tmp_uv, tmp);
				break;
			case BoundaryType::Quad:
				BoundarySampler::sample_parametric_quad_face(local_boundary[i], n_samples, tmp_uv, tmp);
				break;
			case BoundaryType::Tri:
				BoundarySampler::sample_parametric_tri_face(local_boundary[i], n_samples, tmp_uv, tmp);
				break;
			case BoundaryType::Polygon:
				BoundarySampler::sample_polygon_edge(local_boundary.element_id(), local_boundary.global_primitive_id(i), n_samples, mesh_, tmp_uv, tmp);
				break;
			case BoundaryType::Invalid:
				assert(false);
				break;
			default:
				assert(false);
			}

			uv.conservativeResize(uv.rows() + tmp_uv.rows(), tmp_uv.cols());
			uv.bottomRows(tmp_uv.rows()) = tmp_uv;

			samples.conservativeResize(samples.rows() + tmp.rows(), tmp.cols());
			samples.bottomRows(tmp.rows()) = tmp;

			global_primitive_ids.conservativeResize(global_primitive_ids.rows() + tmp.rows());
			global_primitive_ids.bottomRows(tmp.rows()).setConstant(local_boundary.global_primitive_id(i));
		}

		assert(uv.rows() == global_primitive_ids.size());
		assert(samples.rows() == global_primitive_ids.size());

		return true;
	}

} // namespace polyfem


#include <complex>
#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <fmt/format.h>
//#include <boost/config/warning_disable.hpp>
//#include <boost/spirit/home/x3.hpp>
//#include <range/v3/all.hpp>
#include <cstdint>
// 0 ,1
//1,2
//(i,j)( i+j +1
//i,j+1   (i+j+1,0
int main(int argc, char **argv)
{
  int i = 2978 - 1, j = 3083 - 1;
  int n = (i - 1) + j;
  int total = 0;
  for (int i = 0; i <= n; ++i) {
    total += 1 + i;
  }
  total += j + 1;
  //a[total]-1;
  std::int64_t a = 20151125;
  for (int i = 1; i < total; ++i) {
    a *= 252533;
    a %= 33554393;
    //fmt::print(";i:{},a:{}", i, a);
    //
  }
  fmt::print("{}", a);
}


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

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

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

#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/rem_pio2_medium.hpp>
#include <boost/simd/function/rem_pio2_cephes.hpp>
#include <boost/simd/function/rem_pio2_straight.hpp>
#include <boost/simd/function/rem_pio2.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

#include <boost/simd/function/all.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/round2even.hpp>
#include <boost/simd/function/if_else_nan.hpp>
#include <boost/simd/function/is_ngt.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/function/inrad.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/medium_pi.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

//#include <nt2/sdk/meta/as_logical.hpp>
//#include <boost/simd/sdk/meta/is_upgradable.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    template< class A0
            , class unit_tag
            , class style
            , class mode
            , class base_A0 = bd::scalar_of_t<A0>
    >
    struct trig_reduction;

    // This class exposes the public static member:
    // reduce:                to provide range reduction
    //
    // unit_tag allows to choose statically the scaling  among radian_tag, pi_tag, degree_tag
    // meaning that the cosa function will (for example) define respectively
    // x-->cos(x)          (radian_tag),
    // x-->cos(p*x)        (pi_tag)
    // x-->cos((pi/180)*x) (degree_tag)
    //

    // trigonometric reduction strategies in the [-pi/4, pi/4] range.
    // these reductions are used in the accurate and fast
    // trigonometric functions with different policies

    template<class A0, class style, class mode>
    struct trig_reduction < A0, tag::radian_tag, style, mode>
    {
      using i_t = bd::as_integer_t<A0, signed>;
      using l_t = bs::as_logical_t<A0>;
      using conversion_allowed_t = std::false_type; //bd::is_upgradable_on_ext_<A0>; // TODO

      static BOOST_FORCEINLINE auto is_0_pio4_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Pio_4<A0>()))
      {
        return is_ngt(a0, Pio_4<A0>());
      }
      static BOOST_FORCEINLINE auto is_0_pio2_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Pio_2<A0>()))
      {
        return is_ngt(a0, Pio_2<A0>());
      }
      static BOOST_FORCEINLINE auto is_0_20pi_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Real<A0, 0X404F6A7A2955385EULL, 0X427B53D1UL>()))
      {
        return is_ngt(a0, Real<A0, 0X404F6A7A2955385EULL, 0X427B53D1UL>()); //20 pi;
      }
      static BOOST_FORCEINLINE auto is_0_mpi_reduced (const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0, Medium_pi<A0>()))
      {
        return is_ngt(a0, Medium_pi<A0>()); //2^6 pi
      }
      static BOOST_FORCEINLINE auto is_0_dmpi_reduced(const A0&a0) BOOST_NOEXCEPT
      ->  decltype(is_ngt(a0,Constant<A0,262144>()))
      {
        return is_ngt(a0, Ratio<A0,262144>()); //2^18 pi
      }

      static BOOST_FORCEINLINE l_t cot_invalid(const A0& )  BOOST_NOEXCEPT
      {
        return False<l_t>();
      }
      static BOOST_FORCEINLINE l_t tan_invalid(const A0& )  BOOST_NOEXCEPT
      {
        return False<l_t>();
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr)  BOOST_NOEXCEPT
      {
        return inner_reduce(x, xr);
      }

      static BOOST_FORCEINLINE i_t inner_reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xx =  preliminary<mode>::clip(x);
        return select_mode(xx, xr, boost::mpl::int_<mode::start>());
      }

      template < class Mode, bool clipped = Mode::clipped>
      struct preliminary
      {
        static BOOST_FORCEINLINE A0 const& clip(const A0& x) BOOST_NOEXCEPT  { return x; }
      };

      template < class Mode>
      struct preliminary<Mode, true>
      {
        static BOOST_FORCEINLINE A0 clip(const A0& x) BOOST_NOEXCEPT
        {
          return clipto(x, boost::mpl::int_<Mode::range>());
        }
      private :
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_pio4> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_pio4_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_20pi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_20pi_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_mpi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_mpi_reduced(x), x);
        }
        static BOOST_FORCEINLINE A0 clipto(const A0& x
                                          , boost::mpl::int_<tag::r_0_dmpi> const&) BOOST_NOEXCEPT
        {
          return if_else_nan(is_0_dmpi_reduced(x), x);
        }
      };

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_pio4> const&
                  ) BOOST_NOEXCEPT
      {
        xr = xx;
        return Zero<i_t>();
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_pio4> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_pio4_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_pio2>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_<tag::r_0_pio4> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_pio4>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_<tag::r_0_pio2> const&) BOOST_NOEXCEPT
      {
        if(all(is_0_pio2_reduced(xx)))
          return rem_pio2_straight(xx, xr);
        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_20pi>());
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_20pi> const&
                  ) BOOST_NOEXCEPT
      {
        return rem_pio2_cephes(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_20pi> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_20pi_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_mpi>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_20pi> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_20pi>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::true_ const&
                  , boost::mpl::int_<tag::r_0_mpi> const&
                  ) BOOST_NOEXCEPT
      {
        return rem_pio2_medium(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      select_range( const A0& xx, A0& xr
                  , boost::mpl::false_ const&
                  , boost::mpl::int_<tag::r_0_mpi> const& r
                  ) BOOST_NOEXCEPT
      {
        if(all(is_0_mpi_reduced(xx)))
          return select_range(xx,xr,boost::mpl::true_(), r);

        return select_mode(xx,xr,boost::mpl::int_<tag::r_0_dmpi>());
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_mpi> const& r) BOOST_NOEXCEPT
      {
        return select_range(xx,xr,boost::mpl::bool_<mode::range == tag::r_0_mpi>(),r);
      }

      static BOOST_FORCEINLINE i_t
      select_mode(const A0& xx, A0& xr
                 , boost::mpl::int_< tag::r_0_dmpi> const&) BOOST_NOEXCEPT
      {
        if(all(is_0_dmpi_reduced(xx)))
           return use_conversion(xx, xr, style(), conversion_allowed_t());
        return rem_pio2(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & xx,  A0& xr
                    ,  const style &, std::false_type) BOOST_NOEXCEPT
      {
        return rem_pio2(xx, xr);
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & xx,  A0& xr
                    ,  const tag::not_simd_type &, std::true_type) BOOST_NOEXCEPT
      {
        // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
        using uA0 = bd::upgrade_t<A0>;
        using aux_reduction = trig_reduction< uA0,tag::radian_tag,tag::not_simd_type,mode,double>;
        uA0 ux = xx, uxr;
        i_t n = static_cast<i_t>(aux_reduction::reduce(ux, uxr));
        xr = static_cast<A0>(uxr);
        return n;
      }

      static BOOST_FORCEINLINE i_t
      use_conversion(const A0 & x,  A0& xr
                    , const tag::simd_type &,std::true_type) BOOST_NOEXCEPT
      {
        // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
        using uA0 = bd::upgrade_t<A0>;
        using aux_reduc_t = trig_reduction< uA0, tag::radian_tag,  tag::simd_type, mode, double>;
        uA0 ux1, ux2, uxr1, uxr2;
        split(x, ux1, ux2);
        auto n1 = aux_reduc_t::reduce(ux1, uxr1);
        auto n2 = aux_reduc_t::reduce(ux2, uxr2);
        xr = group(uxr1, uxr2);
        split(xr, ux1, ux2);
        return group(n1, n2);
      }
    };

    template<class A0, class style>
    struct trig_reduction<A0,tag::degree_tag, style, tag::big_tag>
    {
      using i_t = bd::as_integer_t<A0, signed>;

      static BOOST_FORCEINLINE auto cot_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_nez(x)&&is_flint(x*Ratio<A0,1,180>()))
      {
        return is_nez(x)&&is_flint(x*Ratio<A0,1,180>());
      }
      static BOOST_FORCEINLINE auto tan_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_flint((x- Ratio<A0,90>())*Ratio<A0,1,180>()))
      {
        return is_flint((x- Ratio<A0,90>())*Ratio<A0,1,180>());
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xi = round2even(x*Ratio<A0,1,90>());
        A0 x2 = x - xi * Ratio<A0,90>();

        xr =  inrad(x2);
        return bs::fast_(toint)(xi);
      }
    };

#ifdef BOOST_SIMD_HAS_X87
    template<class A0>
    struct trig_reduction<A0,degree_tag, tag::not_simd_type, tag::big_tag>
    {
      using i_t = bd::as_integer_t<A0, signed>;

      static BOOST_FORCEINLINE auto cot_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_nez(x)&&is_flint(x*Ratio<A0,1,180>()))
      {
        return is_nez(x)&&is_flint(x/Constant<A0,180>());
      }
      static BOOST_FORCEINLINE auto tan_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_flint((x- Ratio<A0,90>())/Constant<A0,180>()))
      {
        return is_flint((x- Ratio<A0,90>())*Ratio<A0,1,180>());
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x, A0& xr) BOOST_NOEXCEPT
      {
        A0 xi = round2even(x*Ratio<A0,1,90>());
        A0 x2 = x - xi * Ratio<A0,90>();

        xr =  inrad(x2);
        return bs::fast_(toint)(xi);
      }
    };
#endif

    template < class A0, class style>
    struct trig_reduction < A0, tag::pi_tag,  style, tag::big_tag>
    {
      using i_t = bd::as_integer_t<A0, signed>;

      static BOOST_FORCEINLINE auto cot_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_nez(x)&&is_flint(x))
      {
        return is_nez(x)&&is_flint(x);
      }
      static BOOST_FORCEINLINE auto tan_invalid(const A0& x) BOOST_NOEXCEPT
      -> decltype(is_flint(x-Half<A0>()))
      {
        return is_flint(x-Half<A0>()) ;
      }

      static BOOST_FORCEINLINE i_t reduce(const A0& x,  A0& xr) BOOST_NOEXCEPT
      {
        A0 xi = round2even(x*Two<A0>());
        A0 x2 = x - xi * Half<A0>();
        xr = x2*Pi<A0>();
        return fast_(toint)(xi);
      }
    };
  }
} }

#endif