#pragma once
// Verbose because clang/llvm pretend to have gnuc
#if defined(__GNUC__) && !defined(__llvm__) && !defined(__INTEL_COMPILER)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#include <Eigen/Dense>
#if defined(__GNUC__) && !defined(__llvm__) && !defined(__INTEL_COMPILER)
#pragma GCC diagnostic pop
#endif
#include <map>
#include <vector>
namespace mtao {
template <typename T, int A, int B>
using Matrix = Eigen::Matrix<T, A, B>;

template <typename T, int A>
using SquareMatrix = Matrix<T, A, A>;
template <typename T>
using MatrixX = SquareMatrix<T, Eigen::Dynamic>;

template <typename T, int D>
using Vector = Matrix<T, D, 1>;
template <typename T, int D>
using RowVector = Matrix<T, 1, D>;
template <typename T>
using VectorX = Vector<T, Eigen::Dynamic>;
template <typename T>
using RowVectorX = RowVector<T, Eigen::Dynamic>;

template <typename T>
using Vector2 = Vector<T, 2>;
template <typename T>
using RowVector2 = RowVector<T, 2>;

template <typename T>
using Vector3 = Vector<T, 3>;
template <typename T>
using RowVector3 = RowVector<T, 3>;

template <typename T>
using Vector4 = Vector<T, 4>;
template <typename T>
using RowVector4 = RowVector<T, 4>;

template <typename T, int D>
using ColVectors = Matrix<T, D, Eigen::Dynamic>;
template <typename T, int D>
using RowVectors = Matrix<T, Eigen::Dynamic, D>;

using Mat3i = SquareMatrix<int, 3>;
using Mat2i = SquareMatrix<int, 2>;
using ColVecs4i = ColVectors<int, 4>;
using ColVecs3i = ColVectors<int, 3>;
using ColVecs2i = ColVectors<int, 2>;
using RowVecs3i = RowVectors<int, 3>;
using RowVecs2i = RowVectors<int, 2>;
using Vec3i = Vector<int, 3>;
using Vec2i = Vector<int, 2>;
using MatXi = MatrixX<int>;
using VecXi = VectorX<int>;
using RowVecXi = RowVectorX<int>;

using Mat4f = SquareMatrix<float, 4>;
using Mat3f = SquareMatrix<float, 3>;
using Mat2f = SquareMatrix<float, 2>;
using ColVecs4f = ColVectors<float, 4>;
using ColVecs3f = ColVectors<float, 3>;
using ColVecs2f = ColVectors<float, 2>;
using RowVecs3f = RowVectors<float, 3>;
using RowVecs2f = RowVectors<float, 2>;
using Vec4f = Vector<float, 4>;
using Vec3f = Vector<float, 3>;
using Vec2f = Vector<float, 2>;
using MatXf = MatrixX<float>;
using VecXf = VectorX<float>;
using RowVecXf = RowVectorX<float>;

using Mat4d = SquareMatrix<double, 4>;
using Mat3d = SquareMatrix<double, 3>;
using Mat2d = SquareMatrix<double, 2>;
using ColVecs4d = ColVectors<double, 4>;
using ColVecs3d = ColVectors<double, 3>;
using ColVecs2d = ColVectors<double, 2>;
using RowVecs3d = RowVectors<double, 3>;
using RowVecs2d = RowVectors<double, 2>;
using Vec4d = Vector<double, 4>;
using Vec3d = Vector<double, 3>;
using Vec2d = Vector<double, 2>;
using MatXd = MatrixX<double>;
using VecXd = VectorX<double>;
using RowVecXd = RowVectorX<double>;

using Vec2i = Vector2<int>;
using Vec3i = Vector3<int>;
using Vec4i = Vector4<int>;

// types packaged for a given embedded dimensions
template <typename T, int D_>
struct embedded_types {
    using Scalar = T;
    constexpr static int D = D_;
    template <int A, int B>
    using Matrix = Matrix<T, A, B>;
    template <int A>
    using SquareMatrix = SquareMatrix<T, A>;
    template <int A>
    using Vector = Vector<T, A>;
    using VectorD = Vector<D>;
    using SquareMatrixD = SquareMatrix<D>;

    using MatrixX = mtao::MatrixX<T>;
    using VectorX = mtao::VectorX<T>;

    template <int D>
    using ColVectors = Matrix<D, Eigen::Dynamic>;
    template <int D>
    using RowVectors = Matrix<Eigen::Dynamic, D>;

    template <int N>
    using ColVectorsD = Matrix<D, N>;
    template <int N>
    using RowVectorsD = Matrix<N, D>;
    using ColVectorsDX = ColVectorsD<Eigen::Dynamic>;
    using RowVectorsDX = RowVectorsD<Eigen::Dynamic>;
};

template <typename T>
struct scalar_types {
    typedef Eigen::Matrix<T, 2, 1> Vec2;
    typedef Eigen::Matrix<T, 3, 1> Vec3;
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VecX;
    typedef Eigen::Matrix<T, 2, 2> Mat2;
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatX;
};
template <int Dim>
struct dim_types {
    typedef Eigen::Matrix<double, Dim, 1> Vecf;
    typedef Eigen::Matrix<float, Dim, 1> Vecd;
    typedef Eigen::Matrix<int, Dim, 1> Veci;
    typedef Eigen::Matrix<double, Dim, Dim> Matf;
    typedef Eigen::Matrix<float, Dim, Dim> Matd;
};
template <typename T, int Dim>
struct numerical_types {
    typedef Eigen::Matrix<T, Dim, 1> Vec;
    typedef Eigen::Matrix<T, Dim, Dim> Mat;
};

namespace internal {
template <typename T>
struct allocator_selector {
    using type = std::allocator<T>;
};

template <typename T, int R, int C>
struct allocator_selector<Eigen::Matrix<T, R, C>> {
    using type = Eigen::aligned_allocator<Eigen::Matrix<T, R, C>>;
};
}  // namespace internal

template <typename T>
using allocator = typename internal::allocator_selector<T>::type;

template <typename T, typename Allocator = mtao::allocator<T>>
using vector = std::vector<T, Allocator>;

template <typename Key, typename T, typename Compare = std::less<Key>,
          typename Allocator = mtao::allocator<std::pair<const Key, T>>>
using map = std::map<Key, T, Compare, Allocator>;

template <typename T>
struct scalar_type {
    using type = T;
};

template <typename T, int R, int C>
struct scalar_type<Eigen::Matrix<T, R, C>> {
    using type = T;
};

template <typename T>
using scalar_type_t = typename scalar_type<T>::type;

}  // namespace mtao


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIPKE_HPP_INCLUDED
#define NT2_TOOLBOX_ELLIPTIC_FUNCTION_SCALAR_ELLIPKE_HPP_INCLUDED
#include <boost/fusion/tuple.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/ldexp.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/average.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/toolbox/trigonometric/include/constants.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/infinites.hpp>
#include <nt2/sdk/constant/eps_related.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellipke_, tag::cpu_,
                      (A0),
                      (arithmetic_<A0>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellipke_(tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
      {
	typedef typename std::tr1::result_of<meta::floating(A0)>::type etype;
	typedef boost::fusion::tuple<etype, etype>                   type;
      };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type type;
      return ellipke(type(a0), Eps<type>());
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellipke_, tag::cpu_,
                      (A0)(A1),
                      (arithmetic_<A0>)
                      (arithmetic_<A1>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellipke_(tag::arithmetic_,
			    tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>
    struct result<This(A0, A1)>
    {
      typedef typename meta::strip<A0>::type          etype;
      typedef boost::fusion::tuple<etype, etype>       type;
    };

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename std::tr1::result_of<meta::floating(A0)>::type type;
      return ellipke(type(a0), type(a1));
    }
  };
} }
/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ellipke_, tag::cpu_,
                      (A0)(A1),
                      (real_<A0>)
                      (real_<A1>)
                     )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ellipke_(tag::real_,
			    tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0, class A1>
    struct result<This(A0, A1)>
    {
      typedef typename meta::strip<A0>::type          etype;
      typedef boost::fusion::tuple<etype, etype>       type;
    };

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename NT2_RETURN_TYPE(2)::type type;
      type res;
      if (is_ltz(a0) || gt(a0, One<A0>()))
      {
	boost::fusion::at_c<0>(res) = Nan<A0>();
	boost::fusion::at_c<1>(res) = Nan<A0>();
	return res;
      }
      A0 m = a0;
      A0 aa0 = One<A0>();;
      A0 bb0 = sqrt(oneminus(m));
      A0 s0 = m;
      int32_t i1 = 0;
      A0 mm = 1;
      A0 aa1;
      while (mm > a1) {
	aa1 = average(aa0, bb0);
	A0 bb1 = sqrt(aa0*bb0);
	A0 cc1 = average(aa0, -bb0);
	i1++;
	mm = ldexp(sqr(cc1), i1);
	s0 += mm;
	aa0 = aa1;
	bb0 = bb1;
      };
      if (is_equal(m, One<A0>()))
	{
	  boost::fusion::at_c<0>(res) = One<A0>();
	  boost::fusion::at_c<0>(res) = Inf<A0>();
	}
      else
	{
	  boost::fusion::at_c<0>(res) = nt2::Pio_2<A0>()/aa1;
	  boost::fusion::at_c<1>(res) = boost::fusion::at_c<0>(res)*(One<A0>()-s0*Half<A0>());
	}
      return res;
    }
  };
} }

#endif


// vi: set et ts=4 sw=2 sts=2:

#ifndef HMAT_HMATRIX_DATA_HPP
#define HMAT_HMATRIX_DATA_HPP

#include "common.hpp"
#include <armadillo>
#include "scalar_traits.hpp"

namespace hmat {

template <typename ValueType> class HMatrixData {
public:
  virtual void apply(const arma::Mat<ValueType> &X, arma::Mat<ValueType> &Y,
                     TransposeMode trans, ValueType alpha,
                     ValueType beta) const = 0;

  virtual void apply(const arma::subview<ValueType> &X,
                     arma::subview<ValueType> &Y, TransposeMode trans,
                     ValueType alpha, ValueType beta) const = 0;

  virtual int rows() const = 0;
  virtual int cols() const = 0;
  virtual int rank() const = 0;

  virtual typename ScalarTraits<ValueType>::RealType frobeniusNorm() const = 0;

  virtual double memSizeKb() const = 0;
};
}

#endif


#include <math.h>
#include <vector>
#include <deque>

#include <Eigen/Core>

#include <ros/ros.h>

#include <common/Odometry.h>
#include <common/Pose2DWithCovariance.h>
#include <common/OdometryBuffer.h>

#include <geometry_msgs/Twist.h>

// JS: David, I guess this file is the result of a copypaste from HW1.
// There are many things not needed, and I would like to use the logic explained in the Drive document
// Therefore I have put many messages here, with indications.
// Please feel free to consult me

double vx, vy, vth;

// Tuning constants:
double k_d_d = 0.1, k_r_d = 0.1, k_r_r = 0.1; // TODO migrate to rosparams

//, Delta_x, Delta_y, Delta_th; // JS: We do not need Delta
std::deque<common::Odometry> buffer_odom; // JS: changed vector --> deque for an efficient pop_front().

common::Pose2DWithCovariance create_Pose2DWithCovariance_msg(double x, double y, double th, Eigen::MatrixXd m) {
  common::Pose2DWithCovariance output;

  output.pose.x = x;
  output.pose.y = y;
  output.pose.theta = th;

  for(int i = 0; i < m.rows(); i++) {
    for(int j = 0; j < m.cols(); j++) {
      output.covariance[( i * m.rows() ) + j] = m(i, j);
    }
  }

  return output;
}

