/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2020, Alliance for Sustainable Energy, LLC, and other contributors. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#ifndef UTILITIES_GEOMETRY_TRANSFORMATION_HPP
#define UTILITIES_GEOMETRY_TRANSFORMATION_HPP

#include "../UtilitiesAPI.hpp"
#include "../data/Matrix.hpp"
#include "../data/Vector.hpp"
#include "../core/Logger.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  // forward declaration
  class Point3d;
  class Vector3d;
  class Plane;
  class BoundingBox;
  class EulerAngles;

  class UTILITIES_API Transformation{
  public:

    /// default constructor creates identity transformation
    Transformation();

    /// constructor from storage, asserts matrix is 4x4
    Transformation(const Matrix& matrix);

    /// constructor from storage, asserts vector is size 16
    Transformation(const Vector& vector);

    /// copy constructor
    Transformation(const Transformation& other);

    /// rotation about origin defined by axis and angle (radians)
    static Transformation rotation(const Vector3d& axis, double radians);

    /// rotation about point defined by axis and angle (radians)
    static Transformation rotation(const Point3d& origin, const Vector3d& axis, double radians);

    /// rotation specified by Euler angles
    static Transformation rotation(const EulerAngles& angles);

    /// translation along vector
    static Transformation translation(const Vector3d& translation);

    /// transforms system with z' to regular system
    /// will try to align y' with z, but if that fails will align y' with y
    static Transformation alignZPrime(const Vector3d& zPrime);

    /// transforms face coordinates to regular system, face normal will be z'
    /// will try to align y' with z, but if that fails will align y' with y
    /// face origin will be minimum point in x', y' and z'=0
    /// will return identity transformation if cannot compute plane for vertices
    static Transformation alignFace(const std::vector<Point3d>& vertices);

    /// returns a transformation which is the inverse of this
    Transformation inverse() const;

    /// get the matrix representation directly
    Matrix matrix() const;

    /// get the vector representation directly
    Vector vector() const;

    /// get the Euler angles for the transformation, does not include translation
    EulerAngles eulerAngles() const;

    /// get the rotation matrix for the transformation, does not include translation
    Matrix rotationMatrix() const;

    /// get the translation for the transformation, does not include rotation
    Vector3d translation() const;

    /// apply the transformation to the point
    Point3d operator*(const Point3d& point) const;

    /// apply the transformation to the vector
    Vector3d operator*(const Vector3d& vector) const;

    /// apply the transformation to the bounding box
    BoundingBox operator*(const BoundingBox& boundingBox) const;

    /// apply the transformation to the plane
    Plane operator*(const Plane& plane) const;

    /// apply the transformation to a vector of points
    std::vector<Point3d> operator*(const std::vector<Point3d>& points) const;

    /// apply the transformation to a vector of vector
    std::vector<Vector3d> operator*(const std::vector<Vector3d>& vectors) const;

    /// apply the transformation to the other transformation
    Transformation operator*(const Transformation& other) const;

  private:

    REGISTER_LOGGER("utilities.Transformation");
    Matrix m_storage;

  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const Transformation& t);

  // optional Transformation
  typedef boost::optional<Transformation> OptionalTransformation;

  // vector of Transformation
  typedef std::vector<Transformation> TransformationVector;

  /// create rotation about origin defined by axis and angle (radians)
  UTILITIES_API Transformation createRotation(const Vector3d& axis, double radians);

  /// create rotation about point defined by axis and angle (radians)
  UTILITIES_API Transformation createRotation(const Point3d& origin, const Vector3d& axis, double radians);

  /// create rotation specified by Euler angles
  UTILITIES_API Transformation createRotation(const EulerAngles& angles);

  /// create translation along vector
  UTILITIES_API Transformation createTranslation(const Vector3d& translation);

} // openstudio

#endif //UTILITIES_GEOMETRY_TRANSFORMATION_HPP


#ifndef __MANIPULATION_CONTROL_ALGORITHM__
#define __MANIPULATION_CONTROL_ALGORITHM__
#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <limits>
#include <stdexcept>

namespace manipulation_algorithms{
  /**
    Class that implements the dexterous manipulation control algorithm.
  **/
  class ManipulationAlgorithm : public AlgorithmBase
  {
  public:
    ManipulationAlgorithm();
    ~ManipulationAlgorithm();

    /**
      Compute the control values for the system.

      @param x_d The desired control goal.
      @param x_c_aug The current system state.
      @param x_e The end-effector state.
      @return The computed control values
    **/
    Eigen::Vector3d compute(const Eigen::Vector3d &x_d, const Eigen::VectorXd &x_c_aug, const Eigen::Vector3d &x_e);

    /**
      Obtain the parameters relevant to the controller from the parameter server.

      @param n The nodehandle that will be used to query the parameter server
      @return False in case of error
    **/
    bool getParams(const ros::NodeHandle &n);

  private:
    Eigen::Matrix3d Gamma_;
    double k_s_, max_command_x_, max_command_y_, max_command_theta_;

    /**
      Compute the inverse of the task jacobian from the system state.

      @param x_e The end-effector horizontal translation
      @param x_c The contact point horizontal translation
      @param theta_c The object orientation w.r.t the surface frame
      @return The inverse of the task jacobian
    **/
    Eigen::Matrix3d computeInvG(const double x_e, const double x_c, const double theta_c, const double f_c_x, const double f_c_y);
  };
}
#endif


#include <boost/math/tools/promotion.hpp>
#include <ostream>

namespace bernoulli_external_model_namespace
{
    template <typename T0__>
    inline typename boost::math::tools::promote_args<T0__>::type make_odds(const T0__ &
                                                                               theta,
                                                                           std::ostream *pstream__)
    {
        return theta / (1 - theta);
    }
}


#ifndef ROMPC_CTRL_HPP
#define ROMPC_CTRL_HPP

/**
	@file rompc.hpp
	Header file defining the ROMPC controller class.

	@brief ROMPC controller class.
*/
#include <rompc/rompc_utils.hpp>

#include <ros/ros.h>
#include <string>
#include <Eigen/Dense>

using Vec3 = Eigen::Vector3d;
using Vec4 = Eigen::Vector4d;
using VecX = Eigen::VectorXd;
using Mat3 = Eigen::Matrix3d;
using MatX = Eigen::MatrixXd;