// JS: This method is used by more than one node.
//    -  It should be placed in package 'common'
//    -  and also renamed to 'between(pose1, pose2)'
//    -  see the document in Drive !!
common::Pose2DWithCovariance pose_transform(common::Pose2DWithCovariance start_pose,
					    common::Pose2DWithCovariance end_pose) {
  common::Pose2DWithCovariance transform;
  double t_start_th = start_pose.pose.theta;
  double t_end_th = end_pose.pose.theta;
  double cos_th = cos ( t_start_th );
  double sin_th = sin ( t_start_th );
  double dx = end_pose.pose.x - start_pose.pose.x;
  double dy = end_pose.pose.y - start_pose.pose.y;
  double dth = t_end_th - t_start_th;
  dth = std::fmod( dth + M_PI, 2 * M_PI ) - M_PI;
  transform.pose.x = ( cos_th * dx ) + ( sin_th * dy );
  transform.pose.y = ( -1 * sin_th * dx ) + ( cos_th * dy );
  transform.pose.theta = dth;

  return transform;

}

void vel_callback(const geometry_msgs::Twist::ConstPtr& input) {
  vx = input->linear.x;
  vy = input->linear.y;
  vth = input->angular.z;
}

void add_to_buffer(common::Odometry input) {
  if(buffer_odom.size() > 1000) {
    // buffer_odom.clear(); // JS: clear() is not a good policy. Use a deque for the buffer and use pop_front() instead.
      buffer_odom.pop_front();
  }

  buffer_odom.push_back(input);
}

// JS: Rename and adjust API to 'between(keyframe , keyframe)' as in the document
// JS: or, otherwise, just leave as a buffer request, in such case return only one odometry instance, not the increment between two instances.
bool odometry_buffer_request(common::OdometryBuffer::Request &req, common::OdometryBuffer::Response &res) {
  int buffer_search_position = 0;
  int t_start_buffer_position = 0;
  int t_end_buffer_position = 0;
  int t_start = (int) req.t_start.toSec();
  int t_end = (int) req.t_end.toSec();
  bool t_start_found = false;
  bool t_end_found = false;
  std::vector<common::Odometry> odometry_buffer_frozen = buffer_odom;

  for(int i = 0; i < buffer_odom.size(); i++) {
    buffer_search_position = (int) odometry_buffer_frozen[i].ts.toSec();

    if(buffer_search_position == t_start) {
      t_start_buffer_position = i;
      t_start_found = true;
    }

    if(buffer_search_position == t_end) {
      t_end_buffer_position = i;
      t_end_found = true;
    }
  }

  if(t_start_found && t_end_found) {
    common::Pose2DWithCovariance t_start_pose = odometry_buffer_frozen[t_start_buffer_position].pose;
    common::Pose2DWithCovariance t_end_pose = odometry_buffer_frozen[t_end_buffer_position].pose;
    res.delta = pose_transform(t_start_pose, t_end_pose); // JS: we want one pose returned, not the pose increment
    return true;
  }

  return false;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "odometry");
  ros::NodeHandle n;

  ros::Subscriber vel_sub = n.subscribe("/cmd_vel", 1, vel_callback);

  ros::Publisher odom_pub = n.advertise<common::Odometry>("/odometry/odometry", 1);

  ros::Time current_time = ros::Time::now();
  ros::Time last_time = current_time;
  ros::Rate loop_rate(100);

  vx = 0.0;
  vy = 0.0;
  vth = 0.0;

  // JS: we are not using Delta
//  Delta_x = 0.0;
//  Delta_y = 0.0;
//  Delta_th = 0.0;

  while(ros::ok()) {
    current_time = ros::Time::now();
    double delta_t = ( current_time - last_time ).toSec();
    double delta_x = vx * delta_t;
    double delta_y = vy * delta_t;
    double delta_th = vth * delta_t;

    // JS: we are not using Delta
//    double Delta_th_cos = cos ( Delta_th );
//    double Delta_th_sin = sin ( Delta_th );

    // JS: we are not using Delta
//    Delta_x += ( delta_x * Delta_th_cos ) - ( delta_y * Delta_th_sin );
//    Delta_y += ( delta_y * Delta_th_sin ) + ( delta_y * Delta_th_cos );
//    Delta_th += delta_th;
//    Delta_th = std::fmod( Delta_th + M_PI, 2 * M_PI ) - M_PI;

    // JS: we are not integrating covariances through Jacobians, but through constants k_d_d, k_r_d and k_r_r.
//    Eigen::Matrix3d J_D_D = Eigen::Matrix3d::Zero(3, 3);
//    J_D_D(0, 0) = 1;
//    J_D_D(0, 2) = ( -1 * delta_x * Delta_th_sin ) - ( delta_y * Delta_th_cos );
//    J_D_D(1, 1) = 1;
//    J_D_D(1, 2) = ( delta_x * Delta_th_cos ) - ( delta_y * Delta_th_sin );
//    J_D_D(2, 2) = 1;

    // JS: we are not integrating covariances through Jacobians, but through constants k_d_d, k_r_d and k_r_r.
//    Eigen::Matrix3d J_D_d = Eigen::Matrix3d::Zero(3, 3);
//    J_D_d(0, 0) = Delta_th_cos;
//    J_D_d(0, 1) = -1 * Delta_th_sin;
//    J_D_d(1, 0) = Delta_th_sin;
//    J_D_d(1, 1) = Delta_th_cos;
//    J_D_d(2, 2) = 1;

    // JS: we are not publishing delta
//    common::Odometry delta_odom;
//    delta_odom.ts = current_time;
//    delta_odom.pose = create_Pose2DWithCovariance_msg(delta_x, delta_y, delta_th, J_D_d);

    // JS: we are not using Delta
//    common::Odometry Delta_odom;
//    Delta_odom.ts = current_time;
//    Delta_odom.pose = create_Pose2DWithCovariance_msg(Delta_x, Delta_y, Delta_th, J_D_D);

    // JS: we need to integrate the pose, of type Pose2DWithCovariance:
    // JS: we need to create a Odometry message with the integrated pose and the timestamp, consult the Drive document
    // JS: we need to publish the last pose, of type Pose2D, with (x, y, th)
    // JS: proceed as follows (consult the Drive doc, and use compose() for the integration of 'delta' onto the pose (x,y,th).
    //   - Integrate pose (below a copypaste from HW1):
    //    //### Integrate Global Pose ###
    //    x  += delta_x * cos ( th ) - delta_y * sin ( th );
    //    y  += delta_x * sin ( th ) + delta_y * cos ( th );
    //    th += delta_th;
    //    th  = std::fmod( th + M_PI, 2 * M_PI) - M_PI;
    //   - Compute covariance, using k_d_d, k_r_d, and k_r_r as in scanner.cpp
    //   - Construct odometry message
    //   - Bufferize odometry message
    //   - Publish Pose2D(x,y,z)

    // JS: we are not buffering 'delta'
//    add_to_buffer(delta_odom);

    //JS: we are not publishing 'Delta'
//    odom_pub.publish(Delta_odom);

    ros::spinOnce();
    loop_rate.sleep();
  }

  ros::spin();
  return 0;
}


//
// GraphTools library
// Copyright 2017-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Sai Chen <schen6@illumina.com>,
//         Egor Dolzhenko <edolzhenko@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#pragma once

#include <boost/math/distributions/normal.hpp>
#include <cstdint>

using boost::math::normal_distribution;

class DepthTest
{
public:
    /**
     * Set up depth test
     * @param expectedNumReads The mean number of reads when the coverage is as expected
     * @param stdDeviation The standard deviation of the number of reads
     * @param lowerSignificanceThreshold P value cutoff at the lower tail of the distribution
     * @param upperSignificanceThreshold P value cutoff at the upper tail of the distribution
     */
    DepthTest(
        int32_t expectedNumReads, double stdDeviation, double lowerSignificanceThreshold,
        double upperSignificanceThreshold);

    /**
     * Given observed number of reads, return true if pass depth test
     */
    bool testReadCount(int32_t observedNumReads);

private:
    int32_t expectedNumReads_;
    double stdDeviation_;
    double lowerSignificanceThreshold_;
    double upperSignificanceThreshold_;
    const normal_distribution<> coverageDistribution_;
};


#include <boost/accumulators/statistics/sum_kahan.hpp>


#include "rbfcore.h"
#include "utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <unordered_map>
#include <ctime>
#include <chrono>
#include <iomanip>
#include <algorithm>
#include <queue>
#include "readers.h"
//#include "mymesh/UnionFind.h"
//#include "mymesh/tinyply.h"

typedef std::chrono::high_resolution_clock Clock;

double randomdouble() { return static_cast <double> (rand()) / static_cast <double> (RAND_MAX); }

double randomdouble(double be, double ed) { return be + randomdouble() * (ed - be); }

void RBF_Core::NormalRecification(double maxlen, vector<double> &nors) {

    double maxlen_r = -1;
    auto p_vn = nors.data();
    int np = nors.size() / 3;
    if (1) {
        for (int i = 0; i < np; ++i) {
            maxlen_r = max(maxlen_r, MyUtility::normVec(p_vn + i * 3));
        }

        cout << "maxlen_r: " << maxlen_r << endl;
        double ratio = maxlen / maxlen_r;
        for (auto &a:nors)a *= ratio;
    } else {
        for (int i = 0; i < np; ++i) {
            MyUtility::normalize(p_vn + i * 3);
        }

    }

}

bool RBF_Core::Write_Hermite_NormalPrediction(string fname, int mode) {

//    vector<uchar>labelcolor(npt*4);
//    vector<uint>f2v;
//    uchar red[] = {255,0,0, 255};
//    uchar green[] = {0,255,0, 255};
//    uchar blue[] = {0,0,255, 255};
//    for(int i=0;i<labels.size();++i){
//        uchar *pcolor;
//        if(labels[i]==0)pcolor = green;
//        else if(labels[i]==-1)pcolor = blue;
//        else if(labels[i]==1)pcolor = red;
//        for(int j=0;j<4;++j)labelcolor[i*4+j] = pcolor[j];
//    }
    //fname += mp_RBF_METHOD[curMethod];

//    for(int i=0;i<npt;++i){
//        uchar *pcolor = green;
//        for(int j=0;j<4;++j)labelcolor[i*4+j] = pcolor[j];
//    }

    vector<double> nors;
    if (mode == 0)nors = initnormals;
    else if (mode == 1)nors = newnormals;
    else if (mode == 2)nors = initnormals_uninorm;
    NormalRecification(1., nors);

    //for(int i=0;i<npt;++i)if(randomdouble()<0.5)MyUtility::negVec(nors.data()+i*3);
    //cout<<pts.size()<<' '<<f2v.size()<<' '<<nors.size()<<' '<<labelcolor.size()<<endl;
    //writePLYFile(fname,pts,f2v,nors,labelcolor);

//    writeObjFile_vn(fname,pts,nors);
    writePLYFile_VN(fname, pts, nors);

    return 1;
}

void RBF_Core::Set_HermiteRBF(vector<double> &pts) {
    int n = pts.size() / 3;
    cout << "Set_HermiteRBF" << endl;

    isHermite = true;

    M.set_size(n * 4, n * 4);
    double *p_pts = pts.data();
    for (int i = 0; i < n; ++i) {
        for (int j = i; j < n; ++j) {
            M(i, j) = M(j, i) = Kernal_Function_2p(p_pts + i * 3, p_pts + j * 3);
        }
    }

    double G[3];
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < n; ++j) {
            Kernal_Gradient_Function_2p(p_pts + i * 3, p_pts + j * 3, G);
            for (int k = 0; k < 3; ++k)
                M(n + j * 3 + k, i) = M(i, n + j * 3 + k) = G[k];
        }
    }

    double H[9];
    for (int i = 0; i < n; ++i) {
        for (int j = i; j < n; ++j) {
            Kernal_Hessian_Function_2p(p_pts + i * 3, p_pts + j * 3, H);
            for (int k = 0; k < 3; ++k)
                for (int l = 0; l < 3; ++l)
                    M(n + j * 3 + l, n + i * 3 + k)
                            = M(n + i * 3 + k, n + j * 3 + l)
                            = -H[k * 3 + l];
        }
    }

    //cout<<std::setprecision(5)<<std::fixed<<M<<endl;

    bsize = 4;
    N.zeros(n * 4, 4);

    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < 3; ++j)
            N(i, j + 1) = pts[i * 3 + j];
        N(i, 0) = 1;
    }
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < 3; ++j)
            N(n + i * 3 + j, j + 1) = -1;
    }

    //cout<<N<<endl;
    //arma::vec eigval = eig_sym( M ) ;
    //cout<<eigval.t()<<endl;