using TargetPtr = std::unique_ptr<ROMPC_UTILS::Target>;

class ROMPC {
public:
	ROMPC(ros::NodeHandle& nh, const unsigned ctrl_type,
             const unsigned target_type, const unsigned model_type, double dt,
             const std::string filepath, const double tmax, const bool debug = false);
	void init(const double t0, const Vec3 p, const double psi);
    void start();
    void stop();
	void update(const double t, const Vec3 p_b_i_I, const Vec3 v_b_I_B,
                const Vec3 euler, const Vec3 om_B_I_B, const double T,
                const Vec3 ctrl_srf);
	VecX get_ctrl(double t);
	VecX get_xhat();
	VecX get_xbar();
    bool started();

    // Control types
    static const unsigned BODY_RATE = 0;
    static const unsigned CTRL_SURF = 1;

    // Model types
    // Standard model is u = [T, p, q, r]
    //                   y = [u, v, w, phi, th, psi, x_r, y_r, z_r]
    // or                u = [T, a, e, r]
    //                   y = [u, v, w, p, q, r, phi, th, psi, x_r, y_r, z_r]
    static const unsigned STANDARD = 0;

    // CFD measurement is u = [T, th1d, th2d, th3d]
    //                    y = [xd_r, yd_r, zd_r, x_r, y_r, z_r, th1, th2, th3]
    // or                 u = [T, a, e, r]
    //                    y = [xd_r, yd_r, zd_r, x_r, y_r, z_r, th1d, th2d
    //                         th3d, th1, th2, th3]
    // where th = (th1, th2, th3) is axis/angle paramterization of orientation
    static const unsigned CFD = 1;

    // Target types
    static const unsigned SGF = 0; // steady glidelsope flight
    static const unsigned SLF = 1; // steady level flight
    static const unsigned STF = 2; // steady turning flight

private:
    MatX _K; // controller gain matrix
    MatX _L; // estimator gain matrix
    MatX _A; // system dynamics ...
    MatX _B; // ...
    MatX _C; // ...
    MatX _H; // ... matrices
    int _n; // state dimensior
    Eigen::HouseholderQR<MatX> _M_est; // I - dt*(A-LC) factorized

    ROMPC_UTILS::OCP _ocp; // optimal control problem
    MatX _Ad; // discrete time ROM matrix with _ocp dt
    MatX _Bd; // discrete time control matrix with _ocp dt

    VecX _xbar; // nominal ROM state
    Vec4 _ubar; // nominal ROM control
    VecX _xhat; // ROM state estimate
    VecX _u_eq; // equilibrium feed forward control
    VecX _u; // control to be applied
    VecX _y; // measurement
    VecX _zhat; // estimated output variables
    VecX _zbar; // nominal output variables

    double _dt; // control rate
    double _t0; // initialization time (s)
    double _t; // last update time (s)
    double _t_ROM; // last time ROM was updated (s)
    double _qp_dt; // discretization time of MPC
    bool _init = false;
    bool _started = false;
    Vec4 _q_R_to_B;

    unsigned _ctrl_type; // Control type
    unsigned _target_type; // Target type
    unsigned _model_type; // Measurement type
    bool _debug;

    TargetPtr _target; // pointer to target object

    ros::Publisher _e_pos_pub; // tracking position error [m]
    ros::Publisher _e_vel_pub; // tracking velocity error [m/s]
    ros::Publisher _e_att_pub; // attitude error (euler or axis/angle) [rad]
    ros::Publisher _e_attrate_pub; // attitude rate error (omega or axis/angle)
    ros::Publisher _ubar_pub;
    ros::Publisher _u_pub;
    ros::Publisher _zbar_pub;
    ros::Publisher _zhat_pub;
    ros::Publisher _uprev_pub;
    ros::Publisher _y_pub;
    ros::Publisher _qptime_pub;

    void update_ctrl(const double t, const Vec4 u_prev);
};

#endif // ROMPC_CTRL_HPP


// Prepare for G2O-------------------------------------------------

#include <Eigen/StdVector>
#include <iostream>

#include <stdint.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes6D.hpp"
// #include "continuous_to_discrete.h"
// #include "targetTypes3D.hpp"
using namespace Eigen;
using namespace std;
using namespace g2o;

// Prepare for ROS-------------------------------------------------
#include <ros/ros.h>
#include <ros/time.h>
#include "sensor_msgs/Imu.h"
// #include "sensor_msgs/MagneticField.h"
// #include "sensor_msgs/Temperature.h"
// #include "sensor_msgs/FluidPressure.h"
// #include "sensor_msgs/Joy.h"
// #include "sensor_msgs/NavSatFix.h"
// #include "sensor_msgs/Range.h"

// #include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
// #include "geometry_msgs/PoseStamped.h"
// #include "geometry_msgs/TwistWithCovarianceStamped.h"
// #include <geometry_msgs/Vector3Stamped.h>
// #include <sensor_fusion_comm/ExtEkf.h>
#include <sensor_fusion_comm/DoubleArrayStamped.h>

// #include "nav_msgs/Odometry.h"
// #include "crossover_nav/Ack.h"
// #include "crossover_nav/Navdata.h"
// #include "crossover_nav/odom_data.h"
// #include "crossover_nav/Status.h"

#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Quaternion.h>

// #include <dynamic_reconfigure/BoolParameter.h>
// #include <dynamic_reconfigure/Reconfigure.h>
// #include <dynamic_reconfigure/Config.h>

// #include <termios.h>

//function convert arduino to ros
//------------ROS version-------
// #include <iostream>
inline long millis() {
  return (1e3 * ros::Time::now().sec + 1e-6 * ros::Time::now().nsec);
}
inline long micros() {
  return (1e6 * ros::Time::now().sec + 1e-3 * ros::Time::now().nsec);
}
//not use millis() it give me online floating point behind sec
//go use ros::Time::now();
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

#define delay(x) ros::Duration(x/1000.0).sleep()
#define say(x) (std::cout << x)
#define sayend(x) (std::cout << x << std::endl)
#define saytab(x) (std::cout << x << "\t")
#define saycomma(x) (std::cout << x << " ,")
#define Min(a,b) ((a)<(b)?(a):(b))
#define Max(a,b) ((a)>(b)?(a):(b))
#define isfinite(X) std::isfinite(X)
#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))
#define wrap_pi(x) (x < -3.14 ? x+6.28 : (x > 3.14 ? x - 6.28: x))
#define HALF_M_PI 1.570796

sensor_msgs::Imu imu;
geometry_msgs::PoseWithCovarianceStamped gps_pose , poseop_msgs;
sensor_fusion_comm::DoubleArrayStamped state_out_msf;
#define NUM_ITERATE 1

Vector3d gps[NUM_ITERATE];
Vector6d msf_state[200];
Vector3d acc[200];
double dtime[200];
int Iterate = 0;
Vector6d state;
int Numimu = 0;
int Numstate = 0;
void imucallback(const sensor_msgs::Imu::ConstPtr &data)
{
  // imu = *data;
  sensor_msgs::Imu imu_bf = *data;

  static ros::Time st = ros::Time::now();
  static ros::Duration ct = ros::Time::now()-st;
  static ros::Duration pt= ct;
  ct = ros::Time::now()-st;
  dtime[Numimu] = (ct - pt).toSec();

  //transform bf to ef
  tf::Quaternion qimu(imu_bf.linear_acceleration.x,imu_bf.linear_acceleration.y,imu_bf.linear_acceleration.z,0);
  tf::Quaternion q(imu_bf.orientation.x,imu_bf.orientation.y,imu_bf.orientation.z,imu_bf.orientation.w);

  qimu = q*qimu*q.inverse() - tf::Quaternion(0,0,9.81,0);

  imu.linear_acceleration.x = qimu.x();
  imu.linear_acceleration.y = qimu.y();
  imu.linear_acceleration.z = qimu.z();
  imu.orientation = imu_bf.orientation;

  acc[Numimu][0] = imu.linear_acceleration.x;
  acc[Numimu][1] = imu.linear_acceleration.y;
  acc[Numimu][2] = imu.linear_acceleration.z;

  Numimu++;
  // tf::Quaternion q(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w);
  // tf::Matrix3x3 m(q);
  // m.getRPY(roll, pitch, yaw);
  //ROS_INFO("ax:[%f]", yaw);
}
void gpscallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& data) {
  gps_pose = *data;

  if(Iterate>=NUM_ITERATE) return;

  gps[Iterate][0] = gps_pose.pose.pose.position.x;
  gps[Iterate][1] = gps_pose.pose.pose.position.y;
  gps[Iterate][2] = gps_pose.pose.pose.position.z;

  Iterate++;

}
void state_out_callback(const sensor_fusion_comm::DoubleArrayStamped::ConstPtr& data) {

  state_out_msf = *data;

  for(int i =0;i<6;i++)
    msf_state[Numstate][i] = state_out_msf.data[i];
  Numstate++;
}

void Optimize_test(double dt);

int main( int argc, char** argv )
{
  ros::init(argc, argv, "gps_tester");
  ros::NodeHandle n;
  ros::Rate r(200);

  ros::Subscriber imu_sub           = n.subscribe<sensor_msgs::Imu>("/imu_max", 2, imucallback);
  ros::Subscriber gps_sub           = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/imu_max/pose", 2, gpscallback);
  ros::Subscriber state_out_msf_sub = n.subscribe<sensor_fusion_comm::DoubleArrayStamped>("/msf_core/state_out", 2, state_out_callback);
  ros::Publisher  poseop_pub        = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/optimized", 10);

  static ros::Time start_time = ros::Time::now();
  static ros::Duration cur_time = ros::Time::now()-start_time;
  static ros::Duration prev_time= cur_time;
  static double dt = 0.1;

  poseop_msgs.header.frame_id = "odom";

  state_out_msf.data.resize(36);
  // state.setZero();
  // Iterate over the simulation steps

  state.setZero();
  for (int k = 0; k < 3; k++)
    {
      state[k] = 0;
    }
  while(ros::ok())
    {

      // saytab("Iter ");sayend(Iterate);
      // static int k = 1;
      ros::spinOnce();
      r.sleep();
      cur_time = ros::Time::now()-start_time;
      dt = (cur_time - prev_time).toSec();
      if(Iterate >= NUM_ITERATE) {
        prev_time = cur_time;
        sayend("START");
        Optimize_test(dt);
        sayend("END");
        Iterate=0;
        Numimu=0;
        Numstate = 0;
        poseop_pub.publish(poseop_msgs);
      }

      // saytab("cur="); saytab(cur_time.toSec());
      // saytab("prev=");saytab(prev_time.toSec());
      // saytab("dt=");  sayend(dt);
    }
}

// void Optimize_test(double dt ) {
//     // Set up the optimiser
//   SparseOptimizer optimizer;
//   optimizer.setVerbose(false);

//   // Create the block solver - the dimensions are specified because
//   // 3D observations marginalise to a 3D estimate
//   typedef BlockSolver<BlockSolverTraits<3, 3> > BlockSolver_3_3;
//   BlockSolver_3_3::LinearSolverType* linearSolver
//       = new LinearSolverCholmod<BlockSolver_3_3::PoseMatrixType>();
//   BlockSolver_3_3* blockSolver
//       = new BlockSolver_3_3(linearSolver);
//   OptimizationAlgorithmGaussNewton* solver
//     = new OptimizationAlgorithmGaussNewton(blockSolver);
//   optimizer.setAlgorithm(solver);

//   // Sample the actual location of the target
//   Vector3d truePoint(0,
//                      0,
//                      0);

//         // Construct vertex which corresponds to the actual point of the target
//   VertexPosition3D* position = new VertexPosition3D();
//   position->setId(0);
//   optimizer.addVertex(position);

//   // Now generate some noise corrupted measurements; for simplicity
//   // these are uniformly distributed about the true target. These are
//   // modelled as a unary edge because they do not like to, say,
//   // another node in the map.
//   int numMeasurements = Iterate;
//   double noiseLimit = sqrt(12.);
//   double noiseSigma = noiseLimit*noiseLimit / 12.0;
//   double noiseSigma2 = noiseSigma;