//   ` if (!isnewformula) {
//        cout << "start solve M: " << endl;
//        auto t1 = Clock::now();
//        if (isinv)
//            Minv = inv(M);
//        else {
//            arma::mat Eye;
//            Eye.eye(npt * 4, npt * 4);
//            Minv = solve(M, Eye);
//        }
//        cout << "solved M: " << (invM_time = std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9) << endl;
//
//        t1 = Clock::now();
//        if (isinv)
//            bprey = inv_sympd(N.t() * Minv * N) * N.t() * Minv;
//        else {
//            arma::mat Eye2;
//            Eye2.eye(bsize, bsize);
//            bprey = solve(N.t() * Minv * N, Eye2) * N.t() * Minv;
//        }
//        cout << "solved bprey " << std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9 << endl;
//    } else {
//
//
//    }`
}

double Gaussian_2p(const double *p1, const double *p2, double sigma) {

    return exp(-MyUtility::vecSquareDist(p1, p2) / (2 * sigma * sigma));
}

void RBF_Core::Set_Actual_User_LSCoef(double user_ls) {

    User_Lambda = User_Lamnbda_inject = user_ls > 0 ? user_ls : 0;

}

void RBF_Core::Set_Actual_Hermite_LSCoef(double hermite_ls) {

    ls_coef = Hermite_ls_weight_inject = hermite_ls > 0 ? hermite_ls : 0;
}

void RBF_Core::Set_SparsePara(double spa) {
    sparse_para = spa;
}

void RBF_Core::Set_User_Lamnda_ToMatrix(double user_ls) {
    {
        Set_Actual_User_LSCoef(user_ls);
        auto t1 = Clock::now();
        cout << "setting J, User_Lambda" << endl;
        if (User_Lambda > 0) {
            arma::sp_mat eye;
            eye.eye(npt, npt);

            dI = inv(eye + User_Lambda * J00);
            saveJ_finalH = J = J11 - (User_Lambda) * (J01.t() * dI * J01);
        } else saveJ_finalH = J = J11;
        cout << "solved: " << (std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9) << endl;
    }
    finalH = saveJ_finalH;
}

void RBF_Core::Set_HermiteApprox_Lambda(double hermite_ls) {

    {
        Set_Actual_Hermite_LSCoef(hermite_ls);
        auto t1 = Clock::now();
        cout << "setting J, HermiteApprox_Lambda" << endl;
        if (ls_coef > 0) {
            arma::sp_mat eye;
            eye.eye(npt, npt);
            if (ls_coef > 0) {
                arma::mat tmpdI = inv(eye + (ls_coef + User_Lambda) * J00);
                J = J11 - (ls_coef + User_Lambda) * (J01.t() * tmpdI * J01);
            } else {
                J = saveJ_finalH;
            }
        }
        cout << "solved: " << (std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9) << endl;
    }

}

void RBF_Core::Set_Hermite_PredictNormal(vector<double> &pts) {
    Set_HermiteRBF(pts);

    auto t1 = Clock::now();
    cout << "setting J" << endl;

    if (!isnewformula) {
        arma::mat D = N.t() * Minv;
        J = Minv - D.t() * inv(D * N) * D;
        J = J.submat(npt, npt, npt * 4 - 1, npt * 4 - 1);
        finalH = saveJ_finalH = J;

    } else {
        cout << "using new formula" << endl;
        A.zeros((npt + 1) * 4, (npt + 1) * 4);
        A.submat(0, 0, npt * 4 - 1, npt * 4 - 1) = M;
        A.submat(0, npt * 4, (npt) * 4 - 1, (npt + 1) * 4 - 1) = N;
        A.submat(npt * 4, 0, (npt + 1) * 4 - 1, (npt) * 4 - 1) = N.t();

        //for(int i=0;i<4;++i)A(i+(npt)*4,i+(npt)*4) = 1;

        auto t2 = Clock::now();
        Ainv = inv(A);

        cout << "Ainv: " << (setK_time = std::chrono::nanoseconds(Clock::now() - t2).count() / 1e9) << endl;
        for (int i = 0; i < 10; i++) {
            for (int j = 0; j < 10; j++) {
                cout << setprecision(3) << Ainv(i, j) << " ";
            }
            cout << endl;
        }

        A.clear();
        Minv = Ainv.submat(0, 0, npt * 4 - 1, npt * 4 - 1);
        Ninv = Ainv.submat(0, npt * 4, (npt) * 4 - 1, (npt + 1) * 4 - 1);

//        Ainv.clear();
        //J = Minv - Ninv *(N.t()*Minv);
        J = Minv;
        J00 = J.submat(0, 0, npt - 1, npt - 1);
        J01 = J.submat(0, npt, npt - 1, npt * 4 - 1);
        J11 = J.submat(npt, npt, npt * 4 - 1, npt * 4 - 1);

        M.clear();
        N.clear();
        cout << "J11: " << J11.n_cols << endl;

        //Set_Hermite_DesignedCurve();

        Set_User_Lamnda_ToMatrix(User_Lamnbda_inject);

//		arma::vec eigval, ny;
//		arma::mat eigvec;
//		ny = eig_sym( eigval, eigvec, J);
//		cout<<ny<<endl;

        cout << "J: " << J.n_cols << endl;
    }

    //J = ( J.t() + J )/2;
    cout << "solve J total: " << (setK_time = std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9) << endl;
    return;

}

void RBF_Core::SetInitnormal_Uninorm() {

    initnormals_uninorm = initnormals;
    for (int i = 0; i < npt; ++i)
        MyUtility::normalize(initnormals_uninorm.data() + i * 3);

}

int RBF_Core::Solve_Hermite_PredictNormal_UnitNorm() {

    arma::vec eigval, ny;
    arma::mat eigvec;

    if (!isuse_sparse) {
        ny = eig_sym(eigval, eigvec, J);
    } else {
//		cout<<"use sparse eigen"<<endl;
//        int k = 4;
//        do{
//            ny = eigs_sym( eigval, eigvec, sp_K, k, "sa" );
//            k+=4;
//        }while(ny(0)==0);
    }

    cout << "eigval(0): " << eigval(0) << endl;

    int smalleig = 0;

    initnormals.resize(npt * 3);
    arma::vec y(npt * 4);
    for (int i = 0; i < npt; ++i)
        y(i) = 0;
    for (int i = 0; i < npt * 3; ++i)
        y(i + npt) = eigvec(i, smalleig);
    for (int i = 0; i < npt; ++i) {
        initnormals[i * 3] = y(npt + i * 3);
        initnormals[i * 3 + 1] = y(npt + i * 3 + 1);
        initnormals[i * 3 + 2] = y(npt + i * 3 + 2);
        //MyUtility::normalize(normals.data()+i*3);
    }

    SetInitnormal_Uninorm();
    cout << "Solve_Hermite_PredictNormal_UnitNorm finish" << endl;
    return 1;
}

/***************************************************************************************************/
/***************************************************************************************************/
double acc_time;

static int countopt = 0;

double optfunc_Hermite(const vector<double> &x, vector<double> &grad, void *fdata) {

    auto t1 = Clock::now();
    RBF_Core *drbf = reinterpret_cast<RBF_Core *>(fdata);
    int n = drbf->npt;
    arma::vec arma_x(n * 3);

    //(  sin(a)cos(b), sin(a)sin(b), cos(a)  )  a =>[0, pi], b => [-pi, pi];
    vector<double> sina_cosa_sinb_cosb(n * 4);
    for (int i = 0; i < n; ++i) {
        int ind = i * 4;
        sina_cosa_sinb_cosb[ind] = sin(x[i * 2]);
        sina_cosa_sinb_cosb[ind + 1] = cos(x[i * 2]);
        sina_cosa_sinb_cosb[ind + 2] = sin(x[i * 2 + 1]);
        sina_cosa_sinb_cosb[ind + 3] = cos(x[i * 2 + 1]);
    }

    for (int i = 0; i < n; ++i) {
        auto p_scsc = sina_cosa_sinb_cosb.data() + i * 4;
        //        int ind = i*3;
        //        arma_x(ind) = p_scsc[0] * p_scsc[3];
        //        arma_x(ind+1) = p_scsc[0] * p_scsc[2];
        //        arma_x(ind+2) = p_scsc[1];
        arma_x(i * 3) = p_scsc[0] * p_scsc[3];
        arma_x(i * 3 + 1) = p_scsc[0] * p_scsc[2];
        arma_x(i * 3 + 2) = p_scsc[1];
    }

    arma::vec a2;
    //if(drbf->isuse_sparse)a2 = drbf->sp_H * arma_x;
    //else
    a2 = drbf->finalH * arma_x;

    if (!grad.empty()) {
        grad.resize(n * 2);

        for (int i = 0; i < n; ++i) {
            auto p_scsc = sina_cosa_sinb_cosb.data() + i * 4;
            //            int ind = i*3;
            //            grad[i*2] = a2(ind) * p_scsc[1] * p_scsc[3] + a2(ind+1) * p_scsc[1] * p_scsc[2] - a2(ind+2) * p_scsc[0];
            //            grad[i*2+1] = -a2(ind) * p_scsc[0] * p_scsc[2] + a2(ind+1) * p_scsc[0] * p_scsc[3];
            grad[i * 2] = a2(i * 3) * p_scsc[1] * p_scsc[3]
                          + a2(i * 3 + 1) * p_scsc[1] * p_scsc[2]
                          - a2(i * 3 + 2) * p_scsc[0];
            grad[i * 2 + 1] = -a2(i * 3) * p_scsc[0] * p_scsc[2]
                              + a2(i * 3 + 1) * p_scsc[0] * p_scsc[3];

        }
    }

    double re = arma::dot(arma_x, a2);
    countopt++;

    acc_time += (std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9);

    //cout<<countopt++<<' '<<re<<endl;
    return re;

}

int RBF_Core::Opt_Hermite_PredictNormal_UnitNormal() {
    sol.solveval.resize(npt * 2);
    for (int i = 0; i < npt; ++i) {
        double *veccc = initnormals.data() + i * 3;
        {
            //MyUtility::normalize(veccc);
            sol.solveval[i * 2] = atan2(sqrt(veccc[0] * veccc[0] + veccc[1] * veccc[1]), veccc[2]);
            sol.solveval[i * 2 + 1] = atan2(veccc[1], veccc[0]);
        }
    }
    //cout<<"smallvec: "<<smallvec<<endl;

    if (1) {
        vector<double> upper(npt * 2);
        vector<double> lower(npt * 2);
        for (int i = 0; i < npt; ++i) {
            upper[i * 2] = 2 * my_PI;
            upper[i * 2 + 1] = 2 * my_PI;

            lower[i * 2] = -2 * my_PI;
            lower[i * 2 + 1] = -2 * my_PI;
        }

        countopt = 0;
        acc_time = 0;

        //LocalIterativeSolver(sol,kk==0?normals:newnormals,300,1e-7);
        Solver::nloptwrapper(lower, upper, optfunc_Hermite, this, 1e-7, 3000, sol);
        cout << "number of call: " << countopt << " t: " << acc_time << " ave: " << acc_time / countopt << endl;
        callfunc_time = acc_time;
        solve_time = sol.time;
        //for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;

    }
    newnormals.resize(npt * 3);
    arma::vec y(npt * 4);
    for (int i = 0; i < npt; ++i)
        y(i) = 0;
    for (int i = 0; i < npt; ++i) {
        double a = sol.solveval[i * 2], b = sol.solveval[i * 2 + 1];
        newnormals[i * 3] = y(npt + i * 3) = sin(a) * cos(b);
        newnormals[i * 3 + 1] = y(npt + i * 3 + 1) = sin(a) * sin(b);
        newnormals[i * 3 + 2] = y(npt + i * 3 + 2) = cos(a);
        MyUtility::normalize(newnormals.data() + i * 3);
    }

    Set_RBFCoef(y);

    //sol.energy = arma::dot(a,M*a);
    cout << "Opt_Hermite_PredictNormal_UnitNormal" << endl;
    return 1;
}