//   for (int i = 0; i < numMeasurements; i++)
//     {
//         for (int k = 0; k < Numstate; k++)
//           {
//             saytab("stage2");
//             Vector3d measurement2 = msf_state[k];
//             GPSObservationPosition3DEdge* goe2 = new GPSObservationPosition3DEdge();
//             goe2->setVertex(0, position);
//             goe2->setMeasurement(measurement2);
//             goe2->setInformation(Matrix3d::Identity() / noiseSigma2);
//             optimizer.addEdge(goe2);
//             saytab("stage3");

//           }
//       lastposition = position;

//       Vector3d measurement =gps[i];
//       GPSObservationPosition3DEdge* goe = new GPSObservationPosition3DEdge();
//       goe->setVertex(0, position);
//       goe->setMeasurement(measurement);
//       goe->setInformation(Matrix3d::Identity() / noiseSigma);
//       optimizer.addEdge(goe);

//               // Construct vertex which corresponds to the actual point of the target
//     VertexPosition3D* position = new VertexPosition3D();
//     position->setId(i+1);
//     optimizer.addVertex(position);
//     }

//   // Configure and set things going
//   optimizer.initializeOptimization();
//   optimizer.setVerbose(true);
//   optimizer.optimize(5);

//   cout << "truePoint=\n" << truePoint << endl;

//   cerr <<  "computed estimate=\n" << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate() << endl;

//   //position->setMarginalized(true);

//   SparseBlockMatrix<MatrixXd> spinv;

//   optimizer.computeMarginals(spinv, position);

//   //optimizer.solver()->computeMarginals();

//   // covariance
//   //
//   cout << "covariance\n" << spinv << endl;

//   cout << spinv.block(0,0) << endl;

//   Vector3d v =  dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate();

//        poseop_msgs.header.stamp = ros::Time::now();
//        poseop_msgs.pose.pose.position.x = v[0];
//        poseop_msgs.pose.pose.position.y = v[1];
//        poseop_msgs.pose.pose.position.z = v[2];

//        poseop_msgs.pose.pose.orientation = imu.orientation;

//        sayend("CAL COV");
//       // SparseBlockMatrix<MatrixXd> spinv;

//       // optimizer.computeMarginals(spinv, position);

//       // //optimizer.solver()->computeMarginals();

//       // // covariance
//       // //
//       // cout << "covariance\n" << spinv << endl;

//       // for(int u =0;u<6;u++)
//       //   for(int v=0;v<6;v++)
//       //     poseop_msgs.pose.covariance[6*u+v] = spinv(u, v);
//       poseop_msgs.pose.covariance[0]=0.1;
//       poseop_msgs.pose.covariance[7]=0.1;
//       poseop_msgs.pose.covariance[14]=0.1;
//       poseop_msgs.pose.covariance[21]=0.1;
//       poseop_msgs.pose.covariance[28]=0.1;
//       poseop_msgs.pose.covariance[35]=0.1;
// }

void Optimize_test(double dt) {
 // Set up the parameters of the simulation
  int numberOfTimeSteps = NUM_ITERATE;
  const double processNoiseSigma = 0.05;
  const double accelerometerNoiseSigma = 0.038;
  const double gpsNoiseSigma = 1;
  // const double dt = 1;

  // Set up the optimiser and block solver
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  typedef BlockSolver< BlockSolverTraits<6, 6> > BlockSolver;
  BlockSolver::LinearSolverType * linearSolver
      = new LinearSolverCholmod<BlockSolver::PoseMatrixType>();
  BlockSolver* blockSolver = new BlockSolver(linearSolver);
  OptimizationAlgorithm* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(optimizationAlgorithm);

  // Sample the start location of the target

    state = msf_state[0];

  // Construct the first vertex; this corresponds to the initial
  // condition and register it with the optimiser
  VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();
  stateNode->setEstimate(state);
  stateNode->setId(0);
  optimizer.addVertex(stateNode);

  // Set up last estimate
  VertexPositionVelocity3D* lastStateNode = stateNode;

  // Iterate over the simulation steps
  for (int k = 0; k < numberOfTimeSteps; ++k)
    {
      for(int j = 0; j < Numstate-1; j++)
      {
        saytab(j);saytab(Numstate);saytab(Numimu);sayend(Iterate);

          state = msf_state[j];
          // Construct vertex which corresponds to the current state of the target
          VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();

          stateNode->setId((Numstate-1)*k+j+1);
          stateNode->setEstimate(state);
          stateNode->setMarginalized(false);
          optimizer.addVertex(stateNode);

          sayend("STEP1");
          // Construct the accelerometer measurement
          Vector3d accelerometerMeasurement = acc[j];

          TargetOdometry3DEdge* toe = new TargetOdometry3DEdge(0.01, accelerometerNoiseSigma);
          toe->setVertex(0, lastStateNode);
          toe->setVertex(1, stateNode);
          VertexPositionVelocity3D* vPrev= dynamic_cast<VertexPositionVelocity3D*>(lastStateNode);
          VertexPositionVelocity3D* vCurr= dynamic_cast<VertexPositionVelocity3D*>(stateNode);
          toe->setMeasurement(accelerometerMeasurement);
          optimizer.addEdge(toe);
          sayend("STEP2");
          // compute the initial guess via the odometry
          g2o::OptimizableGraph::VertexSet vPrevSet;
          vPrevSet.insert(vPrev);
          toe->initialEstimate(vPrevSet,vCurr);

          lastStateNode = stateNode;
          sayend("STEP3");
      }

        // Construct the GPS observation
        Vector3d gpsMeasurement = gps[k];

      sayend("STEP21");
      // Add the GPS observation
      GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
      goe->setVertex(0, stateNode);
      optimizer.addEdge(goe);
      sayend("STEP22");
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);
  cerr << "number of vertices:" << optimizer.vertices().size() << endl;
  cerr << "number of edges:" << optimizer.edges().size() << endl;

  // Print the results

  cout << "state=\n" << state << endl;

#if 0
  for (int k = 0; k < numberOfTimeSteps; k++)
    {
      cout << "computed estimate " << k << "\n"
           << dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(k)->second)->estimate() << endl;
       }
#endif

  // Vector6d v1 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-2,0))->second)->estimate();
  // Vector6d v2 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();
  // cout << "v1=\n" << v1 << endl;
  // cout << "v2=\n" << v2 << endl;
  // cout << "delta state=\n" << v2-v1 << endl;

       Vector6d v = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();

       poseop_msgs.header.stamp = ros::Time::now();
       poseop_msgs.pose.pose.position.x = v[0];
       poseop_msgs.pose.pose.position.y = v[1];
       poseop_msgs.pose.pose.position.z = v[2];

       poseop_msgs.pose.pose.orientation = imu.orientation;

      SparseBlockMatrix<MatrixXd> spinv;

      optimizer.computeMarginals(spinv, stateNode);

      //optimizer.solver()->computeMarginals();

      // covariance
      //
      cout << "covariance\n" << spinv << endl;

      // for(int u =0;u<6;u++)
      //   for(int v=0;v<6;v++)
      //     poseop_msgs.pose.covariance[6*u+v] = spinv(u, v);
      poseop_msgs.pose.covariance[0]=0.1;
      poseop_msgs.pose.covariance[7]=0.1;
      poseop_msgs.pose.covariance[14]=0.1;
      poseop_msgs.pose.covariance[21]=0.1;
      poseop_msgs.pose.covariance[28]=0.1;
      poseop_msgs.pose.covariance[35]=0.1;

}


/*
#
# Copyright © 2019 Ronald C. Beavis
# Licensed under Apache License, Version 2.0, January 2004
#
# Loads a spectrum file into a vector of spectrum objects
#
*/
#include "pch.h"

#include "rapidjson/document.h"
#include <fstream>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <sys/stat.h>
#include <unordered_map>
#include <map>
#include <string>
#include <set>
#include <vector>
using namespace std;
#include "load_spectra.hpp"
#include "load_kernel.hpp"
#include "create_results.hpp"
#include "create_output.hpp"
//#include <boost/math/distributions/hypergeometric.hpp>
//#include <boost/math/policies/policy.hpp>

create_output::create_output(void)	{
	low = -20;
	high = 20;
	load_distribution();
}

create_output::~create_output(void)	{
}

long create_output::get_cells(double _pm,long _res)	{
	long pm = 100*(long)(_pm/100000);
	long r = 2;
	if(_res == 50)	{
		r = 1;
	}
	else if(_res == 20)	{
		r = 0;
	}
	if(pm < 800)	{
		return (long)(pm*distribution[800][r]);
	}
	if(pm > 4000)	{
		return (long)(pm*distribution[4000][r]);
	}
	return (long)(pm*distribution[pm][r]);
}

bool create_output::apply_model(long _res,string& _seq,double _pm,long _ions,long _lspectrum,double& pscore,double& p){
	p = 0.0001;
	pscore = 400;
	long sfactor = 20;
	long sadjust = 3;
	if(_res > 100)	{
		sfactor = 40;
	}
	long cells = get_cells(_pm,_res);
	long total_ions = 2*(long)(_seq.size() - 1);
	if(total_ions > sfactor)	{
		total_ions = sfactor;
	}
	if(total_ions < _ions)	{
		total_ions = _ions + 1;
	}
	long sc = _lspectrum * sadjust;
	if(_ions >= sc)	{
		sc = _ions + 2;
	}
//	boost::math::hypergeometric_distribution<double> hyper(sc,total_ions,cells);
//	p = boost::math::pdf<double>(hyper, _ions);
	hypergeom hp(sc,total_ions,cells);
	p = hp.pdf(_ions);
	pscore = -100.0*log(p)/2.3;
	return true;
}

bool create_output::load_mods(void)	{
	ifstream istr;
	istr.open("report_mods.txt");
	if(istr.fail())	{
		return false;
	}
	mt.clear();
	string line;
	while(getline(istr,line))	{
		size_t tab = line.find('\t');
		mt[atoi(line.substr(0,tab).c_str())] = line.substr(tab+1,line.size()-1);
	}
	istr.close();
	return true;
}

bool create_output::find_window(void)	{
	map<long,long> vs;
	long i = 0;
	for(i = -20; i < 21; i++)	{
		vs[i] = 0;
	}
	auto it = ppms.begin();
	long max = -21;
	long center = -1;
	while(it != ppms.end())	{
		i = (long)(0.5 + it->second);
		if(i >= -20 and i <= 20)	{
			vs[i] += 1;
		}
		if(vs[i] > max)	{
			max = vs[i];
			center = i;
		}
	}
	double ic = (double)max;
	if(ic < 100.0)	{
		low = -20;
		high = 20;
		return true;
	}
	low = -20;
	for(i = -20;i < center; i++)	{
		if(vs[i]/ic >= 0.01)	{
			low = i;
			break;
		}
	}
	high = 20;
	for(i = 20; i > center; i--)	{
		if(vs[i]/ic >= 0.01)	{
			high = i;
			break;
		}
	}
	return true;
}