void RBF_Core::Set_RBFCoef(arma::vec &y) {
    cout << "Set_RBFCoef" << endl;
    if (curMethod == HandCraft) {
        cout << "HandCraft, not RBF" << endl;
        return;
    }
    if (!isnewformula) {
        b = bprey * y;
        a = Minv * (y - N * b);
    } else {
        if (User_Lambda > 0)
            y.subvec(0, npt - 1) = -User_Lambda * dI * J01 * y.subvec(npt, npt * 4 - 1);
        a = Minv * y;
        b = Ninv.t() * y;
    }
}

int RBF_Core::Lambda_Search_GlobalEigen() {
//    vector<double> lambda_list({0.01});
    vector<double> lambda_list({0.001, 0.01, 0.1, 1});
    //vector<double>lambda_list({  0.5,0.6,0.7,0.8,0.9,1,1.1,1.5,2,3});
    //lambda_list.clear();
    //for(double i=1.5;i<2.5;i+=0.1)lambda_list.push_back(i);
    //vector<double>lambda_list({0});
    vector<double> initen_list(lambda_list.size());
    vector<double> finalen_list(lambda_list.size());
    vector<vector<double>> init_normallist;
    vector<vector<double>> opt_normallist;

    lamnbda_list_sa = lambda_list;
    for (int i = 0; i < lambda_list.size(); ++i) {
        Set_HermiteApprox_Lambda(lambda_list[i]);
        if (curMethod == Hermite_UnitNormal) {
            Solve_Hermite_PredictNormal_UnitNorm();
        }
        //Solve_Hermite_PredictNormal_UnitNorm();
        OptNormal(1);

        initen_list[i] = sol.init_energy;
        finalen_list[i] = sol.energy;

        init_normallist.emplace_back(initnormals);
        opt_normallist.emplace_back(newnormals);
    }

    lamnbdaGlobal_Be.emplace_back(initen_list);
    lamnbdaGlobal_Ed.emplace_back(finalen_list);

    cout << std::setprecision(8);
    for (int i = 0; i < initen_list.size(); ++i) {
        cout << lambda_list[i] << ": " << initen_list[i] << " -> " << finalen_list[i] << endl;
    }

    int minind = min_element(finalen_list.begin(), finalen_list.end()) - finalen_list.begin();
    cout << "min energy: " << endl;
    cout << lambda_list[minind] << ": " << initen_list[minind] << " -> " << finalen_list[minind] << endl;

    initnormals = init_normallist[minind];
    SetInitnormal_Uninorm();
    newnormals = opt_normallist[minind];
    return 1;
}

void RBF_Core::Print_LamnbdaSearchTest(string fname) {

    cout << setprecision(7);
    cout << "Print_LamnbdaSearchTest" << endl;
    for (int i = 0; i < lamnbda_list_sa.size(); ++i)cout << lamnbda_list_sa[i] << ' ';
    cout << endl;
    cout << lamnbdaGlobal_Be.size() << endl;
    for (int i = 0; i < lamnbdaGlobal_Be.size(); ++i) {
        for (int j = 0; j < lamnbdaGlobal_Be[i].size(); ++j) {
            cout << lamnbdaGlobal_Be[i][j] << "\t" << lamnbdaGlobal_Ed[i][j] << "\t";
        }
        cout << gtBe[i] << "\t" << gtEd[i] << endl;
    }

    ofstream fout(fname);
    fout << setprecision(7);
    if (!fout.fail()) {
        for (int i = 0; i < lamnbda_list_sa.size(); ++i)fout << lamnbda_list_sa[i] << ' ';
        fout << endl;
        fout << lamnbdaGlobal_Be.size() << endl;
        for (int i = 0; i < lamnbdaGlobal_Be.size(); ++i) {
            for (int j = 0; j < lamnbdaGlobal_Be[i].size(); ++j) {
                fout << lamnbdaGlobal_Be[i][j] << "\t" << lamnbdaGlobal_Ed[i][j] << "\t";
            }
            fout << gtBe[i] << "\t" << gtEd[i] << endl;
        }
    }
    fout.close();

}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/swap.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/portfolio/swap.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/calendars/unitedstates.hpp>
#include <ql/time/daycounters/actual360.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace ore::data;
using std::vector;

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        // valuation date
        asof_ = Date(22, Aug, 2016);

        // build vectors with dates and discount factors
        vector<Date> datesEUR = {asof_,
                                 asof_ + 6 * Months,
                                 asof_ + 7 * Months,
                                 asof_ + 8 * Months,
                                 asof_ + 9 * Months,
                                 asof_ + 10 * Months,
                                 asof_ + 11 * Months,
                                 asof_ + 12 * Months,
                                 asof_ + 13 * Months,
                                 asof_ + 14 * Months,
                                 asof_ + 15 * Months,
                                 asof_ + 16 * Months,
                                 asof_ + 17 * Months,
                                 asof_ + 18 * Months,
                                 asof_ + 2 * Years,
                                 asof_ + 3 * Years,
                                 asof_ + 4 * Years,
                                 asof_ + 5 * Years,
                                 asof_ + 6 * Years};
        vector<Date> datesUSD = {asof_,
                                 asof_ + 3 * Months,
                                 asof_ + 4 * Months,
                                 asof_ + 7 * Months,
                                 asof_ + 10 * Months,
                                 asof_ + 13 * Months,
                                 asof_ + 16 * Months,
                                 asof_ + 19 * Months,
                                 asof_ + 2 * Years,
                                 asof_ + 3 * Years,
                                 asof_ + 4 * Years,
                                 asof_ + 5 * Years,
                                 asof_ + 6 * Years};
        vector<DiscountFactor> dfsEUR = {1.0,      1.000972, 1.001138, 1.001309, 1.001452, 1.001663, 1.001826,
                                         1.002005, 1.002196, 1.002369, 1.002554, 1.00275,  1.002918, 1.003114,
                                         1.004134, 1.006005, 1.007114, 1.006773, 1.004282};
        vector<DiscountFactor> dfsUSD = {1.0,      0.997872, 0.997147, 0.99499, 0.992416, 0.989948, 0.987405,
                                         0.984774, 0.980358, 0.96908,  0.95704, 0.944041, 0.93004};

        // build discount
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")] =
            intDiscCurve(datesEUR, dfsEUR, Actual360(), TARGET());
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "USD")] =
            intDiscCurve(datesUSD, dfsUSD, Actual360(), UnitedStates());

        // build ibor index
        Handle<IborIndex> hEUR(parseIborIndex("EUR-EURIBOR-6M", intDiscCurve(datesEUR, dfsEUR, Actual360(), TARGET())));
        iborIndices_[make_pair(Market::defaultConfiguration, "EUR-EURIBOR-6M")] = hEUR;

        // add Eurib 6M fixing
        hEUR->addFixing(Date(18, Aug, 2016), -0.00191);
        Handle<IborIndex> hUSD(
            parseIborIndex("USD-LIBOR-3M", intDiscCurve(datesUSD, dfsUSD, Actual360(), UnitedStates())));
        iborIndices_[make_pair(Market::defaultConfiguration, "USD-LIBOR-3M")] = hUSD;

        // add Libor 3M fixing
        hUSD->addFixing(Date(18, Aug, 2016), 0.00811);

        // add fx rates
        fxSpots_[Market::defaultConfiguration].addQuote("EURUSD",
                                                        Handle<Quote>(boost::make_shared<SimpleQuote>(1.1306)));
    }

private:
    Handle<YieldTermStructure> intDiscCurve(vector<Date> dates, vector<DiscountFactor> dfs, DayCounter dc,
                                            Calendar cal) {
        boost::shared_ptr<YieldTermStructure> idc(
            new QuantLib::InterpolatedDiscountCurve<LogLinear>(dates, dfs, dc, cal));
        return Handle<YieldTermStructure>(idc);
    }
};
} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CcySwapWithResetsTest)