bool create_output::create(map<string,string>& _params,create_results& _cr)	{
	ifstream istr;
	istr.open(_params["kernel file"]);
	if(istr.fail())	{
		return false;
	}
	if(!load_mods())	{
		cout << "Warning (idx:1001): annotation file \"report_mods.txt\" was not present" << endl;
	}
	long k = 0;
	for(size_t j = 0; j < _cr.ids.size(); j++)	{
		sv[_cr.ids[j].sn] = _cr.ids[j];
		for(size_t i = 0; i < _cr.ids[j].ks.size(); i++)	{
			k = _cr.ids[j].ks[i];
			if(sdict.find(k) != sdict.end())	{
				sdict[k].insert(_cr.ids[j].sn);
			}
			else	{
				sdict.insert(pair<long,set<long> >(k,set<long>()));
				sdict[k].insert(_cr.ids[j].sn);
			}
		}
	}

	long res = atoi(_params["fragment tolerance"].c_str());
	long inferred = 0;
	long specs = atoi(_params["spectra"].c_str());
	double score_min = 200.0;
	if(specs > 0)	{
		score_min += 100.0 * log(specs)/2.3;
	}
	double total_prob = 0.0;
	long min_c = 8;
	if(res == 50)	{
		min_c = 7;
	}
	else if(res == 20)	{
		min_c = 6;
	}
	string line;
	using namespace rapidjson;
	long c = 0;
	long h = 0;

	while(getline(istr,line))	{
		if(c != 0 and c % 10000 == 0)	{
			cout << '.';
			cout.flush();
		}
		if(c != 0 and c % 200000 == 0)	{
			cout << " " << c << endl;
			cout.flush();
		}
		c++;
 		Document js;
   		js.Parse(line.c_str(),line.length());
		if(!js.HasMember("pm"))	{
			continue;
		}
		h = (long)js["h"].GetInt();
		if(sdict.find(h) == sdict.end())	{
			continue;
		}
		if(h != (long)js["u"].GetInt())	{
			inferred += 1;
		}
		long last_i = 0;

		double max_prob = 0.0;
		double prob = 0.0;
		double delta = 0.0;
		double pm = 0.0;
		double ppm = 0.0;
		double score = 0.0;
		auto it = sdict[h].begin();
		long s = 0;
		string seq;
		while(it != sdict[h].end())	{
			s = *it;
			it++;
			pm = js["pm"].GetFloat();
			delta = (sv[s].pm-pm)/1000.0;
			ppm = 1.0e6*(sv[s].pm-pm)/pm;
			if(delta > 0.5)	{
				ppm = 1.0e6*(sv[s].pm-c13-pm)/pm;
			}
			seq = js["seq"].GetString();
			apply_model(res,seq,pm,sv[s].peaks,sv[s].ions,score,prob);
			if(score < score_min or sv[s].ri < 0.20 or sv[s].peaks < min_c)	{
				continue;
			}
			if(prob > max_prob)	{
				max_prob = prob;
			}
			ostringstream oline;
			oline << sv[s].sc << "\t" << fixed << setprecision(3) << pm/1000.0 << "\t";
			oline << delta << "\t" << setprecision(1) << ppm << setprecision(3) << "\t";
			oline << sv[s].pz << "\t" << js["lb"].GetString() << "\t";
			oline << js["beg"].GetInt() << "\t" << js["end"].GetInt() << "\t";
			oline << js["pre"].GetString() << "\t" << js["seq"].GetString() << "\t";
			oline << js["post"].GetString() << "\t" << sv[s].peaks << "\t";
			const Value& jbs = js["ns"];
			long lns = 0;
			for(SizeType a = 0; a < jbs.Size();a++)	{
				lns += jbs[a].GetInt();
			}
			if(lns > 0)	{
				oline << sv[s].ri << "\t" << log(lns)/2.3 << "\t" << -0.01*score << "\t";
			}
			else	{
				oline << sv[s].ri << "\t" << "-" << "\t" << -0.01*score << "\t";
			}
			if(js.HasMember("mods"))	{
				const Value& jmods = js["mods"];
				vector<mod> mods;
				mod tmod;
				for(SizeType a = 0; a < jmods.Size();a++)	{
					const Value& lmods = jmods[a];
					tmod.pos = lmods[1].GetInt();
					tmod.res = lmods[0].GetString();
					tmod.mass = lmods[2].GetInt();
					mods.push_back(tmod);
				}
				sort(mods.begin(),mods.end());
				for(size_t a = 0; a < mods.size(); a++)	{
					if(mt.find(mods[a].mass) != mt.end())	{
						oline << mods[a].res << mods[a].pos << "~" << mt.find(mods[a].mass)->second << ";";
					}
					else	{
						oline << mods[a].res << mods[a].pos << "#" << mods[a].mass/1000.0 << ";";
					}
				}
			}
			last_i = s+1;
			if(odict.find(s) != odict.end())	{
				odict[s].push_back(oline.str());
			}
			else	{
				odict.insert(pair<long,vector<string> >(s,vector<string>()));
				odict[s].push_back(oline.str());
			}
			ppms[s] = (long)(0.5+ppm);
		}
		total_prob += max_prob;

	}
	ofstream ofs;
	ofs.open(_params["output file"]);
	string header = "Id\tSub\tScan\tPeptide mass\tDelta\tppm\tz\tProtein acc\t";
	header += "Start\tEnd\tPre\tSequence\tPost\tIC\tRI\tlog(f)\tlog(p)\tModifications";
	ofs << header << endl;
	long ids = 0;
	for(long s = 0; s < odict.size(); s++)	{
		for(long l = 0; l < odict[s].size(); l++)	{
			ofs << ids+1 << "\t" << l+1 << "\t" << odict[s][l] << endl;
		}
		if(!odict[s].empty())	{
			ids++;
		}
	}
	ofs.close();
	cout << endl;
	cout.flush();
	istr.close();
	return true;
}


// test.cpp
// Test program for testing the pipeline / detector against ground truth dataset

#include "io/DatasetLoader.hpp"
#include "detection/FumaroleDetector.hpp"
#include "evaluation/Evaluation.hpp"
#include "evaluation/AlgorithmEvaluator.hpp"

#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <numeric>
#include <boost/filesystem.hpp>

const std::string FOLDER { "test_set_4/" };
const std::string CSV_SAVE_FOLDER { "confusion_matrix" };
const std::string DETECTION_METRICS_SAVE_FOLDER { "detection_metrics" };
const std::string IOU_METRICS_SAVE_FOLDER { "iou_metrics" };
const std::string CONFUSION_MATRIX_FILE_NAME { "classification_confusion_matrix" };

// Eval functions for detector and classifier
void EvaluateDetector(const Evaluation::AlgorithmEvaluation& evaluation);
void EvaluateClassifier(const Evaluation::AlgorithmEvaluation& evaluation);
void SaveConfusionMatrices(const Evaluation::AlgorithmEvaluation& evaluation);
void SaveDetectionsMetrics(const Evaluation::AlgorithmEvaluation& evaluation);