// Ccy Swap with notional resets test, example from Bloomberg
BOOST_AUTO_TEST_CASE(testCcySwapWithResetsPrice) {

    BOOST_TEST_MESSAGE("Testing CcySwapWithResets Price...");

    // Bloomberg CCYswap and CCYswapReset prices (in USD)
    Real npvCCYswap = -349.69;
    Real npvCCYswapReset = 0;

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>();
    Settings::instance().evaluationDate() = market->asofDate();

    // test asof date
    Date today = market->asofDate();
    BOOST_CHECK_EQUAL(today, Date(22, Aug, 2016));

    // Test if EUR discount curve is empty
    Handle<YieldTermStructure> dts = market->discountCurve("EUR");
    QL_REQUIRE(!dts.empty(), "EUR discount curve not found");

    // Test EUR discount(today+3*Years)) against base value
    BOOST_CHECK_CLOSE(market->discountCurve("EUR")->discount(today + 3 * Years), 1.006005, 0.0001);

    // Test USD discount(today+3*Years)) against base value
    BOOST_CHECK_CLOSE(market->discountCurve("USD")->discount(today + 3 * Years), 0.96908, 0.0001);

    // Test EURUSD fx spot object against base value
    BOOST_CHECK_EQUAL(market->fxSpot("EURUSD")->value(), 1.1306);

    // envelope
    Envelope env("CP");

    // Start/End date
    Date startDate = today;
    Date endDate = today + 5 * Years;

    // date 2 string
    std::ostringstream oss;
    oss << io::iso_date(startDate);
    string start(oss.str());
    oss.str("");
    oss.clear();
    oss << io::iso_date(endDate);
    string end(oss.str());

    // Schedules
    string conv = "MF";
    string rule = "Forward";
    ScheduleData scheduleEUR(ScheduleRules(start, end, "6M", "TARGET", conv, conv, rule));
    ScheduleData scheduleUSD(ScheduleRules(start, end, "3M", "US", conv, conv, rule));

    // EUR Leg without and with Notional resets
    bool isPayerEUR = true;
    string indexEUR = "EUR-EURIBOR-6M";
    bool isInArrears = false;
    int days = 2, fxFixingDays = 0;
    vector<Real> spreadEUR(1, 0.000261);
    string dc = "ACT/360";
    vector<Real> notionalEUR(1, 8833141.95);
    string paymentConvention = "F";
    bool notionalInitialXNL = true;
    bool notionalFinalXNL = true;
    bool notionalAmortizingXNL = false;
    string foreignCCY = "USD";
    Real foreignAmount = 10000000;
    string fxIndex = "FX-ECB-EUR-USD";
    auto legdataEUR = boost::make_shared<FloatingLegData>(indexEUR, days, isInArrears, spreadEUR);
    LegData legEUR1(legdataEUR, isPayerEUR, "EUR", scheduleEUR, dc, notionalEUR, vector<string>(), paymentConvention,
                    notionalInitialXNL, notionalFinalXNL, notionalAmortizingXNL, notionalFinalXNL, foreignCCY,
                    foreignAmount, fxIndex, fxFixingDays);
    LegData legEUR2(legdataEUR, isPayerEUR, "EUR", scheduleEUR, dc, notionalEUR, vector<string>(), paymentConvention,
                    notionalInitialXNL, notionalFinalXNL, notionalAmortizingXNL, false, foreignCCY, foreignAmount,
                    fxIndex, fxFixingDays);

    // USD Leg without notional resets
    bool isPayerUSD = false;
    string indexUSD = "USD-LIBOR-3M";
    vector<Real> spreadUSD(1, 0);
    vector<Real> notionalUSD(1, 10000000);
    auto legdataUSD = boost::make_shared<FloatingLegData>(indexUSD, days, isInArrears, spreadUSD);
    LegData legUSD(legdataUSD, isPayerUSD, "USD", scheduleUSD, dc, notionalUSD, vector<string>(), paymentConvention,
                   notionalInitialXNL, notionalFinalXNL, notionalAmortizingXNL);

    // Build swap trades
    boost::shared_ptr<Trade> swap1(new ore::data::Swap(env, legUSD, legEUR1));
    boost::shared_ptr<Trade> swap2(new ore::data::Swap(env, legUSD, legEUR2));

    // engine data and factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CrossCurrencySwap") = "DiscountedCashflows";
    engineData->engine("CrossCurrencySwap") = "DiscountingCrossCurrencySwapEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<CrossCurrencySwapEngineBuilder>());

    // build swaps and portfolio
    boost::shared_ptr<Portfolio> portfolio(new Portfolio());
    swap1->id() = "XCCY_Swap_EUR_USD";
    swap2->id() = "XCCY_Swap_EUR_USD_RESET";

    portfolio->add(swap1);
    portfolio->add(swap2);
    portfolio->build(engineFactory);

    // check CCYswap NPV
    BOOST_TEST_MESSAGE("CcySwap Price = " << swap1->instrument()->NPV() << " " << swap1->npvCurrency()
                                          << ". BBG Price = " << npvCCYswap << " USD");
    // check if difference between ore and bbg NPV does not exceed 250 USD (on 10M notional)
    BOOST_CHECK_CLOSE(std::abs(swap1->instrument()->NPV() - npvCCYswap), 250, 5);

    // check CCYswapReset NPV
    BOOST_TEST_MESSAGE("CcySwapReset Price = " << swap2->instrument()->NPV() << " " << swap2->npvCurrency()
                                               << ". BBG Price = " << npvCCYswapReset << " USD");
    // check if difference between ore and bbg NPV does not exceed 250 USD (on 10M notional)
    BOOST_CHECK_CLOSE(std::abs(swap2->instrument()->NPV() - npvCCYswapReset), 250, 5);

    // check if sum(XNL) on resetting leg is zero
    Real sumXNL = 0;
    const vector<Leg>& legs = swap2->legs();
    for (Size i = 3; i < legs.size(); i++) {
        const QuantLib::Leg& leg = legs[i];
        for (Size j = 0; j < leg.size(); j++) {
            boost::shared_ptr<QuantLib::CashFlow> ptrFlow = leg[j];
            sumXNL += ptrFlow->amount();
        }
    }
    BOOST_CHECK_EQUAL(sumXNL, 0);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/*
 * ukf.cpp
 *
 *  Created on: Jan 12, 2012
 *      Author: eba
 */

#include "mhfpython.h"
#include <MultiHypDist.h>
#include <iau_ukf_eigen.hpp>
#include <boost/foreach.hpp>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/Geometry>

#include "transformations.h"

template<int dim, int procnoisedim, int inputdim>
struct transformationsWrap : transformations<dim,procnoisedim,inputdim>
							,wrapper<transformations<dim,procnoisedim,inputdim> >
{
	virtual Matrix<double,dim,1> statetrans(
			const Matrix<double,dim,1> & oldstate,
			const Matrix<double,procnoisedim,1> & noise,
			const Matrix<double,inputdim,1> & input) {return this->get_override("statetrans")(oldstate,noise,input);}
	virtual Matrix<double,inputdim,1> measerr(Matrix<double,dim,1> state,
											  Matrix<double,procnoisedim,1> noise,
											  Matrix<double,inputdim,1> meas,
											  nullclass auxin){return this->get_override("measerr")(state,noise,meas);}
};

template< class T > // T extends transformations
void UKFPredict(
		MultiHypDist<dim> & mhd,
		const Matrix<double,inputdim,1> & input,
		const Matrix<double,procnoisedim,procnoisedim> & noisecov,
		T & transobj
		) {
	mhd.predict(input,noisecov,transobj,&T::statetrans);
}

template< class T > // T extends transformations
void UKFUpdate(
		MultiHypDist<dim> & mhd,
		const Matrix<double,measdim,1> & meas,
		const Matrix<double,measdim,measdim> & noisecov,
		T & transobj
		) {
	nullclass nullobj;
	mhd.update(meas,noisecov,nullobj,transobj,&T::measerr);
}

//struct odometryWrap: public odometryTransformation, public wrapper<transformations<3,2,2> >{
//	odometryWrap(double B, double E2, double A2): odometryTransformation(B,E2,A2){}
//};

void export_ukfs() {
	class_<transformationsWrap<dim,procnoisedim,inputdim>, boost::noncopyable>("transformations")
	    ;
	class_<transformationsWrap<dim,measdim,measdim>, boost::noncopyable>("transformationsMeas")
	    ;

    def("UKFPredict",UKFPredict<transformations<dim,procnoisedim,inputdim> >);
    def("UKFUpdate",UKFUpdate<transformations<dim,measdim,measdim> >);
}


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/eps.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/eps.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i< N; ++i)
  {
    a1[i] = T(i);
    b[i] = bs::eps(a1[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_IEEE_EQUAL(bs::eps(aa1), bb);
}

STF_CASE_TPL("Check eps on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL ("eps for IEEE types",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::eps;
  using p_t = bs::pack<T>;
  using r_t = decltype(eps(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
 #ifndef BOOST_SIMD_NO_INVALIDS
  STF_IEEE_EQUAL(eps(bs::Inf<p_t>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(eps(bs::Minf<p_t>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(eps(bs::Nan<p_t>()), bs::Nan<r_t>());
 #endif

  STF_EQUAL(eps(p_t{-1}), bs::Eps<r_t>());
  STF_EQUAL(eps(p_t{1}) , bs::Eps<r_t>());

 #if !defined(BOOST_SIMD_NO_DENORMALS)
  STF_EQUAL(eps(p_t{0}), bs::Mindenormal<r_t>());
 #endif
}

STF_CASE_TPL ("eps on integers",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::eps;
  using p_t = bs::pack<T>;
  using r_t = decltype(eps(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(eps(p_t(-1)), p_t{1});
  STF_EQUAL(eps(p_t(0)) , p_t{1});
  STF_EQUAL(eps(p_t(-1)), p_t{1});
  STF_EQUAL(eps(p_t(42)), p_t{1});
}


///////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2012, Ben Lane
//
// Permission to use, copy, modify, and/or distribute this software for any
// purpose with or without fee is hereby granted, provided that the above
// copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
// WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
// SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
// WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
// OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
// CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
//
// Contains common headers that are included by all files, but are too big/too
// complicated to be precompiled by Visual Studio.
//
// TODO: It's only Eigen that VS struggles with. Maybe use glm or roll our own
//		instead, the stuff we need only represents a small fraction of Eigen.
///////////////////////////////////////////////////////////////////////////////

#pragma once

#include <Eigen/Eigen>

using namespace Eigen;


#include <Eigen/Core>
USING_PART_OF_NAMESPACE_EIGEN
using namespace std;

template<typename Derived>
Eigen::Block<Derived>
topLeftCorner(MatrixBase<Derived>& m, int rows, int cols)
{
  return Eigen::Block<Derived>(m.derived(), 0, 0, rows, cols);
}

template<typename Derived>
const Eigen::Block<Derived>
topLeftCorner(const MatrixBase<Derived>& m, int rows, int cols)
{
  return Eigen::Block<Derived>(m.derived(), 0, 0, rows, cols);
}

int main(int, char**)
{
  Matrix4d m = Matrix4d::Identity();
  cout << topLeftCorner(4*m, 2, 3) << endl; // calls the const version
  topLeftCorner(m, 2, 3) *= 5;              // calls the non-const version
  cout << "Now the matrix m is:" << endl << m << endl;
  return 0;
}


//  gcd.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  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_RATIO_MPL_GCD_HPP
#define BOOST_RATIO_MPL_GCD_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/ratio/detail/mpl/gcd.hpp>

namespace boost {
namespace mpl {

template<>
struct gcd_impl< rational_c_tag,rational_c_tag >
{
    template< typename R1, typename R2 > struct apply
        : ratio_gcd<R1, R2>
    {
    };
};
}
}

#endif  // BOOST_RATIO_MPL_GCD_HPP


#include "eeg_change_point.hpp"

#include <armadillo>

using namespace arma;

static inline int round_up(int num_to_round, int multiple)
{
  return ((num_to_round + multiple - 1) / multiple) * multiple;
}

void init_cp_data_t(cp_data_t* cp_data, int nt)
{

  frowvec m = zeros<frowvec>(nt);
  m(0) = 1000;
  frowvec mu = zeros<frowvec>(nt);
  mu(0) = 1000;

  frowvec cu = zeros<frowvec>(nt);
  frowvec cl = zeros<frowvec>(nt);

  frowvec cp = zeros<frowvec>(nt);
  frowvec yp = zeros<frowvec>(nt);

  cp_data->cp = cp;
  cp_data->yp = yp;
  cp_data->cu = cu;
  cp_data->cl = cl;
  cp_data->mu = mu;
  cp_data->m = m;
  cp_data->total_count = 0;
}

void get_change_points_as_arr(float* spec_arr, int n_rows, int n_cols, cp_data_t* cp_data)
{
  fmat spec_mat = fmat(spec_arr, n_rows, n_cols);
  get_change_points(spec_mat, cp_data);
}

// TODO(joshblum): make real variable names
void get_change_points(fmat& spec_mat,
                       cp_data_t* cp_data)
{
  frowvec s = sum(spec_mat, 0); // sum rows
  int stride = 10;
  int nt = round_up(s.n_cols / stride, 10);
  init_cp_data_t(cp_data, nt);

  int max_val = 5000;

  float b = 0.95;
  float bb = 0.995;
  int max_amp = 3000;

  // CUMSUM parameters
  int sigma = 100;
  int K = 4 * sigma / 2;
  int h = 5;
  int H = h * sigma;

  int ct = 0;
  int total_count = 0;

  float np = 0.;
  float nm = 0.;
  for (int j = 1; j < nt; j++)
  {
    ct++;
    double s_j = s[j * stride] > max_val ? max_val : s[j * stride];
    //printf("s_j: %f\n", s_j);

    // signal we are tracking -- short term mean
    cp_data->m[j] = fmin(cp_data->m[j - 1] * b + (1 - b) * s_j, max_amp);
    if (ct < 20)
    {
      cp_data->mu[j] = fmin(cp_data->mu[j - 1] * bb + (1 - bb) * cp_data->m[j], max_amp);
    }
    else
    {
      cp_data->mu[j] = cp_data->mu[j - 1];
    }

    cp_data->cu[j] = fmax(0, cp_data->cu[j - 1] + cp_data->m[j] - cp_data->mu[j] - K);
    cp_data->cl[j] = fmax(0, cp_data->cl[j - 1] + cp_data->mu[j] - cp_data->m[j] - K);

    np = (np + 1) * (cp_data->cu[j] > 0);
    nm = (nm + 1) * (cp_data->cl[j] > 0);

    if (cp_data->cu[j] > H || cp_data->cl[j] > H)
    {
      cp_data->cp[total_count] = j * stride; // time of change point
      total_count++;
      ct = 0;

      if (cp_data->cu[j] > H)
      {
        cp_data->mu[j] = cp_data->mu[j] + K + cp_data->cu[j] / np;
        cp_data->cu[j] = 0.;
        np  = 0;
      }
      if (cp_data->cl[j] > H)
      {
        cp_data->mu[j] = cp_data->mu[j] - K - cp_data->cl[j] / nm;
        cp_data->cl[j] = 0.;
        nm = 0.;
      }
    }
  }
  // TODO(joshblum): ensure we are clipping the array properly here
  // TODO(joshblum): head method missing on ubuntu package?
  // cp_data->cp.head(total_count);
  // cp_data->yp.head(total_count);
  cp_data->yp.fill(max_amp);
  cp_data->total_count = total_count;

}

void print_frowvec(char* name, frowvec vector, int num_samples)
{
  printf("%s: [ ", name);
  for (int i = 0; i < num_samples; i++)
  {
    printf("%.2f, ", vector[i]);
  }
  printf("]\n");
}

void example_change_points_as_arr(float* spec_arr, int n_rows, int n_cols)
{
  cp_data_t cp_data;
  get_change_points_as_arr(spec_arr, n_rows, n_cols, &cp_data);
  print_cp_data_t(&cp_data);
}

void example_change_points(fmat& spec_mat)
{
  cp_data_t cp_data;
  get_change_points(spec_mat, &cp_data);
  print_cp_data_t(&cp_data);
}

void print_cp_data_t(cp_data_t* cp_data)
{
  printf("Total change points found: %d\n", cp_data->total_count);
  print_frowvec("cp", cp_data->cp, cp_data->total_count);
  print_frowvec("yp", cp_data->yp, cp_data->total_count);
  print_frowvec("cu", cp_data->cu, cp_data->total_count);
  print_frowvec("cl", cp_data->cl, cp_data->total_count);
  print_frowvec("mu", cp_data->mu, cp_data->total_count);
  print_frowvec("m", cp_data->m, cp_data->total_count);
}


#ifndef CPPID_GM3D_DETAIL_BASIC_VECTOR_ND_HPP
#define CPPID_GM3D_DETAIL_BASIC_VECTOR_ND_HPP

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <functional>
#include <numeric>
#include <type_traits>
#include <utility>

#include <boost/qvm/all.hpp>

#include "basic_point_vector.hpp"
#include "type_traits.hpp"

namespace cppid::gm3d::detail {

template<typename T, std::size_t N>
struct basic_vector_nd
: public basic_point_vector<basic_vector_nd<T, N>, T, N> {
  using typename basic_point_vector<basic_vector_nd<T, N>, T, N>::scalar_type;

  using basic_point_vector<basic_vector_nd<T, N>, T, N>::dimensions;

  using basic_point_vector<basic_vector_nd<T, N>, T, N>::basic_point_vector;

  constexpr basic_vector_nd() noexcept = default;

  template<typename U,
           std::enable_if_t<
            detail::is_explicit_constructible_v<scalar_type, U>>* = nullptr>
  explicit constexpr basic_vector_nd(basic_vector_nd<U, dimensions> v) noexcept
  : basic_vector_nd{v, std::make_index_sequence<dimensions>()}
  {
  }

  template<typename U,
           std::size_t... Ns,
           std::enable_if_t<
            (sizeof...(Ns) == dimensions) &&
            detail::is_explicit_constructible_v<scalar_type, U>>* = nullptr>
  constexpr basic_vector_nd(basic_vector_nd<U, dimensions> v,
                            std::index_sequence<Ns...>) noexcept
  : basic_vector_nd{v.elems[Ns]...}
  {
  }

  template<typename U,
           std::enable_if_t<
            detail::is_implicit_constructible_v<scalar_type, U>>* = nullptr>
  constexpr basic_vector_nd(basic_vector_nd<U, dimensions> v) noexcept
  : basic_vector_nd{v, std::make_index_sequence<dimensions>()}
  {
  }

  template<typename U,
           std::size_t... Ns,
           std::enable_if_t<
            (sizeof...(Ns) == dimensions) &&
            detail::is_implicit_constructible_v<scalar_type, U>>* = nullptr>
  constexpr basic_vector_nd(basic_vector_nd<U, dimensions> v,
                            std::index_sequence<Ns...>) noexcept
  : basic_vector_nd{scalar_type(v.elems[Ns])...}
  {
  }

  template<typename U,
           typename = std::enable_if_t<std::is_convertible_v<U, scalar_type>>>
  constexpr auto operator=(const basic_vector_nd<U, dimensions> rhs) noexcept
   -> basic_vector_nd&
  {
    op_assign(rhs.elems, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator+=(const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd&
  {
    op_assign(rhs.elems, std::plus{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator+=(scalar_type rhs) noexcept -> basic_vector_nd&
  {
    op_assign(rhs, std::plus{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator-=(const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd&
  {
    op_assign(rhs.elems, std::minus{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator-=(scalar_type rhs) noexcept -> basic_vector_nd&
  {
    op_assign(rhs, std::minus{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator*=(const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd&
  {
    op_assign(
     rhs.elems, std::multiplies{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator*=(scalar_type rhs) noexcept -> basic_vector_nd&
  {
    op_assign(rhs, std::multiplies{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator/=(const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd&
  {
    op_assign(
     rhs.elems, std::divides{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator/=(scalar_type rhs) noexcept -> basic_vector_nd&
  {
    op_assign(rhs, std::divides{}, std::make_index_sequence<dimensions>());
    return *this;
  }

  constexpr auto operator==(const basic_vector_nd& rhs) const noexcept -> bool
  {
    return is_equal(rhs.elems, std::make_index_sequence<dimensions>());
  }

  constexpr auto operator!=(const basic_vector_nd& rhs) const noexcept -> bool
  {
    return !((*this) == rhs);
  }

  constexpr auto dot(const basic_vector_nd& other) const noexcept -> scalar_type
  {
    return dot(other, std::make_index_sequence<dimensions>());
  }

  constexpr auto magnitude() const noexcept -> scalar_type
  {
    return std::sqrt(dot(*this));
  }

  constexpr auto magnitude_squared() const noexcept -> scalar_type
  {
    return dot(*this);
  }

  constexpr auto to_unit() const noexcept -> basic_vector_nd
  {
    return *this / magnitude();
  }

  constexpr auto reflect(const basic_vector_nd& n) const noexcept
   -> basic_vector_nd
  {
    return *this - 2 * dot(n) * n;
  }

  template<typename V = void, std::enable_if_t<dimensions == 3, V>* = nullptr>
  constexpr auto cross(const basic_vector_nd& other) const noexcept
   -> basic_vector_nd
  {
    return boost::qvm::cross(*this, other);
  }

  friend constexpr auto operator+(const basic_vector_nd& val) noexcept
   -> basic_vector_nd
  {
    return val;
  }

  // friend constexpr auto operator+(const basic_vector_nd& lhs,
  //                                 const basic_vector_nd& rhs) noexcept
  //  -> basic_vector_nd
  // {
  //   return invoke_op{}(
  //    std::plus{}, lhs, rhs, std::make_index_sequence<dimensions>());
  // }

  template<typename U,
           typename =
            std::enable_if_t<std::is_invocable_v<std::plus<>, scalar_type, U>>>
  friend constexpr auto
  operator+(const basic_vector_nd& lhs,
            const basic_vector_nd<U, dimensions>& rhs) noexcept
   -> basic_vector_nd<std::invoke_result_t<std::plus<>, scalar_type, U>,
                      dimensions>
  {
    return invoke_op{}(
     std::plus{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  friend constexpr auto operator+(const basic_vector_nd& lhs,
                                  scalar_type rhs) noexcept -> basic_vector_nd
  {
    return lhs + basic_vector_nd{rhs};
  }

  friend constexpr auto operator+(scalar_type lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return basic_vector_nd{lhs} + rhs;
  }

  friend constexpr auto operator-(const basic_vector_nd& lhs) noexcept
   -> basic_vector_nd
  {
    return invoke_op{}(
     std::negate{}, lhs, std::make_index_sequence<dimensions>());
  }

  friend constexpr auto operator-(const basic_vector_nd& lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return invoke_op{}(
     std::minus{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  template<typename U,
           typename =
            std::enable_if_t<std::is_invocable_v<std::minus<>, scalar_type, U>>>
  friend constexpr auto
  operator-(const basic_vector_nd& lhs,
            const basic_vector_nd<U, dimensions>& rhs) noexcept
   -> basic_vector_nd<std::invoke_result_t<std::minus<>, scalar_type, U>,
                      dimensions>
  {
    return invoke_op{}(
     std::minus{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  friend constexpr auto operator-(const basic_vector_nd& lhs,
                                  scalar_type rhs) noexcept -> basic_vector_nd
  {
    return lhs - basic_vector_nd{rhs};
  }

  friend constexpr auto operator-(scalar_type lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return basic_vector_nd{lhs} - rhs;
  }

  friend constexpr auto operator*(const basic_vector_nd& lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return invoke_op{}(
     std::multiplies{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  template<typename U,
           typename = std::enable_if_t<
            std::is_invocable_v<std::multiplies<>, scalar_type, U>>>
  friend constexpr auto
  operator*(const basic_vector_nd& lhs,
            const basic_vector_nd<U, dimensions>& rhs) noexcept
   -> basic_vector_nd<std::invoke_result_t<std::multiplies<>, scalar_type, U>,
                      dimensions>
  {
    return invoke_op{}(
     std::multiplies{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  friend constexpr auto operator*(const basic_vector_nd& lhs,
                                  scalar_type rhs) noexcept -> basic_vector_nd
  {
    return lhs * basic_vector_nd{rhs};
  }

  friend constexpr auto operator*(scalar_type lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return basic_vector_nd{lhs} * rhs;
  }

  friend constexpr auto operator/(const basic_vector_nd& lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return invoke_op{}(
     std::divides{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  template<typename U,
           typename = std::enable_if_t<
            std::is_invocable_v<std::divides<>, scalar_type, U>>>
  friend constexpr auto
  operator/(const basic_vector_nd& lhs,
            const basic_vector_nd<U, dimensions>& rhs) noexcept
   -> basic_vector_nd<std::invoke_result_t<std::divides<>, scalar_type, U>,
                      dimensions>
  {
    return invoke_op{}(
     std::divides{}, lhs, rhs, std::make_index_sequence<dimensions>());
  }

  friend constexpr auto operator/(const basic_vector_nd& lhs,
                                  scalar_type rhs) noexcept -> basic_vector_nd
  {
    return lhs / basic_vector_nd{rhs};
  }

  friend constexpr auto operator/(scalar_type lhs,
                                  const basic_vector_nd& rhs) noexcept
   -> basic_vector_nd
  {
    return basic_vector_nd{lhs} / rhs;
  }

private:
  struct invoke_op {
    template<typename P, typename U, std::size_t... Ns>
    constexpr auto operator()(const P op,
                              const basic_vector_nd<U, dimensions>& lhs,
                              std::index_sequence<Ns...>) const noexcept
     -> basic_vector_nd<std::invoke_result_t<P, U>, dimensions>
    {
      static_assert(sizeof...(Ns) == dimensions);
      return {op(lhs.elems[Ns])...};
    }

    template<typename P, typename U, typename V, std::size_t... Ns>
    constexpr auto operator()(const P op,
                              const basic_vector_nd<U, dimensions>& lhs,
                              const basic_vector_nd<V, dimensions>& rhs,
                              std::index_sequence<Ns...>) const noexcept
     -> basic_vector_nd<std::invoke_result_t<P, U, V>, dimensions>
    {
      static_assert(sizeof...(Ns) == dimensions);
      return {op(lhs.elems[Ns], rhs.elems[Ns])...};
    }
  };

  template<typename U, typename P, std::size_t... Ns>
  inline constexpr void op_assign(const U (&rhs)[dimensions],
                                  P op,
                                  std::index_sequence<Ns...>) noexcept
  {
    ((this->elems[Ns] = op(this->elems[Ns], rhs[Ns])), ...);
  }

  template<typename U, typename P, std::size_t... Ns>
  inline constexpr void
  op_assign(U rhs, P op, std::index_sequence<Ns...>) noexcept
  {
    ((this->elems[Ns] = op(this->elems[Ns], rhs)), ...);
  }

  template<typename U, std::size_t... Ns>
  inline constexpr void op_assign(const U (&rhs)[dimensions],
                                  std::index_sequence<Ns...>) noexcept
  {
    ((this->elems[Ns] = rhs[Ns]), ...);
  }

  template<typename U, std::size_t... Ns>
  constexpr auto is_equal(const U (&rhs)[dimensions],
                          std::index_sequence<Ns...>) const noexcept -> bool
  {
    return ((this->elems[Ns] == rhs[Ns]) && ... && true);
  }

  template<std::size_t... Ns>
  constexpr auto negate(std::index_sequence<Ns...>) const noexcept
   -> basic_vector_nd
  {
    return basic_vector_nd{(-this->elems[Ns])...};
  }

  template<std::size_t... Ns>
  constexpr auto dot(const basic_vector_nd& other,
                     std::index_sequence<Ns...>) const noexcept -> scalar_type
  {
    return ((this->elems[Ns] * other.elems[Ns]) + ... + 0);
  }
};

} // namespace cppid::gm3d::detail

namespace boost::qvm {

template<typename T, std::size_t N>
struct vec_traits<cppid::gm3d::detail::basic_vector_nd<T, N>>
: public vec_traits<
   cppid::gm3d::detail::
    basic_point_vector<cppid::gm3d::detail::basic_vector_nd<T, N>, T, N>> {
};

} // namespace boost::qvm

#endif


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

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ifnot_dec.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/two.hpp>

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

  // return type conformity test
  STF_EXPR_IS( ifnot_dec(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::Mone<T>()), bs::Mtwo<T>());
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::Zero<T>()), bs::Mone<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
}

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

  // return type conformity test
  STF_EXPR_IS( ifnot_dec(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::Two<T>()), bs::One<T>());
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());}

STF_CASE_TPL(" ifnot_dec floating", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnot_dec;

  // return type conformity test
  STF_EXPR_IS( ifnot_dec(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::Two<T>()), bs::One<T>());
  STF_EQUAL(ifnot_dec(bs::Zero<T>(), bs::Zero<T>()), bs::Mone<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnot_dec(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
}


// Copyright 2013 Daniel Parker
// Distributed under Boost license

#include <boost/test/unit_test.hpp>
#include "jsoncons/json.hpp"
#include "jsoncons/json_serializer.hpp"
#include <sstream>
#include <vector>
#include <utility>
#include <ctime>
#include <cwchar>

using namespace jsoncons;

BOOST_AUTO_TEST_SUITE(string_to_double_test_suite)

BOOST_AUTO_TEST_CASE(test_string_to_double)
{
    jsoncons::float_reader reader;
    const char* begin = "1.15507e-173";
    const char* endptr = begin + strlen(begin);
    const double value1 = 1.15507e-173;
    const double value2 = strtod((char*) begin, (char**)&endptr );
    const double value3 = reader.read(begin,endptr-begin);

    BOOST_CHECK(value1 == value2);
    BOOST_CHECK(value2 == value3);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by Avi on 29/03/2020.
//

#include <iostream>
#include <boost/icl/interval_map.hpp>
#include "CountMinSketch.h"
#include "SketchRing.h"

struct C {
    C(int i = 0) : i(i) {}

    bool operator==(const C& rhs) const {
        return i == rhs.i;
    }

    bool operator!=(const C& rhs) const {
        return !(rhs == *this);
    }

    friend std::ostream& operator<<(std::ostream& os, const C& c) {
        os << "i: " << c.i;
        return os;
    }

    int i;
};

int main() {

    using namespace boost::icl;
    using namespace std;
    interval_map<int, C> im;

    im.set(make_pair(interval<int>::type(0, 99), C(1)));
    cout << im << endl;

    im.set(make_pair(interval<int>::type(0, 50), C(11)));
    cout << im << endl;

    im.set(make_pair(interval<int>::type(25, 75), C(111)));
    cout << im << endl;

    im.set(make_pair(interval<int>::type(60, 90), C(1111)));
    cout << im << endl;

    cout << im.find(26)->second << endl;
    cout << im.find(26)->first << endl;

    im.set(make_pair(interval<int>::type(16, 25), C(50)));
    cout << im << endl;

    im.erase(interval<int>::type(19, 21));
    cout << im << endl;

    im.set(make_pair(interval<int>::type(19, 21), C(50)));
    cout << im << endl;

    CountMinSketch cm(10, 34, 1);

    std::cout << "hello" << std::endl;
    std::cout << cm.size() << std::endl;

    cm.update(3, 5);
    cm.update(6, 5);
    cm.update(3, 5);
    cm.update(1, 5);
    cm.update(6, -5);

    std::cout << cm.size() << std::endl;

    std::cout << "new cm" << std::endl;

    cm = CountMinSketch(5, 5, 1);
    const vector<int> values{1, 223, 325, 326, 544, 555, 556};

    for (unsigned i : values) {
        cm.update(i, 1024 - i);
    }

    for (unsigned i : values) {
        printf("%4d: %4d %4d\n", i, 1024 - i, cm.estimate(i));
    }

    const auto& hh = cm.heavyHitters(.5);
    for (auto [item, freq] : hh) {
        printf("ITEM: %5d   Freq: %5d\n", item, freq);
    }

//    return 0;
    SketchRing sketchRing(CountMinSketch(5, 5, 1));

    cout << sketchRing.size() << endl;
    cout << "+++++++++++++++++++++" << endl;

    for (unsigned i : values) {
        sketchRing.update(i, 1024 - i);
    }

    for (unsigned i : values) {
        printf("%4d: %4d %4d\n", i, 1024 - i, sketchRing.estimate(i));
    }

    sketchRing.resize(6);
    cout << "RESIZE" << endl;

    for (unsigned i : values) {
        printf("%4d: %4d %4d\n", i, 1024 - i, sketchRing.estimate(i));
    }

    const vector<int> values2{111, 222, 333};

    for (unsigned i : values2) {
        sketchRing.update(i, i);
    }
    for (unsigned i : values2) {
        printf("%4d: %4d %4d\n", i, i, sketchRing.estimate(i));
    }

    return 0;
}


/// @file  embed_cmp.cpp
/// @brief Embeds an object collection using oracle comparisons.

#include <boost/program_options.hpp>
#include <ordgeo/ordgeo.hpp>
#include <json.hpp>
#include <algorithm>
#include <chrono>
#include <map>

using Eigen::Index;
using Eigen::MatrixXd;
using namespace boost::program_options;
using nlohmann::json;
using ORDGEO_NAMESPACE::core::CmpOutcome;
using ORDGEO_NAMESPACE::core::CmpConstraint;
using ORDGEO_NAMESPACE::embed::EmbedConfig;
using ORDGEO_NAMESPACE::embed::EmbedResult;
using ORDGEO_NAMESPACE::io::loadCsvMatrix;
using ORDGEO_NAMESPACE::io::openSink;
using ORDGEO_NAMESPACE::io::openSource;
using ORDGEO_NAMESPACE::io::saveCsvMatrix;
using ORDGEO_NAMESPACE::io::saveEmbedding;
using ORDGEO_NAMESPACE::linalg::embeddingFromKernelSVD;
using std::cerr;
using std::cout;
using std::endl;
using std::map;
using std::max;
using std::string;
using std::stringstream;
using std::vector;

/// Main program.
int run_soe(
	string cmp,
	size_t nDim,
	string initPos,
	double margin,
	double minDelta,
	size_t maxIter,
	size_t bestOf,
	bool saveAll,
	double bestTol,
	bool verbose,
	string out,
	string outstats) {
	json results;
	results["args"] = {
		{"cmp", cmp},
		{"nDim", nDim},
		{"initPos", initPos},
		{"margin", margin},
		{"minDelta", minDelta},
		{"maxIter", maxIter},
		{"bestOf", bestOf},
		{"saveAll", saveAll},
		{"bestTol", bestTol},
		{"verbose", verbose},
		{"out", out},
		{"outstats", outstats},
	};
	EmbedConfig config;
	config.nDim = nDim;
	config.minDelta = minDelta;
	config.maxIter = maxIter;
	config.verbose = verbose;
	config.margin = margin;

	// Load the triples
	vector<CmpConstraint> cons;
	auto incmp = openSource(cmp);
	if (!incmp || !*incmp) {
		cerr << "Could not load constraints from: " << cmp << endl;
		return 1;
	}
	CmpOutcome con;
	map<size_t,size_t> points; // Map from true ID to temp ID
	vector<size_t> pointIds; // Map from temp ID to true ID
	size_t maxObj = 0;
	while (*incmp >> con) {
		for (size_t pt : {con.a, con.b, con.c}) {
			auto it = points.find(pt);
			if (it == points.end()) {
				points[pt] = pointIds.size();
				pointIds.push_back(pt);
				maxObj = max(maxObj, pt);
			}
		}
		cons.emplace_back(con);
	}
	size_t nObj = pointIds.size();
	cout << "Embedding " << nObj << " points referenced by triples" << endl;
	results["args"]["nObj"] = nObj;
	results["num_triples"] = cons.size();

	// Rename objects as needed to embed only what we have constrained
	if (nObj < maxObj + 1) {
		for (auto& con : cons) {
			con.a = points[con.a];
			con.b = points[con.b];
			con.c = points[con.c];
			con.d = points[con.d];
		}
	} else {
		pointIds.clear();
	}

	// Load the initial positions, if we have an init file
	MatrixXd X0(nObj, nDim);
	if (!initPos.empty()) {
		try {
			auto X = loadCsvMatrix(initPos);
			if (pointIds.empty()) {
				X0 = X;
			} else {
				for (Index ii = 0; ii < X0.rows(); ii++) {
					X0.row(ii) = X.row(points[ii]);
				}
			}
		} catch (std::exception &e) {
			cerr << "Could not load initial positions: " << e.what() << endl;
			return 1;
		}
	}
	points.clear();

	// Save the embedding
	auto save = [&](EmbedResult& res, string pathX) {
		try {
			if (!pointIds.empty()) {
				cout << "Writing " << pathX << endl;
				saveEmbedding(res.X, pointIds, pathX);
			} else {
				cout << "Writing " << pathX << endl;
				saveEmbedding(res.X, pathX);
			}
		} catch (const std::exception& e) {
			cerr << e.what() << endl;
			return;
		}
	};

	// Produce the embedding
	EmbedResult bestRes;
	bestOf = max<size_t>(bestOf, 1);
	bestRes.loss = 1000 + bestTol;
	for (size_t rep = 0; rep < bestOf && (saveAll || bestRes.loss > bestTol); rep++) {
		if (initPos.empty()) {
			X0 = MatrixXd::Random(nObj, nDim);
		}

		cout << "Rep " << rep+1 << " / " << bestOf << ": Embedding " << nObj
			<< " objects into R^" << config.nDim << " using "
			<< cons.size() << " constraints" << endl;
		EmbedResult res;
		auto startTime = std::chrono::steady_clock::now();
		auto startProc = clock();
		res = embedCmpWithSOE(cons, X0, config);
		auto endTime = std::chrono::steady_clock::now();
		auto endProc = clock();
		std::chrono::duration<double> diffTime = (endTime - startTime);
		double durTime = diffTime.count();
		double durProc = (endProc - startProc) / CLOCKS_PER_SEC;

		cout << "Rep " << rep+1 << " / " << bestOf << ": Duration: " << durProc
			<< " CPU, " << durTime << " wall" << endl;
		cout << "Rep " << rep+1 << " / " << bestOf << ": Objective: "
			<< res.loss << endl;
		if (rep == 0 || bestRes.loss > res.loss) {
			bestRes = res;
			results["elapsed_perf_counter"] = durProc;
			results["elapsed_process_time"] = durTime;
			results["loss"] = res.loss;
		}

		if (saveAll) {
			stringstream outX;
			outX << out << ".try_" << rep+1 << ".csv.gz";
			save(res, outX.str());
		}
	}
	save(bestRes, out);

	// Save our statistics
	if (!outstats.empty()) {
		cout << "Writing " << outstats << endl;
		auto fstats = openSink(outstats);
		if (!fstats || !fstats->good()) {
			cerr << "Could not open statistics output file" << endl;
			return 1;
		}
		*fstats << std::setw(2) << results << endl;
	}

	cout << "Done" << endl;
	return 0;
}

/// Program entry point.
int main(int argc, char * argv[]) {
	try {
		options_description desc{"soe options"};
		desc.add_options()
			("help,h", "Show this usage message")
			("cmp", value<string>(), "Path to a file containing comparisons to embed (*.csv)")
			("ndim", value<size_t>(), "Embedding dimensionality")
			("init_pos", value<string>()->default_value(""), "Initialize from the specified positions instead of at random (*.csv)")
			("margin", value<double>()->default_value(0), "Constraint margin")
			("min_delta", value<double>()->default_value(1e-12), "Min loss objective delta to stop")
			("max_iter", value<size_t>()->default_value(1000), "Max iteration count")
			("best_of", value<size_t>()->default_value(1), "Return best of n runs")
			("best_tol", value<double>()->default_value(0), "Stop when a run achieves this score")
			("save_all", value<bool>()->default_value(false), "Save intermediate embeddings")
			("verbose", value<bool>()->default_value(false), "Verbose output")
			("out", value<string>(), "Output path for the embedding (*.csv)")
			("outstats", value<string>()->default_value(""), "Output path for statistics (*.json)")
			;

		variables_map vm;
		store(parse_command_line(argc, argv, desc), vm);
		notify(vm);

		bool valid = true;
		if (vm.count("help")) {
			cout << desc << endl;
			valid = false;
		}
		if (!vm.count("cmp")) {
			cerr << "--cmp is required" << endl;
			valid = false;
		}
		if (!vm.count("ndim")) {
			cerr << "--ndim is required" << endl;
			valid = false;
		}
		if (!vm.count("out")) {
			cerr << "--out is required" << endl;
			valid = false;
		}
		if (valid) {
			return run_soe(
				vm["cmp"].as<string>(),
				vm["ndim"].as<size_t>(),
				vm["init_pos"].as<string>(),
				vm["margin"].as<double>(),
				vm["min_delta"].as<double>(),
				vm["max_iter"].as<size_t>(),
				vm["best_of"].as<size_t>(),
				vm["save_all"].as<bool>(),
				vm["best_tol"].as<double>(),
				vm["verbose"].as<bool>(),
				vm["out"].as<string>(),
				vm["outstats"].as<string>());
		}
	} catch (const error &ex) {
		cerr << ex.what() << endl;
	}
	return 1;
}


#ifndef BOOST_ASTRONOMY_COORDINATE_BASE_REPRESENTATION_HPP
#define BOOST_ASTRONOMY_COORDINATE_BASE_REPRESENTATION_HPP

#include <cstddef>
#include <type_traits>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/static_assert.hpp>

#include <boost/astronomy/detail/is_base_template_of.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bg = boost::geometry;
typedef bg::degree degree;
typedef bg::radian radian;

// structure which is the base for all the representation
template
<
    std::size_t DimensionCount,
    typename CoordinateSystem,
    typename CoordinateType=double
>
struct base_representation
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((DimensionCount == 2 || DimensionCount == 3),
        "DimensionCount is expected to be 2 or 3");
    BOOST_STATIC_ASSERT_MSG((std::is_arithmetic<CoordinateType>::value),
        "Coordinate Type must be an arithmetic type");
    ///@endcond

protected:
    bg::model::point<CoordinateType, DimensionCount, CoordinateSystem> point;

public:

    typedef CoordinateSystem system;
    typedef CoordinateType type;

    //! converts current representation into specified representation
    template <typename ReturnType>
    ReturnType to_representation() const
    {
        /*checking return type if they both are not subclass of
        base_representaion then compile time erorr is generated*/
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_representation, ReturnType>::value),
            "return type is expected to be a representation class");

        return ReturnType(this->point);
    }

    bg::model::point<CoordinateType, DimensionCount, CoordinateSystem> get_point() const
    {
        return this->point;
    }

    template
    <
        std::size_t OtherDimensionCount,
        typename OtherCoordinateSystem,
        typename OtherCoordinateType
    >
    bool operator==
    (
        base_representation
        <
            OtherDimensionCount,
            OtherCoordinateSystem,
            OtherCoordinateType
        >  const& other
    ) const
    {
        /*converting both coordinates/vector into cartesian system*/
        bg::model::point
        <
            typename std::conditional
            <
                sizeof(OtherCoordinateType) >= sizeof(CoordinateType),
                OtherCoordinateType,
                CoordinateType
            >::type,
            3,
            bg::cs::cartesian
        > tempPoint1, tempPoint2;
        bg::transform(this->point, tempPoint1);
        bg::transform(other.get_point(), tempPoint2);

        return (bg::get<0>(tempPoint1) == bg::get<0>(tempPoint2)) &&
            (bg::get<1>(tempPoint1) == bg::get<1>(tempPoint2)) &&
            (bg::get<2>(tempPoint1) == bg::get<2>(tempPoint2));
    }

}; //base_representation

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_BASE_REPRESENTATION_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/operator/include/functions/is_equal.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/three.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/true.hpp>
#include <boost/simd/include/constants/false.hpp>

NT2_TEST_CASE_TPL ( is_equal_integer,  BOOST_SIMD_INTEGRAL_TYPES)
{

  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  typedef typename boost::dispatch::meta::call<is_equal_(T,T)>::type r_t;
  typedef boost::simd::logical<T> wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(is_equal(boost::simd::One<T>(), boost::simd::One<T>()), r_t(true));
  NT2_TEST_EQUAL(is_equal(boost::simd::One<T>(),boost::simd::Zero<T>()), r_t(false));
  NT2_TEST_EQUAL(is_equal(boost::simd::Zero<T>(), boost::simd::Zero<T>()), r_t(true));
} // end of test for integer_

NT2_TEST_CASE_TPL ( is_equal_real,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  typedef typename boost::dispatch::meta::call<is_equal_(T,T)>::type r_t;
  typedef boost::simd::logical<T> wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(is_equal(boost::simd::Inf<T>(), boost::simd::Inf<T>()), r_t(true));
  NT2_TEST_EQUAL(is_equal(boost::simd::Minf<T>(), boost::simd::Minf<T>()), r_t(true));
  NT2_TEST_EQUAL(is_equal(boost::simd::Nan<T>(), boost::simd::Nan<T>()), r_t(false));
  NT2_TEST_EQUAL(is_equal(boost::simd::One<T>(),boost::simd::Zero<T>()), r_t(false));
  NT2_TEST_EQUAL(is_equal(boost::simd::Zero<T>(), boost::simd::Zero<T>()), r_t(true));
} // end of test for floating_

NT2_TEST_CASE_TPL ( is_equall,  BOOST_SIMD_SIMD_TYPES)
{

  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  typedef typename boost::dispatch::meta::call<is_equal_(T,T)>::type r_t;
  typedef boost::simd::logical<T> wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  NT2_TEST_EQUAL(is_equal(boost::simd::True< boost::simd::logical<T> >(), boost::simd::True< boost::simd::logical<T> >()), r_t(true));
  NT2_TEST_EQUAL(is_equal(boost::simd::False< boost::simd::logical<T> >(), boost::simd::False< boost::simd::logical<T> >()), r_t(true));
  NT2_TEST_EQUAL(is_equal(boost::simd::True< boost::simd::logical<T> >(), boost::simd::False< boost::simd::logical<T> >()), r_t(false));
  NT2_TEST_EQUAL(is_equal(boost::simd::False< boost::simd::logical<T> >(), boost::simd::True< boost::simd::logical<T> >()), r_t(false));
}

NT2_TEST_CASE ( is_equal_bool)
{
  using boost::simd::is_equal;
  using boost::simd::tag::is_equal_;
  typedef  boost::dispatch::meta::call<is_equal_(bool, bool)>::type r_t;
  typedef bool wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(is_equal(true, false), false);
  NT2_TEST_EQUAL(is_equal(false, true), false);
  NT2_TEST_EQUAL(is_equal(true, true), true);
  NT2_TEST_EQUAL(is_equal(false, false), true);
}


/**
 * @file LinearCost.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <string>

#include <Eigen/Dense>

#include <ScsEigen/LinearCost.h>
#include <ScsEigen/Logger.h>

using namespace ScsEigen;

LinearCost::LinearCost(const Eigen::Ref<const Eigen::VectorXd>& a, double b)
    : Cost(a.size(), "Linear cost")
    , m_a(a)
    , m_b(b)
{
}

bool LinearCost::setA(const Eigen::Ref<const Eigen::VectorXd>& a)
{
    if (m_a.size() != 0)
    {
        if (a.size() != m_a.size())
        {
            log()->error("[LinearCost::setA] The size of the vector 'a' cannot change.");
            return false;
        }
    } else if (!this->setNumberOfVariables(a.size()))
    {
        log()->error("[LinearCost::setA] Unable to set the number of variables.");
        return false;
    }

    m_a = a;
    return true;
}

void LinearCost::setB(double b)
{
    m_b = b;
}

Eigen::Ref<const Eigen::VectorXd> LinearCost::getA() const
{
    return m_a;
}

double LinearCost::getB() const
{
    return m_b;
}


// Copyright Nick Thompson, 2019
// 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_INTERPOLATORS_CARDINAL_QUADRATIC_B_SPLINE_HPP
#define BOOST_MATH_INTERPOLATORS_CARDINAL_QUADRATIC_B_SPLINE_HPP
#include <memory>
#include <boost/math/interpolators/detail/cardinal_quadratic_b_spline_detail.hpp>

namespace boost{ namespace math{ namespace interpolators {

template <class Real>
class cardinal_quadratic_b_spline
{
public:
    // If you don't know the value of the derivative at the endpoints, leave them as nans and the routine will estimate them.
    // y[0] = y(a), y[n - 1] = y(b), step_size = (b - a)/(n -1).
    cardinal_quadratic_b_spline(const Real* const y,
                                size_t n,
                                Real t0 /* initial time, left endpoint */,
                                Real h  /*spacing, stepsize*/,
                                Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
                                Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN())
     : impl_(std::make_shared<detail::cardinal_quadratic_b_spline_detail<Real>>(y, n, t0, h, left_endpoint_derivative, right_endpoint_derivative))
    {}

    // Oh the bizarre error messages if we template this on a RandomAccessContainer:
    cardinal_quadratic_b_spline(std::vector<Real> const & y,
                                Real t0 /* initial time, left endpoint */,
                                Real h  /*spacing, stepsize*/,
                                Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
                                Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN())
     : impl_(std::make_shared<detail::cardinal_quadratic_b_spline_detail<Real>>(y.data(), y.size(), t0, h, left_endpoint_derivative, right_endpoint_derivative))
    {}

    Real operator()(Real t) const {
        return impl_->operator()(t);
    }

    Real prime(Real t) const {
       return impl_->prime(t);
    }

    Real t_max() const {
        return impl_->t_max();
    }

private:
    std::shared_ptr<detail::cardinal_quadratic_b_spline_detail<Real>> impl_;
};

}}}
#endif