int main(int argc, char** argv)
{
    // Load test files and ground truth
    std::map<std::string, std::string> testFiles;
    std::map<std::string, std::vector<Detection::FumaroleDetection>> groundTruth;
    IO::DatasetLoader::LoadTestData(FOLDER, testFiles, groundTruth);

    // run detection
    std::map<std::string, std::vector<Detection::FumaroleDetection>> results;

    // save the results
    Detection::FumaroleDetector detector(true);
    detector.DetectFumaroles(testFiles, results);
    detector.SaveResults(results);

    // evaluate the results
    std::cout << "\n\nRunning evaluation..." << std::endl;
    Evaluation::AlgorithmEvaluator evaluator;
    Evaluation::AlgorithmEvaluation eval = evaluator.EvaluateDetectionPipeline(results, groundTruth);

    // draw the results vs ground truth for comparison
    std::cout << "\n\nSaving ground truth vs results" << std::endl;
    for (const auto& y : groundTruth) {
        evaluator.DrawDetectionsVsGroundtruth(y.first, FOLDER, results[y.first], y.second);
    }

    // print stats for detector and classifier performance
    EvaluateDetector(eval);
    EvaluateClassifier(eval);

    // save detection metrics to csv files
    SaveDetectionsMetrics(eval);

    // save confusion matrices to csv
    SaveConfusionMatrices(eval);

    std::cout << std::endl;

    return 0;
}

// Evaluate detection
void EvaluateDetector(const Evaluation::AlgorithmEvaluation& evaluation)
{
    const float precision = static_cast<float>(evaluation.TotalNumberDetected) / static_cast<float>(evaluation.TotalNumberOfActualFumaroles) * 100;

    // print out evaluation results
    std::cout << "\n\n--------------- Detector Evaluation ---------------\n";
    std::cout << "\nTotal number of detections = " << evaluation.TotalNumberDetected;
    std::cout << "\nActual number of detections = " << evaluation.TotalNumberOfActualFumaroles;
    std::cout << "\nTotal Average IoU = " << evaluation.TotalAverageIoU;
    std::cout << "\nTotal Accuracy (%) = " << evaluation.TotalAverageIoU * 100;
    std::cout << "\nTotal Precision (%) = " << precision;
    if (precision > 100) {
        std::cout << " (Over detected)";
    }
    std::cout << std::endl;

    // print out individual results
    std::cout << "\n--------------- Image Evaluation ---------------\n";
    std::cout << std::setw(12) << std::setfill(' ') << "\nImage ID";
    std::cout << std::setw(10) << std::setfill(' ') << " ";
    std::cout << std::setw(10) << std::setfill(' ') << "Avg IoU";
    std::cout << std::setw(10) << std::setfill(' ') << " ";
    std::cout << std::setw(10) << std::setfill(' ') << "Accuracy %";
    std::cout << std::setw(10) << std::setfill(' ') << " ";
    std::cout << std::setw(10) << std::setfill(' ') << "Detected";
    std::cout << std::setw(10) << std::setfill(' ') << " ";
    std::cout << std::setw(10) << std::setfill(' ') << "Actual";
    std::cout << std::endl;

    for (const Evaluation::FumaroleDetectionEvaluation& e : evaluation.Evaluations)
    {
        std::cout << std::setw(12) << std::setfill(' ') << e.ImageID;
        std::cout << std::setw(10) << std::setfill(' ') << " ";
        std::cout << std::setw(10) << std::setfill(' ') << e.AverageIoU;
        std::cout << std::setw(10) << std::setfill(' ') << " ";
        std::cout << std::setw(10) << std::setfill(' ') << e.AverageIoU * 100;
        std::cout << std::setw(10) << std::setfill(' ') << " ";
        std::cout << std::setw(10) << std::setfill(' ') << e.NumberDetected;
        std::cout << std::setw(10) << std::setfill(' ') << " ";
        std::cout << std::setw(10) << std::setfill(' ') << e.NumberOfActualFumaroles;

        std::cout << std::endl;
    }
}

// Evaluate classifier
void EvaluateClassifier(const Evaluation::AlgorithmEvaluation& evaluation)
{
    // print out overall classification results
    std::cout << "\n\n--------------- Classifier Evaluation ---------------\n";
    std::cout << "\nOverall Accuracy Rate (%) = " << evaluation.ConfusionMatrix.GetAccuracy() * 100;
    std::cout << "\nOverall Misclassification Rate (%) = " << (1.0 - evaluation.ConfusionMatrix.GetAccuracy()) * 100;
    std::cout << std::endl;

    // print out individual results per image
    std::cout << "\n--------------- Image Evaluation ---------------\n";
    std::cout << std::setw(12) << std::setfill(' ') << "\nImage ID";
    std::cout << std::setw(10) << std::setfill(' ') << " ";
    std::cout << std::setw(10) << std::setfill(' ') << "Accuracy";
    std::cout << std::setw(10) << std::setfill(' ') << " ";
    std::cout << std::setw(10) << std::setfill(' ') << "Misclassification Rate";
    std::cout << std::endl;

    for (const Evaluation::FumaroleDetectionEvaluation& e : evaluation.Evaluations)
    {
        std::cout << std::setw(12) << std::setfill(' ') << e.ImageID;
        std::cout << std::setw(10) << std::setfill(' ') << " ";
        std::cout << std::setw(10) << std::setfill(' ') << e.ConfusionMatrix.GetAccuracy() * 100;
        std::cout << std::setw(10) << std::setfill(' ') << " ";
        std::cout << std::setw(10) << std::setfill(' ') << (1.0 - e.ConfusionMatrix.GetAccuracy()) * 100;

        std::cout << std::endl;
    }
}

// Save confusion matrices as CSVs
void SaveConfusionMatrices(const Evaluation::AlgorithmEvaluation& evaluation)
{
    // create folder if required
    const std::string folderPath = CSV_SAVE_FOLDER + "/" + FOLDER;
    if (!boost::filesystem::exists(folderPath)) {
        boost::filesystem::create_directories(folderPath);
    }

    // save overall algo classification confusion matrix
    std::ofstream fs;
    fs.open(folderPath + CONFUSION_MATRIX_FILE_NAME + ".csv", std::ios::out);
    fs << evaluation.ConfusionMatrix;
    fs.close();

    // save individual confusion matrices
    for (const Evaluation::FumaroleDetectionEvaluation& e : evaluation.Evaluations)
    {
        std::ofstream fs;
        fs.open(folderPath + e.ImageID + ".csv", std::ios::out);
        fs << e.ConfusionMatrix;
        fs.close();
    }
}

// Write detection metrics to CSV files
void SaveDetectionsMetrics(const Evaluation::AlgorithmEvaluation& evaluation)
{
    std::cout << "\nSaving detection metrics for single images" << std::endl;

    // create folder if needed
    if (!boost::filesystem::exists(DETECTION_METRICS_SAVE_FOLDER)) {
        boost::filesystem::create_directories(DETECTION_METRICS_SAVE_FOLDER);
    }

    if (!boost::filesystem::exists(IOU_METRICS_SAVE_FOLDER)) {
        boost::filesystem::create_directories(IOU_METRICS_SAVE_FOLDER);
    }

    std::string filePath;
    for (const Evaluation::FumaroleDetectionEvaluation& e : evaluation.Evaluations)
    {
        // save detection metrics
        filePath = DETECTION_METRICS_SAVE_FOLDER + "/" + e.ImageID + ".csv";
        Evaluation::AlgorithmEvaluator::SaveDetectionEvaluationMetricsToCSV(filePath, e.DetectionMetrics);

        // save IoU metrics
        filePath = IOU_METRICS_SAVE_FOLDER + "/" + e.ImageID + ".csv";
        Evaluation::AlgorithmEvaluator::SaveIoUEvaluationMetricsToCSV(filePath, e.IoUMetrics);
    }

    // save total detection metrics
    std::cout << "\nSaving total detection metrics to CSV file" << std::endl;

    filePath = DETECTION_METRICS_SAVE_FOLDER + "/total_metrics.csv";
    Evaluation::AlgorithmEvaluator::SaveDetectionEvaluationMetricsToCSV(filePath, evaluation.DetectionMetrics);

    filePath = IOU_METRICS_SAVE_FOLDER + "/total_metrics.csv";
    Evaluation::AlgorithmEvaluator::SaveIoUEvaluationMetricsToCSV(filePath, evaluation.IoUDetectionMetrics);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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 BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_COMMON_IS_NOT_EQUAL_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_COMMON_IS_NOT_EQUAL_HPP_INCLUDED

#include <boost/simd/operator/functions/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/logical_not.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/meta/as_arithmetic.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/sizeof.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF         ( is_not_equal_
                                      , boost::simd::tag::cpu_
                                      , (A0)(X)
                                      , (mpl::equal_to< mpl::sizeof_<A0>
                                                      , mpl::sizeof_<typename A0::type>
                                                      >
                                        )
                                      , ((simd_<logical_<A0>,X>))
                                        ((simd_<logical_<A0>,X>))
                                      )
  {
    typedef A0                                                result_type;
    typedef typename meta::as_arithmetic<A0>::type            base_t;
    typedef typename dispatch::meta::as_integer<base_t>::type cast_t;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return bitwise_cast<A0> ( neq( bitwise_cast<cast_t>(a0)
                                   , bitwise_cast<cast_t>(a1)
                                   )
                              );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( is_not_equal_
                                    , boost::simd::tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                      ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return logical_not( a0 == a1 );
    }
  };

} } }

#endif


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

    @Copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_TOFLOAT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_TOFLOAT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/bitwise_notand.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/twoto31.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( tofloat_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                         )
  {
    using result = bd::as_floating_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return _mm_cvtepi32_ps(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( tofloat_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                         )
  {
    using result = bd::as_floating_t<A0>;
    using stype = bd::scalar_of_t<result>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return {static_cast<stype>(a0[0]), static_cast<stype>(a0[1])};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( tofloat_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                         )
  {
    using result = bd::as_floating_t<A0>;
    using stype = bd::scalar_of_t<result>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return {static_cast<stype>(a0[0]), static_cast<stype>(a0[1])};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( tofloat_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                         )
  {
    using result = bd::as_floating_t<A0>;
    BOOST_FORCEINLINE result operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      using si_type = bd::as_integer_t<A0, signed>;
      si_type a00 = bitwise_cast<si_type>(a0);
      si_type a01 = bitwise_notand(Signmask<si_type>(), a0);
      return if_plus(is_ltz(a00), tofloat(a01), Twoto31<result>());
    }
  };

} } }

#endif


//  (C) Copyright John Maddock 2014.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_MP_T
#define BOOST_MATH_TOOLS_MP_T

#ifndef BOOST_MATH_PRECISION
#define BOOST_MATH_PRECISION 1000
#endif

#if defined(BOOST_MATH_USE_MPFR)

#include <boost/multiprecision/mpfr.hpp>

typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<BOOST_MATH_PRECISION *301L / 1000L> > mp_t;

#else

#include <boost/multiprecision/cpp_bin_float.hpp>

typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<BOOST_MATH_PRECISION, boost::multiprecision::digit_base_2> > mp_t;

#endif

inline mp_t ConvPrec(mp_t arg, int digits)
{
   int e1, e2;
   mp_t t = frexp(arg, &e1);
   t = frexp(floor(ldexp(t, digits + 1)), &e2);
   return ldexp(t, e1);
}

inline mp_t relative_error(mp_t a, mp_t b)
{
   mp_t min_val = boost::math::tools::min_value<mp_t>();
   mp_t max_val = boost::math::tools::max_value<mp_t>();

   if((a != 0) && (b != 0))
   {
      // mp_tODO: use isfinite:
      if(fabs(b) >= max_val)
      {
         if(fabs(a) >= max_val)
            return 0;  // one infinity is as good as another!
      }
      // If the result is denormalised, treat all denorms as equivalent:
      if((a < min_val) && (a > 0))
         a = min_val;
      else if((a > -min_val) && (a < 0))
         a = -min_val;
      if((b < min_val) && (b > 0))
         b = min_val;
      else if((b > -min_val) && (b < 0))
         b = -min_val;
      return (std::max)(fabs((a - b) / a), fabs((a - b) / b));
   }

   // Handle special case where one or both are zero:
   if(min_val == 0)
      return fabs(a - b);
   if(fabs(a) < min_val)
      a = min_val;
   if(fabs(b) < min_val)
      b = min_val;
   return (std::max)(fabs((a - b) / a), fabs((a - b) / b));
}

#endif // BOOST_MATH_TOOLS_MP_T