#pragma once

#include <Eigen/Core>

namespace GooBalls {

/**
 * A bunch of typedef for general use.
 * */

typedef double Float;
typedef Eigen::Matrix<Float, Eigen::Dynamic, 1, Eigen::ColMajor> Coordinates1d;
/// Each row is a coordinate tuple: (x,y)
typedef Eigen::Matrix<Float, Eigen::Dynamic, 2, Eigen::RowMajor> Coordinates2d;
/// Each row is a coordiante tuple: (x,y,z)
typedef Eigen::Matrix<Float, Eigen::Dynamic, 3, Eigen::RowMajor> Coordinates3d;

typedef Eigen::Matrix<Float, 2, 2, Eigen::RowMajor> RotationMatrix;
typedef Eigen::Matrix<Float, 1, 2, Eigen::RowMajor> TranslationVector;

/// Colors in the range [0,1]
typedef Float ColorFloat;

/// Holds a list of intensity values
typedef Eigen::Matrix<ColorFloat, Eigen::Dynamic, 1, Eigen::RowMajor> ColorsFloatGrey;

/// Holds a list of (intensity, alpha) values
typedef Eigen::Matrix<ColorFloat, Eigen::Dynamic, 2, Eigen::RowMajor> ColorsFloatGreyA;

/// Holds a list of (red, green, blue) values
typedef Eigen::Matrix<ColorFloat, Eigen::Dynamic, 3, Eigen::RowMajor> ColorsFloatRGB;

/// Holds a list of (red, green, blue, alpha) values
typedef Eigen::Matrix<ColorFloat, Eigen::Dynamic, 4, Eigen::RowMajor> ColorsFloatRGBA;

/// Colors in the range [0,255]
typedef char ColorInt;

/// Holds a list of intensity values
typedef Eigen::Matrix<ColorInt, Eigen::Dynamic, 1, Eigen::RowMajor> ColorsIntGrey;

/// Holds a list of (intensity, alpha) values
typedef Eigen::Matrix<ColorInt, Eigen::Dynamic, 2, Eigen::RowMajor> ColorsIntGreyA;

/// Holds a list of (red, green, blue) values
typedef Eigen::Matrix<ColorInt, Eigen::Dynamic, 3, Eigen::RowMajor> ColorsIntRGB;

/// Holds a list of (red, green, blue, alpha) values
typedef Eigen::Matrix<ColorInt, Eigen::Dynamic, 4, Eigen::RowMajor> ColorsIntRGBA;

typedef int VertexIndex;
typedef int TriangleIndex;

/// A list of triangles where each triangle corresponds to one row entry consisting of three indices to vertices.
typedef Eigen::Matrix<VertexIndex, Eigen::Dynamic, 3, Eigen::RowMajor> TriangleList;

} // GooBalls


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/function/fast.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD( rsqrt_
                         , (typename A0)
                         , bs::avx_
                         , bs::fast_tag
                         , bs::pack_< bd::single_<A0>, bs::avx_>
                         )
   {
     BOOST_FORCEINLINE A0 operator()( const bs::fast_tag &, A0 const& a0) const
      {
        return _mm256_rsqrt_ps( a0 );
      }
   };

  BOOST_DISPATCH_OVERLOAD( rsqrt_
                         , (typename A0)
                         , bs::avx_
                         , bs::pack_< bd::single_<A0>, bs::avx_>
                         )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a0) const
      {
        A0 const nr  = _mm256_rsqrt_ps( a0 );
        return nr * Half<A0>() * fnms(a0, sqr(nr), Three<A0>());
      }
   };
} } }

#endif


#include <stan/math/mix.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <vector>

TEST(ProbDistributionsMultinomial, fvar_var) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::fvar;
  using stan::math::var;
  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<fvar<var>, Dynamic, 1> theta(3, 1);
  theta << 0.2, 0.3, 0.5;
  for (int i = 0; i < 3; i++)
    theta(i).d_ = 1.0;

  EXPECT_FLOAT_EQ(-2.002481, stan::math::multinomial_log(ns, theta).val_.val());
  EXPECT_FLOAT_EQ(17.666666, stan::math::multinomial_log(ns, theta).d_.val());
}

TEST(ProbDistributionsMultinomial, fvar_fvar_var) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::fvar;
  using stan::math::var;
  std::vector<int> ns;
  ns.push_back(1);
  ns.push_back(2);
  ns.push_back(3);
  Matrix<fvar<fvar<var> >, Dynamic, 1> theta(3, 1);
  theta << 0.2, 0.3, 0.5;
  for (int i = 0; i < 3; i++)
    theta(i).d_.val_ = 1.0;

  EXPECT_FLOAT_EQ(-2.002481,
                  stan::math::multinomial_log(ns, theta).val_.val_.val());
  EXPECT_FLOAT_EQ(17.666666,
                  stan::math::multinomial_log(ns, theta).d_.val_.val());
}


#pragma once

#include <Eigen/Dense>
#include <vector>

class LinearModel
{
public:
    virtual Eigen::MatrixXd get_A(const std::vector<double>& x, const std::vector<double>& control) = 0;
    virtual Eigen::MatrixXd get_B(const std::vector<double>& x, const std::vector<double>& control) = 0;
    virtual Eigen::MatrixXd get_Xdot(const std::vector<double>& x, const std::vector<double>& control) = 0;
    static std::vector<double> flatten_matrix(Eigen::MatrixXd matrix);
    LinearModel(/* args */);
};


#include <stan/math/rev/arr.hpp>

#include <gtest/gtest.h>

#include <vector>

#include <boost/numeric/odeint.hpp>

// very small michaelis menten example
#include <test/unit/math/rev/arr/functor/coupled_mm.hpp>

#include <test/unit/util.hpp>

// test which triggers the too much work exception from odeint
TEST(StanOde_tooMuchWork_test, odeint_coupled_mm) {

  coupled_mm_ode_fun f_;

  // initial value and parameters from model definition
  std::vector<stan::math::var> y0_v(2);
  y0_v[0] = 1E5;
  y0_v[1] = 1E-1;

  double t0 = 0;

  std::vector<double> ts_long;
  ts_long.push_back(1E10);

  std::vector<double> ts_short;
  ts_short.push_back(1);

  std::vector<stan::math::var> theta_v(4);

  theta_v[0] = 1.0;
  theta_v[1] = 0.5;
  theta_v[2] = 0.5;
  theta_v[3] = 0.1;

  std::vector<double> data;

  std::vector<int> data_int;

  EXPECT_THROW_MSG(stan::math::integrate_ode_rk45(f_, y0_v, t0, ts_long, theta_v, data, data_int, 0, 1E-6, 1E-6, 100),
                   boost::numeric::odeint::no_progress_error,
                   "Max number of iterations exceeded (100).");

  EXPECT_NO_THROW(stan::math::integrate_ode_rk45(f_, y0_v, t0, ts_short, theta_v, data, data_int, 0, 1E-6, 1E-6, 100));

}


//Eigen
#include <Eigen/Core>

//OpenCV
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>

//PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <chrono>
#include <iostream>

#include "estimate_motion.h"

using namespace p3dv;

bool MotionEstimator::estimate2D2D_E5P_RANSAC(frame_t &cur_frame_1, frame_t &cur_frame_2,
                                              std::vector<cv::DMatch> &matches, std::vector<cv::DMatch> &inlier_matches,
                                              Eigen::Matrix4f &T, double ransac_thre, double ransac_prob, bool show)
{
    std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();

    std::vector<cv::Point2f> pointset1;
    std::vector<cv::Point2f> pointset2;

    for (int i = 0; i < (int)matches.size(); i++)
    {
        pointset1.push_back(cur_frame_1.keypoints[matches[i].queryIdx].pt);
        pointset2.push_back(cur_frame_2.keypoints[matches[i].trainIdx].pt);
    }

    cv::Mat camera_mat;
    cv::eigen2cv(cur_frame_1.K_cam, camera_mat);

    cv::Mat essential_matrix;

    cv::Mat inlier_matches_indicator;

    essential_matrix = cv::findEssentialMat(pointset1, pointset2, camera_mat,
                                            CV_RANSAC, ransac_prob, ransac_thre, inlier_matches_indicator);

    std::cout << "essential_matrix is " << std::endl
              << essential_matrix << std::endl;

    for (int i = 0; i < (int)matches.size(); i++)
    {
        if (inlier_matches_indicator.at<bool>(0, i) == 1)
        {
            inlier_matches.push_back(matches[i]);
        }
    }

    cv::Mat R;
    cv::Mat t;

    cv::recoverPose(essential_matrix, pointset1, pointset2, camera_mat, R, t, inlier_matches_indicator);

    std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(toc - tic);
    std::cout << "Estimate Motion [2D-2D] cost = " << time_used.count() << " seconds. " << std::endl;
    std::cout << "Find [" << inlier_matches.size() << "] inlier matches from [" << matches.size() << "] total matches." << std::endl;

    Eigen::Matrix3f R_eigen;
    Eigen::Vector3f t_eigen;
    cv::cv2eigen(R, R_eigen);
    cv::cv2eigen(t, t_eigen);
    T.block(0, 0, 3, 3) = R_eigen;
    T.block(0, 3, 3, 1) = t_eigen;
    T(3, 0) = 0;
    T(3, 1) = 0;
    T(3, 2) = 0;
    T(3, 3) = 1;

    std::cout << "Transform is " << std::endl
              << T << std::endl;

    if (show)
    {
        cv::Mat ransac_match_image;
        cv::namedWindow("RANSAC inlier matches", 0);
        cv::drawMatches(cur_frame_1.rgb_image, cur_frame_1.keypoints, cur_frame_2.rgb_image, cur_frame_2.keypoints, inlier_matches, ransac_match_image);
        cv::imshow("RANSAC inlier matches", ransac_match_image);
        cv::waitKey(0);
    }

    return 1;
}

bool MotionEstimator::estimate2D3D_P3P_RANSAC(frame_t &cur_frame, pointcloud_sparse_t &cur_map_3d, double ransac_thre,
                                              int iterationsCount, double ransac_prob, bool show)
{
    std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();

    cv::Mat camera_mat;
    cv::eigen2cv(cur_frame.K_cam, camera_mat);

    //std::cout<< "K Mat:" <<std::endl<< camera_mat<<std::endl;

    std::vector<cv::Point2f> pointset2d;
    std::vector<cv::Point3f> pointset3d;
    std::vector<int> pointset3d_index;

    int count = 0;
    float dist_thre = 300;

    std::cout << "2D points: " << cur_frame.unique_pixel_ids.size() << std::endl
              << "3D points: " << cur_map_3d.unique_point_ids.size() << std::endl;

    // Construct the 2d-3d initial matchings
    for (int i = 0; i < cur_frame.unique_pixel_ids.size(); i++)
    {
        for (int j = 0; j < cur_map_3d.unique_point_ids.size(); j++)
        {
            if (cur_frame.unique_pixel_ids[i] == cur_map_3d.unique_point_ids[j])
            {

                float x_3d = cur_map_3d.rgb_pointcloud->points[j].x;
                float y_3d = cur_map_3d.rgb_pointcloud->points[j].y;
                float z_3d = cur_map_3d.rgb_pointcloud->points[j].z;
                float x_2d = cur_frame.keypoints[i].pt.x;
                float y_2d = cur_frame.keypoints[i].pt.y;

                if (std::abs(x_3d) < dist_thre && std::abs(y_3d) < dist_thre && std::abs(z_3d) < dist_thre)
                {
                    // Assign value for pointset2d and pointset3d
                    pointset2d.push_back(cv::Point2f(x_2d, y_2d));
                    //pointset2d.push_back(pixel2cam(cur_frame.keypoints[i].pt, camera_mat));

                    pointset3d.push_back(cv::Point3f(x_3d, y_3d, z_3d));
                    pointset3d_index.push_back(j);

                    count++;
                }
                //std::cout << "2D: " << cur_frame.unique_pixel_ids[i] << " " << cur_frame.keypoints[i].pt << std::endl;
                //std::cout << "3D: " << cur_map_3d.unique_point_ids[j] << " " << cv::Point3f(x_3d, y_3d, z_3d) << std::endl;
            }
        }
    }

    std::cout << count << " initial correspondences are used." << std::endl;

    // Use RANSAC P3P to estiamte the optimal transformation

    cv::Mat distort_para = cv::Mat::zeros(1, 4, CV_64FC1); // Assuming no lens distortion
    cv::Mat r_vec;
    cv::Mat t_vec;
    cv::Mat inliers;

    cv::solvePnPRansac(pointset3d, pointset2d, camera_mat, distort_para, r_vec, t_vec,
                       false, iterationsCount, ransac_thre, ransac_prob, inliers, cv::SOLVEPNP_EPNP);

    //cv::solvePnP(pointset3d, pointset2d, camera_mat, distort_para, r_vec, t_vec, false, cv::SOLVEPNP_EPNP);

    std::cout << "Inlier count: " << inliers.rows << std::endl;

    std::vector<int> outlier;
    for (int i = 0; i < inliers.rows; i++)
    {
        pointset3d_index[inliers.at<float>(i, 0)] = -1; // inlier 's index
    }

    for (int i = 0; i < pointset3d_index.size(); i++)
    {
        if (pointset3d_index[i] >= 0)
            outlier.push_back(pointset3d_index[i]);
    }
    for (int i = 0; i < outlier.size(); i++)
    {
        cur_map_3d.is_inlier[outlier[i]] = 0;
    }

    cv::Mat R_mat;
    cv::Rodrigues(r_vec, R_mat);

    Eigen::Matrix3f R_eigen;
    Eigen::Vector3f t_eigen;
    Eigen::Matrix4f T_mat;

    cv::cv2eigen(R_mat, R_eigen);
    cv::cv2eigen(t_vec, t_eigen);

    T_mat.block(0, 0, 3, 3) = R_eigen;
    T_mat.block(0, 3, 3, 1) = t_eigen;
    T_mat(3, 0) = 0;
    T_mat(3, 1) = 0;
    T_mat(3, 2) = 0;
    T_mat(3, 3) = 1;

    // std::cout << "Transform is: " << std::endl
    //           << T_mat << std::endl;

    cur_frame.pose_cam = T_mat;

    // Calculate the reprojection error
    std::vector<cv::Point2f> proj_points;
    cv::projectPoints(pointset3d, R_mat, t_vec, camera_mat, distort_para, proj_points);

    float reproj_err = 0.0;
    for (int i = 0; i < proj_points.size(); i++)
    {
        float cur_repro_error = norm(proj_points[i] - pointset2d[i]);
        reproj_err += cur_repro_error;

        //std::cout << cur_repro_error << std::endl;
    }

    reproj_err /= proj_points.size();
    double inlier_ratio = 1.0 * inliers.rows / count;
    std::cout << "Mean reprojection error: " << reproj_err << std::endl;

    std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(toc - tic);
    std::cout << "Estimate Motion [3D-2D] cost = " << time_used.count() << " seconds. " << std::endl;

    if (reproj_err > 10 && inlier_ratio < 0.5) // mean reprojection error is too big (may be some problem)
    {
        std::cout << "[Warning] pnp may encounter some problem, the inlier ratio is [ " << inlier_ratio * 100 << " % ], the mean reprojection error is [ " << reproj_err << " ]." << std::endl;
        return 0;
    }
    else
        return 1;
}

bool MotionEstimator::getDepthFast(frame_t &cur_frame_1, frame_t &cur_frame_2, Eigen::Matrix4f &T_21,
                                   const std::vector<cv::DMatch> &matches, double &appro_depth, int random_rate)
{
    cv::Mat T1_mat;
    cv::Mat T2_mat;
    cv::Mat camera_mat;

    Eigen::Matrix4f Teye = Eigen::Matrix4f::Identity();
    Eigen::Matrix<float, 3, 4> T1 = Teye.block<3, 4>(0, 0);
    Eigen::Matrix<float, 3, 4> T2 = (T_21 * Teye).block<3, 4>(0, 0);

    cv::eigen2cv(cur_frame_1.K_cam, camera_mat);
    cv::eigen2cv(T1, T1_mat);
    cv::eigen2cv(T2, T2_mat);

    std::vector<cv::Point2f> pointset1;
    std::vector<cv::Point2f> pointset2;

    for (int i = 0; i < matches.size(); i++)
    {
        if (i % random_rate == 0)
        {
            pointset1.push_back(pixel2cam(cur_frame_1.keypoints[matches[i].queryIdx].pt, camera_mat));
            pointset2.push_back(pixel2cam(cur_frame_2.keypoints[matches[i].trainIdx].pt, camera_mat));
        }
    }

    cv::Mat pts_3d_homo;
    if (pointset1.size() > 0)
        cv::triangulatePoints(T1_mat, T2_mat, pointset1, pointset2, pts_3d_homo);

    // De-homo and calculate mean depth
    double depth_sum = 0;
    for (int i = 0; i < pts_3d_homo.cols; i++)
    {
        cv::Mat pts_3d = pts_3d_homo.col(i);

        pts_3d /= pts_3d.at<float>(3, 0);

        Eigen::Vector3f pt_temp;
        pt_temp(0) = pts_3d.at<float>(0, 0);
        pt_temp(1) = pts_3d.at<float>(1, 0);
        pt_temp(2) = pts_3d.at<float>(2, 0);

        depth_sum = depth_sum + pt_temp.norm();
    }
    appro_depth = depth_sum / pts_3d_homo.cols;

    std::cout << "Mean relative depth is about " << appro_depth << " * baseline length. " << std::endl;
}

bool MotionEstimator::doTriangulation(frame_t &cur_frame_1, frame_t &cur_frame_2,
                                      const std::vector<cv::DMatch> &matches,
                                      pointcloud_sparse_t &sparse_pointcloud, bool show)
{
    std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();

    cv::Mat T1_mat;
    cv::Mat T2_mat;
    cv::Mat camera_mat;

    Eigen::Matrix<float, 3, 4> T1 = cur_frame_1.pose_cam.block(0, 0, 3, 4);
    Eigen::Matrix<float, 3, 4> T2 = cur_frame_2.pose_cam.block(0, 0, 3, 4);

    cv::eigen2cv(cur_frame_1.K_cam, camera_mat);
    cv::eigen2cv(T1, T1_mat);
    cv::eigen2cv(T2, T2_mat);

    // std::cout<<camera_mat<<std::endl;
    // std::cout<<T1_mat<<std::endl;
    // std::cout<<T2_mat<<std::endl;

    std::vector<cv::Point2f> pointset1;
    std::vector<cv::Point2f> pointset2;

    int count_newly_triangu = 0;
    for (int i = 0; i < matches.size(); i++)
    {
        bool already_in_world = 0;
        for (int k = 0; k < sparse_pointcloud.unique_point_ids.size(); k++)
        {
            if (sparse_pointcloud.unique_point_ids[k] == cur_frame_1.unique_pixel_ids[matches[i].queryIdx])
            {
                already_in_world = 1;
                break;
            }
        }
        if (!already_in_world)
        {
            sparse_pointcloud.unique_point_ids.push_back(cur_frame_1.unique_pixel_ids[matches[i].queryIdx]);
            sparse_pointcloud.is_inlier.push_back(1);

            pointset1.push_back(pixel2cam(cur_frame_1.keypoints[matches[i].queryIdx].pt, camera_mat));
            pointset2.push_back(pixel2cam(cur_frame_2.keypoints[matches[i].trainIdx].pt, camera_mat));

            count_newly_triangu++;
        }
    }

    cv::Mat pts_3d_homo;
    if (pointset1.size() > 0)
        cv::triangulatePoints(T1_mat, T2_mat, pointset1, pointset2, pts_3d_homo);

    // De-homo and assign color
    for (int i = 0; i < pts_3d_homo.cols; i++)
    {
        cv::Mat pts_3d = pts_3d_homo.col(i);

        pts_3d /= pts_3d.at<float>(3, 0);

        pcl::PointXYZRGB pt_temp;
        pt_temp.x = pts_3d.at<float>(0, 0);
        pt_temp.y = pts_3d.at<float>(1, 0);
        pt_temp.z = pts_3d.at<float>(2, 0);

        // check here if(pt_temp.x> )

        cv::Point2f cur_key_pixel = cur_frame_1.keypoints[matches[i].queryIdx].pt;

        uchar blue = cur_frame_1.rgb_image.at<cv::Vec3b>(cur_key_pixel.y, cur_key_pixel.x)[0];
        uchar green = cur_frame_1.rgb_image.at<cv::Vec3b>(cur_key_pixel.y, cur_key_pixel.x)[1];
        uchar red = cur_frame_1.rgb_image.at<cv::Vec3b>(cur_key_pixel.y, cur_key_pixel.x)[2];

        pt_temp.r = 1.0 * red;
        pt_temp.g = 1.0 * green;
        pt_temp.b = 1.0 * blue;
        sparse_pointcloud.rgb_pointcloud->points.push_back(pt_temp);
    }

    std::cout << "Triangulate [ " << count_newly_triangu << " ] new points, [ " << sparse_pointcloud.rgb_pointcloud->points.size() << " ] points in total." << std::endl;

    std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(toc - tic);
    std::cout << "Triangularization done in " << time_used.count() << " seconds. " << std::endl;

    if (show)
    {
        // Show 2D image pair and correspondences
        cv::Mat match_image_pair;
        cv::drawMatches(cur_frame_1.rgb_image, cur_frame_1.keypoints, cur_frame_2.rgb_image, cur_frame_2.keypoints, matches, match_image_pair);
        cv::imshow("Triangularization matches", match_image_pair);
        cv::waitKey(0);

        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Sfm Viewer"));
        viewer->setBackgroundColor(0, 0, 0);

        // Draw camera
        char t[256];
        std::string s;
        int n = 0;
        float frame_color_r, frame_color_g, frame_color_b;
        float sphere_size = 0.2;
        float line_size_cam = 0.4;

        pcl::PointXYZ pt_cam1(cur_frame_1.pose_cam(0, 3), cur_frame_1.pose_cam(1, 3), cur_frame_1.pose_cam(2, 3));
        pcl::PointXYZ pt_cam2(cur_frame_2.pose_cam(0, 3), cur_frame_2.pose_cam(1, 3), cur_frame_2.pose_cam(2, 3));

        sprintf(t, "%d", n);
        s = t;
        viewer->addSphere(pt_cam1, sphere_size, 1.0, 0.0, 0.0, s);
        n++;

        sprintf(t, "%d", n);
        s = t;
        viewer->addSphere(pt_cam2, sphere_size, 0.0, 0.0, 1.0, s);
        n++;

        sprintf(t, "%d", n);
        s = t;
        viewer->addLine(pt_cam1, pt_cam2, 0.0, 1.0, 0.0, s);
        n++;

        // for (int i = 0; i < sparse_pointcloud->points.size(); i++)
        // {
        //     char sparse_point[256];
        //     pcl::PointXYZ ptc_temp;
        //     ptc_temp.x = sparse_pointcloud->points[i].x;
        //     ptc_temp.y = sparse_pointcloud->points[i].y;
        //     ptc_temp.z = sparse_pointcloud->points[i].z;
        //     sprintf(sparse_point, "SP_%03u", i);
        //     viewer->addSphere(ptc_temp, 0.2, 1.0, 0.0, 0.0, sparse_point);
        // }

        viewer->addPointCloud(sparse_pointcloud.rgb_pointcloud, "sparsepointcloud");

        std::cout << "Click X(close) to continue..." << std::endl;
        while (!viewer->wasStopped())
        {
            viewer->spinOnce(100);
            boost::this_thread::sleep(boost::posix_time::microseconds(100000));
        }
    }

    std::cout << "Generate new sparse point cloud done." << std::endl;
    return true;
}

bool MotionEstimator::doUnDistort(frame_t &cur_frame, cv::Mat distort_coeff)
{
    cv::Mat camera_mat;
    cv::eigen2cv(cur_frame.K_cam, camera_mat);
    cv::Mat undistorted_img;
    cv::undistort(cur_frame.rgb_image, undistorted_img, camera_mat, distort_coeff);
    cur_frame.rgb_image = undistorted_img;

    std::cout << "Undistort the image done." << std::endl;
    return true;
}

/**
* \brief Transform a Point Cloud using a given transformation matrix
* \param[in]  cloud_in : A pointer of the Point Cloud before transformation
* \param[out] cloud_out : A pointer of the Point Cloud after transformation
* \param[in]  trans : A 4*4 transformation matrix
*/
bool MotionEstimator::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in,
                                     pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out,
                                     Eigen::Matrix4f &trans)
{
    Eigen::Matrix4Xf PC;
    Eigen::Matrix4Xf TPC;
    PC.resize(4, cloud_in->size());
    TPC.resize(4, cloud_in->size());
    for (int i = 0; i < cloud_in->size(); i++)
    {
        PC(0, i) = cloud_in->points[i].x;
        PC(1, i) = cloud_in->points[i].y;
        PC(2, i) = cloud_in->points[i].z;
        PC(3, i) = 1;
    }
    TPC = trans * PC;
    for (int i = 0; i < cloud_in->size(); i++)
    {
        pcl::PointXYZ pt;
        pt.x = TPC(0, i);
        pt.y = TPC(1, i);
        pt.z = TPC(2, i);
        cloud_out->points.push_back(pt);
    }
    //cout << "Transform done ..." << endl;
}

bool MotionEstimator::outlierFilter(pointcloud_sparse_t &sparse_pointcloud, int MeanK, double std)
{
    // Create the filtering object
    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;

    std::vector<int> filtered_indice;

    sor.setInputCloud(sparse_pointcloud.rgb_pointcloud);
    sor.setMeanK(MeanK);         //50
    sor.setStddevMulThresh(std); //1.0
    sor.filter(filtered_indice);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    std::vector<int> output_unique_points_id;
    std::vector<int> output_is_inlier;
    for (int i = 0; i < filtered_indice.size(); i++)
    {
        output_pointcloud->points.push_back(sparse_pointcloud.rgb_pointcloud->points[filtered_indice[i]]);
        output_unique_points_id.push_back(sparse_pointcloud.unique_point_ids[filtered_indice[i]]);
        output_is_inlier.push_back(sparse_pointcloud.is_inlier[filtered_indice[i]]);
    }

    std::cout << "apply outlier filter: [ " << sparse_pointcloud.unique_point_ids.size() << " ] points before filtering, [ " << filtered_indice.size() << " ] points after filtering." << std::endl;

    sparse_pointcloud.rgb_pointcloud->points.swap(output_pointcloud->points);
    sparse_pointcloud.unique_point_ids.swap(output_unique_points_id);
    sparse_pointcloud.is_inlier.swap(output_is_inlier);

    return 1;
}

bool MotionEstimator::estimateE8Points(std::vector<cv::KeyPoint> &keypoints1,
                                       std::vector<cv::KeyPoint> &keypoints2,
                                       std::vector<cv::DMatch> &matches,
                                       Eigen::Matrix3f &K,
                                       Eigen::Matrix4f &T)
{
#if 0
    std::vector<cv::Point2f> pointset1;
    std::vector<cv::Point2f> pointset2;

    for (int i = 0; i < (int)matches.size(); i++)
    {
        pointset1.push_back(keypoints1[matches[i].queryIdx].pt);
        pointset2.push_back(keypoints2[matches[i].trainIdx].pt);
    }

    cv::Point2d principal_point(K(0, 2), K(1, 2));
    double focal_length = (K(0, 0) + K(1, 1)) * 0.5;
    cv::Mat essential_matrix;
    essential_matrix = cv::findEssentialMat(pointset1, pointset2, focal_length, principal_point);
    std::cout << "essential_matrix is " << std::endl
              << essential_matrix << std::endl;

    cv::Mat R;
    cv::Mat t;

    cv::recoverPose(essential_matrix, pointset1, pointset2, R, t, focal_length, principal_point);

    Eigen::Matrix3f R_eigen;
    Eigen::Vector3f t_eigen;
    cv::cv2eigen(R, R_eigen);
    cv::cv2eigen(t, t_eigen);
    T.block(0, 0, 3, 3) = R_eigen;
    T.block(0, 3, 3, 1) = t_eigen;
    T(3, 0) = 0;
    T(3, 1) = 0;
    T(3, 2) = 0;
    T(3, 3) = 1;

    std::cout << "Transform is " << std::endl
              << T << std::endl;
#endif
}


/*
 Copyright (C) 2018 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 "toplevelfixture.hpp"
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ql/currencies/america.hpp>
#include <ql/settings.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>

#include <qle/pricingengines/discountingcommodityforwardengine.hpp>
#include <qle/termstructures/pricecurve.hpp>

using namespace std;
using namespace boost::unit_test_framework;
using namespace QuantLib;
using namespace QuantExt;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(DiscountingCommodityForwardEngineTest)

BOOST_AUTO_TEST_CASE(testPricing) {

    BOOST_TEST_MESSAGE("Testing discounting commodity forward engine pricing");

    SavedSettings backup;

    // Tolerance for npv comparison
    Real tolerance = 1e-10;

    // Commodity forward base data
    Date asof(19, Feb, 2018);
    string name("GOLD_USD");
    USDCurrency currency;
    Date maturity(19, Feb, 2019);

    // Day counter for time
    Actual365Fixed dayCounter;

    // Discount curve
    vector<Date> dates(2);
    vector<DiscountFactor> dfs(2);
    dates[0] = asof;
    dfs[0] = 1.0;
    dates[1] = maturity;
    dfs[1] = 0.85;
    Handle<YieldTermStructure> discountCurve(boost::make_shared<InterpolatedDiscountCurve<LogLinear> >(
        InterpolatedDiscountCurve<LogLinear>(dates, dfs, dayCounter)));

    // Commodity Price curve
    vector<Real> prices(2);
    prices[0] = 1346.0;
    prices[1] = 1384.0;
    Handle<PriceTermStructure> priceCurve(
        boost::make_shared<InterpolatedPriceCurve<Linear> >(InterpolatedPriceCurve<Linear>(dates, prices, dayCounter)));

    // Create the engine
    boost::shared_ptr<DiscountingCommodityForwardEngine> engine =
        boost::make_shared<DiscountingCommodityForwardEngine>(priceCurve, discountCurve);

    // Set evaluation date
    Settings::instance().evaluationDate() = asof;

    // Long 100 units with strike 1380.0
    Position::Type position = Position::Long;
    Real quantity = 100;
    Real strike = 1380.0;
    boost::shared_ptr<CommodityForward> forward =
        boost::make_shared<CommodityForward>(name, currency, position, quantity, maturity, strike);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), quantity * (prices[1] - strike) * dfs[1], tolerance);

    // Short 100 units with strike 1380.0
    position = Position::Short;
    forward = boost::make_shared<CommodityForward>(name, currency, position, quantity, maturity, strike);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), -quantity * (prices[1] - strike) * dfs[1], tolerance);

    // Short 50 units with strike 1380.0
    quantity = 50.0;
    forward = boost::make_shared<CommodityForward>(name, currency, position, quantity, maturity, strike);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), -quantity * (prices[1] - strike) * dfs[1], tolerance);

    // Short 50 units with strike 1375.0
    strike = 1375.0;
    forward = boost::make_shared<CommodityForward>(name, currency, position, quantity, maturity, strike);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), -quantity * (prices[1] - strike) * dfs[1], tolerance);

    // Bring maturity of forward in 6 months
    maturity = Date(19, Aug, 2018);
    forward = boost::make_shared<CommodityForward>(name, currency, position, quantity, maturity, strike);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(),
                      -quantity * (priceCurve->price(maturity) - strike) * discountCurve->discount(maturity),
                      tolerance);

    // Make maturity of forward equal to today
    forward = boost::make_shared<CommodityForward>(name, currency, position, quantity, asof, strike);
    // includeReferenceDateEvents_ of Settings is false by default => value should equal 0
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), 0.0, tolerance);
    // Set includeReferenceDateEvents_ of Settings to true => value should be today's price - strike
    Settings::instance().includeReferenceDateEvents() = true;
    forward->recalculate();
    BOOST_CHECK_CLOSE(forward->NPV(), -quantity * (prices[0] - strike), tolerance);
    // Override behaviour in engine i.e. don't include flows on reference date even when
    // Settings::instance().includeReferenceDateEvents() is true
    engine = boost::make_shared<DiscountingCommodityForwardEngine>(priceCurve, discountCurve, false);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), 0.0, tolerance);
    // Trying the other way around should not work as the trade is marked as expired
    Settings::instance().includeReferenceDateEvents() = false;
    engine = boost::make_shared<DiscountingCommodityForwardEngine>(priceCurve, discountCurve, true);
    forward->setPricingEngine(engine);
    BOOST_CHECK_CLOSE(forward->NPV(), 0.0, tolerance);

    // Reinstate original maturity and change the npv date in the engine to 2 days after asof
    maturity = Date(19, Feb, 2019);
    forward = boost::make_shared<CommodityForward>(name, currency, position, quantity, maturity, strike);
    Date npvDate = asof + 2 * Days;
    engine = boost::make_shared<DiscountingCommodityForwardEngine>(priceCurve, discountCurve, boost::none, npvDate);
    forward->setPricingEngine(engine);
    DiscountFactor npvDateDiscount = discountCurve->discount(npvDate);
    BOOST_CHECK_CLOSE(forward->NPV(), -quantity * (prices[1] - strike) * dfs[1] / npvDateDiscount, tolerance);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include "engine/douglas_peucker.hpp"
#include "util/coordinate.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/integer_range.hpp"
#include "util/web_mercator.hpp"

#include <boost/assert.hpp>

#include <algorithm>
#include <cmath>
#include <iterator>
#include <stack>
#include <utility>

namespace osrm
{
namespace engine
{

// Normed to the thresholds table
std::uint64_t fastPerpendicularDistance(const util::FloatCoordinate &projected_start,
                                        const util::FloatCoordinate &projected_target,
                                        const util::FloatCoordinate &projected)
{
    util::FloatCoordinate projected_point_on_segment;
    std::tie(std::ignore, projected_point_on_segment) =
        util::coordinate_calculation::projectPointOnSegment(
            projected_start, projected_target, projected);
    auto squared_distance = util::coordinate_calculation::squaredEuclideanDistance(
        projected, projected_point_on_segment);
    return squared_distance;
}

std::vector<util::Coordinate> douglasPeucker(std::vector<util::Coordinate>::const_iterator begin,
                                             std::vector<util::Coordinate>::const_iterator end,
                                             const unsigned zoom_level)
{
    BOOST_ASSERT_MSG(zoom_level < detail::DOUGLAS_PEUCKER_THRESHOLDS_SIZE,
                     "unsupported zoom level");

    const std::size_t size = std::distance(begin, end);
    if (size < 2)
    {
        return {};
    }

    std::vector<util::FloatCoordinate> projected_coordinates(size);
    std::transform(begin, end, projected_coordinates.begin(), [](const util::Coordinate coord) {
        return util::web_mercator::fromWGS84(coord);
    });

    std::vector<bool> is_necessary(size, false);
    BOOST_ASSERT(is_necessary.size() >= 2);
    is_necessary.front() = true;
    is_necessary.back() = true;
    using GeometryRange = std::pair<std::size_t, std::size_t>;

    std::stack<GeometryRange> recursion_stack;

    recursion_stack.emplace(0UL, size - 1);

    // mark locations as 'necessary' by divide-and-conquer
    while (!recursion_stack.empty())
    {
        // pop next element
        const GeometryRange pair = recursion_stack.top();
        recursion_stack.pop();
        // sanity checks
        BOOST_ASSERT_MSG(is_necessary[pair.first], "left border must be necessary");
        BOOST_ASSERT_MSG(is_necessary[pair.second], "right border must be necessary");
        BOOST_ASSERT_MSG(pair.second < size, "right border outside of geometry");
        BOOST_ASSERT_MSG(pair.first <= pair.second, "left border on the wrong side");

        std::uint64_t max_distance = 0;
        auto farthest_entry_index = pair.second;

        // sweep over range to find the maximum
        for (auto idx = pair.first + 1; idx != pair.second; ++idx)
        {
            using namespace util::coordinate_calculation;
            const auto distance = fastPerpendicularDistance(projected_coordinates[pair.first],
                                                            projected_coordinates[pair.second],
                                                            projected_coordinates[idx]);
            // found new feasible maximum?
            if (distance > max_distance &&
                distance > detail::DOUGLAS_PEUCKER_THRESHOLDS[zoom_level])
            {
                farthest_entry_index = idx;
                max_distance = distance;
            }
        }

        // check if maximum violates a zoom level dependent threshold
        if (max_distance > detail::DOUGLAS_PEUCKER_THRESHOLDS[zoom_level])
        {
            //  mark idx as necessary
            is_necessary[farthest_entry_index] = true;
            if (pair.first < farthest_entry_index)
            {
                recursion_stack.emplace(pair.first, farthest_entry_index);
            }
            if (farthest_entry_index < pair.second)
            {
                recursion_stack.emplace(farthest_entry_index, pair.second);
            }
        }
    }

    auto simplified_size = std::count(is_necessary.begin(), is_necessary.end(), true);
    std::vector<util::Coordinate> simplified_geometry;
    simplified_geometry.reserve(simplified_size);
    for (auto idx : util::irange<std::size_t>(0UL, size))
    {
        if (is_necessary[idx])
        {
            simplified_geometry.push_back(begin[idx]);
        }
    }

    return simplified_geometry;
}
} // namespace engine
} // namespace osrm


// #pragma GCC optimize ("O0")

#include <boost/numeric/odeint.hpp>       // odeint function definitions

using namespace boost::numeric::odeint;

#include "eigenIncluder.hpp"
#include "forceModels.hpp"
#include "streamTrace.hpp"
#include "instrument.hpp"
#include "navigation.hpp"
#include "satRefSys.hpp"
#include "constants.hpp"
#include "acsConfig.hpp"
#include "gravity.hpp"
#include "common.hpp"
#include "jplEph.hpp"
#include "tides.hpp"
#include "enums.h"

typedef Eigen::Vector<double, 48> VectorXSTMS;			//todo aaron, delete this

map<int, OrbitPropagator>	orbitPropagatorMap;

GravityModel OrbitPropagator::gravityModel;

/* Frac: Gives the fractional part of a number
*/
double Frac (double x)
{
	return x - floor(x);
}

/* Computes the Sun's geocentric position using a low precision analytical series
*/
Vector3d SunPos(
	double mjdTT)		///< Terrestrial time: modified Julian date
{
	/* Constants
	*/
	const double eps	= 23.43929111 * D2R;				// Obliquity of J2000 ecliptic
	const double T		= (mjdTT - mjdJ2000) / 36525.0;		// Julian cent. since J2000

	/* Mean anomaly, ecliptic longitude and radius
	*/
	double M = PI2 * Frac ( 0.9931267 + 99.9973583 * T);										// [rad]
	double L = PI2 * Frac ( 0.7859444 + M/PI2 + ( 6892 * sin(M) + 72 * sin(2 * M) ) / 1296e3 );	// [rad]
	double R = 149.619e9 - 2.499e9 * cos(M) - 0.021e9 * cos(2 * M);								// [m]

	/* Solar position vector [m] with respect to the mean equator and equinox of J2000 (EME2000, ICRF)
	*/
	return R_x(-eps) * R * Vector3d(cos(L), sin(L), 0);
}

/* Computes the fractional illumination of a spacecraft in the vicinity of the Earth assuming a cylindrical shadow model
*
*/
double Illumination(
	const Vector3d& 		rSat,			///< Spacecraft position vector [m]
	const Vector3d& 		rSun )			///< Sun position vector [m]
{
	Vector3d eSun = rSun / rSun.norm();    // Sun direction unit vector
	double s     	= rSat.dot(eSun);      // Projection of s/c position

	/* Illumination factor:
	*  1: Spacecraft fully illuminated by the Sun
	*  0: Spacecraft in Earth shadow
	*/
	return ( ( s > 0 || (rSat - s * eSun).norm() > RE_WGS84 ) ?  1 : 0 );
}

/* Class type: The class of solar radiation pressure
*
*/
SolarRadPressure::SolarRadPressure(
	SRPPara 				paraSRP)
{
	mSRPPara		= paraSRP;
}

Vector3d SolarRadPressure::directSolarRadiationAcc(
	Trace&				trace,		 				///< Trace to output to
	double 				mjdTT,		 				///< Terrestrial time (modified Julian date)
	const Vector3d& 	rSat)        				///< Satellite position vector, unit: m, m/s
{

	/* Relative position vector of spacecraft w.r.t. Sun
	*/
	Vector3d rSun;
	jplEphPos(nav.jplEph_ptr, mjdTT + JD2MJD, E_ThirdBody::SUN, rSun);

	// trace << "Calculated sun position: " << std::setw(14) << mjdTT << std::setw(14) << rSun.transpose() << std::endl;

	Vector3d rDis = rSat - rSun;
	double illumination = Illumination(rSat, rSun);

	switch (mSRPPara.srpMdlName)
	{
		case E_SRPModels::CANNONBALL:		return illumination * mSRPPara.srpCoef * (mSRPPara.srpArea / mSRPPara.satMass) * PSOL * (AU * AU) * rDis / pow(rDis.norm(), 3);
		case E_SRPModels::BOXWING:			return Vector3d::Zero();
		case E_SRPModels::ECOM:				return Vector3d::Zero();
		case E_SRPModels::ECOM2:			return Vector3d::Zero();
		default:							return Vector3d::Zero();
	}
}

Vector3d SolarRadPressure::indirectSolarRadiationAcc(
	Trace&				trace,		 					///< Trace to output to (similar to cout)
	double 				mjdTT,		 					///< Terrestrial time (modified Julian date)
	const Vector3d& 	rSat)        					///< Satellite position vector, unit: m, m/s
{
	return Vector3d::Zero();
}

Vector3d antennarThrustAcc()
{
	return Vector3d::Zero();
}

Vector3d empiricalAcc()
{
	return Vector3d::Zero();
}

Vector3d manoeuvreAcc()
{
	return Vector3d::Zero();
}

/* Set options for force models in the propagator
*
*/
void OrbitPropagator::setPropOption(
	ForceModels				forceMdl)
{
	/* Options from yaml file */
	propOpt.optEarthGravMdl.earthGravMdl		= E_GravMdl::GGM03S;

	/* Parameters from yaml file */
	propOpt.optEarthGravMdl.earthGravAccDeg.mMax = forceMdl.egmAccDeg;
	propOpt.optEarthGravMdl.earthGravAccDeg.nMax = forceMdl.egmAccOrd;
	propOpt.optEarthGravMdl.earthGravSTMDeg.mMax = forceMdl.egmSTMDeg;
	propOpt.optEarthGravMdl.earthGravSTMDeg.nMax = forceMdl.egmSTMOrd;

	propOpt.paraSRP.srpMdlName	= forceMdl.srp_model;
	propOpt.paraSRP.satMass		= forceMdl.sat_mass;
	propOpt.paraSRP.srpArea		= forceMdl.srp_area;
	propOpt.paraSRP.srpCoef		= forceMdl.srp_coef;
}

/* initialise the propagator with time, state and necessary parameters
*
*/
void OrbitPropagator::init(
	Vector6d				rvSatECI,
	double 					mjdUTC,
	double					leapSec,
	ERP*					erpSrc,
	EGMCoef					egmCoef)
{
	inertialState 		= rvSatECI;
	mMJDUTC 			= mjdUTC;
	erp 				= erpSrc;

	geterp(*erp, mMJDUTC, erpv);

	gravityModel = GravityModel(propOpt.optEarthGravMdl, egmCoef);

	if	(acsConfig.forceModels.solar_radiation_pressure)
	{
		solarRadPressure = SolarRadPressure(propOpt.paraSRP);
	}
}

void OrbitPropagator::update(
	double				mjdUTC)
{
	// update time
	mMJDUTC = mjdUTC;

	// update erp
	geterp(*erp, mMJDUTC, erpv);
	double dUTC_TAI	= -(19 + erpv.leaps);
	double xp		= erpv.xp;
	double yp		= erpv.yp;
	double dUT1_UTC	= erpv.ut1_utc;
	double lod		= erpv.lod;

	// update iers
	iers = IERS(dUT1_UTC, dUTC_TAI, xp, yp, lod);

	//update srp parameters
	solarRadPressure.mSRPPara = propOpt.paraSRP;

	//update third body positions
	double mjdTT = mMJDUTC + iers.TT_UTC() / 86400.0;
	for (int i = 0; i < E_ThirdBody::_size(); i++)
	{
		E_ThirdBody	body 		= E_ThirdBody::_values()[i];
		string		bodyName	= E_ThirdBody::_names()[i];

		if (acsConfig.forceModels.process_third_body[body] == false)
		{
			continue;
		}

		// Relative position vector of satellite w.r.t. point mass
		Vector3d thirdBodyPos;
		jplEphPos(nav.jplEph_ptr, mjdTT + JD2MJD, body, thirdBodyPos);

		thirdBodyPositionMap[body] = thirdBodyPos;
	}

	//update reference frame matrices
	eci2ecef_sofa(mjdUTC, iers, mECI2ECEF, mdECI2ECEF);

	//update egm coefficients for tides
	gravityModel.correctEgmCoefficients(std::cout, mjdUTC, erpv, mECI2ECEF);
}

Vector3d  OrbitVEQPropagator::calculateAccelNGradient(
	Trace&    				trace,	 			///< Trace to output to (similar to cout)
	const Vector3d& 		rSat,		 		///< Inertial position of satellite (m)
	const Vector3d& 		vSat,		 		///< Inertial velocity of satellite (m/s)
	Matrix3d&				mGradient,			///< Gradient (G=da/dr) in the ICRF/J2000 system
	Vector3d&				dadCr,       		///< Partials of acceleration w.r.t. to the solar radiation coefficient
	const Matrix3d& 		mECI2ECEF)	 		///< Transformation matrix from ECI coordinate to ECEF
{
// 	bool bVarEq = true;
// 	Vector3d earthGravityAcc = gravityModel.centralBodyGravityAcc(trace, mMJDUTC, mERPv, rSat, mECI2ECEF, bVarEq);
// 	Vector3d drSat;
// 	const double dinc = 1;   // Position increment [m]
// 	Vector3d daSat;
//
// 	/* Gradient
// 	*/
//   	for (int i = 0; i < 3; i++)
// 	{
//     	drSat = Vector3d::Zero();
// 		drSat(i) = dinc;	// Set offset in i-th component of the position vector
//     	daSat = gravityModel.centralBodyGravityAcc (trace, mMJDUTC, mERPv, rSat + drSat, mECI2ECEF, bVarEq) -  earthGravityAcc;	// Acceleration difference
//     	mGradient.col(i) = daSat / dinc;	// Derivative with respect to i-th component
//   	}
//
//   	/* Radiation pressure coefficient partials
// 	*/
// 	double mjdTT = mIERS.TT_UTC(mMJDUTC) / 86400.0 + mMJDUTC;
// 	solarRadPressure.mSRPPara.srpCoef = 1;
// 	dadCr = solarRadPressure.directSolarRadiationAcc(trace, mjdTT, rSat);
	Vector3d acc;
	return acc;
}
/** Propagate orbits by ODE functor.
 * Used by RK - input is a 6 element inertial state and time, and output is the derivative of the state at the provided time.
 * This function requires the update function to be called first to set planetary ephemerides, erp values, etc.
 */
void OrbitPropagator::operator ()(
	Vector6d&		 inertialState,				 ///< Inertial position and velocity of satellite (m, m/s)
	Vector6d&		dInertialState,
	const double	mjdUTCinSec)
{
	Instrument instrument(__FUNCTION__);

	//Get sub vectors from the state
	Vector3d rSat = inertialState.head(3);
	Vector3d vSat = inertialState.tail(3);

	auto& trace = std::cout;

	double mjdUTC = mjdUTCinSec / 86400;

	// double erpv[4] = {};
	// geterp_from_utc(&nav.erp, mjdUTC, erpv);
	// double leapSec	= nav.leaps;
	// double dUT1_UTC = erpv[2];
	// double dUTC_TAI	= -(19 + leapSec);
	// double xp = erpv[0];
	// double yp = erpv[1];
	// double lod = erpv[3];
	// clIERS iersInstance;
	// iersInstance.Set(dUT1_UTC, dUTC_TAI, xp, yp, lod);
	// double mjdTT = mjdUTC + iersInstance.TT_UTC(mjdUTC) / 86400;

	// Matrix3d mECI2ECEF = Matrix3d::Identity();
	// Matrix3d mdECI2ECEF = Matrix3d::Identity();
	// eci2ecef_sofa(mjdUTC, iersInstance, mECI2ECEF, mdECI2ECEF);
	// std::cout << "mECI2ECEF: " << mjdUTC << "  " << std::setw(14) << mECI2ECEF << std::endl;
	// Vector3d aSat = calculateAcceleration(std::cout, mjdTT, rSat, vSat, mECI2ECEF, egm.cmn, egm.smn, 12, 12);
	// std::cout << "Accelerations: " << mjdUTC << "  " << std::setw(14) << aSat.transpose() << std::endl;

	// std::cout << "mECI2ECEF: " << orbitProp.mMJDUTC << "  " << std::setw(14) << mECI2ECEF << std::endl;

	//calculate acceleration components
	Vector3d aSat = Vector3d::Zero();

	bool bVarEq = false;
	if (acsConfig.forceModels.earth_gravity)
	{
		Instrument instrument("Grav");

		Vector3d earthgravityAcc = -GM_Earth * rSat.normalized() / rSat.squaredNorm();// = gravityModel.centralBodyGravityAcc(trace, mMJDUTC, erpv, rSat, mECI2ECEF, bVarEq);

// 		trace << "Calculated accleration due to the Earth's central body gravity: " << std::setw(14) << mMJDUTC << std::setw(14) << earthgravityAcc.transpose() << std::endl;

		aSat += earthgravityAcc;
	}

	double mjdTT = iers.TT_UTC() / 86400.0 + mMJDUTC;
	for (int i = 0; i < E_ThirdBody::_size(); i++)
	{
		E_ThirdBody	body 		= E_ThirdBody::_values()[i];
		string		bodyName	= E_ThirdBody::_names()[i];

		if (acsConfig.forceModels.process_third_body[body] == false)
		{
			continue;
		}

		Vector3d& bodyPos = thirdBodyPositionMap[body];

		Vector3d thirdbodyAcc = accelPointMassGravity(trace, mjdTT, rSat, body, bodyPos);

// 		trace << "Calculated acceleration due to " << bodyName << "'s attraction: " << std::setw(14) << mMJDUTC << std::setw(14) << thirdbodyAcc.transpose() << std::endl;

		aSat += thirdbodyAcc;
	}

	if (acsConfig.forceModels.relativity_effect)
	{
		Instrument instrument("Rel");

		Vector3d relativityAcc = gravityModel.relativityEffectsAcc(trace, rSat, vSat);

// 		trace << "Calculated acceleration due to the relativity effect: " << std::setw(14) << mMJDUTC << std::setw(14) << relativityAcc.transpose() << std::endl;

		aSat += relativityAcc;
	}

	if (acsConfig.forceModels.solar_radiation_pressure)
	{
		Instrument instrument("SRP");

		Vector3d directSRPAcc = solarRadPressure.directSolarRadiationAcc(trace, mMJDUTC, rSat);

// 		trace << "Calculated acceleration due to the direct solar radiation: " << std::setw(14) << mMJDUTC << std::setw(14) << directSRPAcc.transpose() << std::endl;

		aSat += directSRPAcc;
	}

	// std::cout << "Total accelerations: " << mjdUTC << "  " << std::setw(14) << aSat.transpose() << std::endl;

	//set ODE output from sub vectors
	dInertialState.head(3) = vSat;
	dInertialState.tail(3) = aSat;

	std::cout << "\n" << (aSat.dot(rSat.normalized()));
}

/** Observer, prints time and state when called (during integration)
*
*/
void obsvrSatMotion(
	const Vector6d& x,
	const double mjdUTC)
{
	// std::cout << std::endl;
	// std::cout << std::setw(8) << mjdUTC << std::setw(14) << x[0] << std::setw(14) << x[1] << std::setw(14) << x[2]
	// 		  << std::setw(14) << x[3] << std::setw(14) << x[4] << std::setw(14) << x[5] << std::setw(14) <<  std::endl;
}

/** ODE of variational equation to be solved, i.e. the derivative of the state vector and the state transition matrix
*
*/
// struct VarEqPropagator
// {
// 	void operator()(
// 		const	VectorXSTMS&	rvPhiS,
// 				VectorXSTMS&	drvPhiSdt,
// 		const	double			mjdUTCinSec)
// 	{
// 		double mjdUTC = mjdUTCinSec / 86400;	// Time
//
// 		Vector3d rSat = rvPhiS.segment(0, 3);	// Position components
// 		Vector3d vSat = rvPhiS.segment(3, 3);	// Velocity components
//
// 		MatrixXd Phi = MatrixXd::Identity(6, 6);	// State transition matrix
// 		// for (int j = 0; j < 6; j++)
// 		// {
// 		// 	Phi.col(j) = rvPhiS.segment(6 * (j + 1), 6);
// 		// }
//
// 		MatrixXd S = MatrixXd::Zero(6, 1);	// Sensitivity matrix
// 		// for (int j = 0; j < 1; j++)
// 		// {
// 		// 	S.col(j) = rvPhiS.segment(6 * (j + 7), 6);
// 		// }
//
// 		Matrix3d mECI2ECEF	= Matrix3d::Identity();
// 		Matrix3d mdECI2ECEF	= Matrix3d::Identity();
// 		updPropagator(mjdUTC); //time epoch inside the integrator
// 		eci2ecef_sofa(mjdUTC, mIERS, mECI2ECEF, mdECI2ECEF);
//
// 		Matrix3d mGradient;
// 		Vector3d dadCr;
// 		Vector3d aSat = calculateAccelNGradient(std::cout, rSat, vSat, mGradient, dadCr, mECI2ECEF);
//
// 		/* Time derivative of state transition matrix
// 		*
// 		*/
// 		MatrixXd dfdy = MatrixXd::Zero(6, 6);
// 		for (int i = 0; i < 3; i++)
// 		for (int j = 0; j < 3; j++)
// 		{
// 			dfdy(i, 	j	 ) = 0;						// dv/dr(i, j)
// 			dfdy(i + 3, j	 ) = mGradient(i, j);		// da/dr(i, j)
// 			dfdy(i,     j + 3) = ( i == j ? 1 : 0 );	// dv/dv(i, j)
// 			dfdy(i + 3, j + 3) = 0;						// da/dv(i, j)
// 		}
// 		MatrixXd dPhi = dfdy * Phi;	// Time derivative of state transition matrix
//
//
// 		/* Time derivative of sensitivity matrix
// 		*
// 		*/
// 		MatrixXd dfdp(6, 1);
// 		for (int i = 0; i < 3; i++)
// 		{
// 			dfdp(i    ) = 0;							// dv/dCr(i)
// 			dfdp(i + 3) = dadCr(i);						// da/dCr(i)
// 		}
//
// 		MatrixXd dS = MatrixXd::Zero(6, 1);
// 		dS = dfdy * S + dfdp;
//
// 		/* Derivative of combined state vector and state transition matrix
// 		*
// 		*/
// 		for (int i = 0; i < 3; i++)
// 		{
// 			drvPhiSdt(i    ) = vSat(i);                    // dr/dt(i)
// 			drvPhiSdt(i + 3) = aSat(i);                    // dv/dt(i)
// 		}
//
// 		for (int i = 0; i < 6; i++)
// 		for (int j = 0; j < 6; j++)
// 		{
// 			drvPhiSdt(6 * (j + 1) + i  ) = dPhi(i, j);     // dPhi/dt(i,j)
// 		}
//
// 		for (int i = 0; i < 6; i++)
// 		for (int j = 6; j < 7; j++)
// 		{
// 			drvPhiSdt(6 * (j + 1) + i) = dS(i, j - 6);     // dS/dt(i,j)
// 		}
// 	}
// };

void updateOrbits(
	Trace&			trace,
	KFState&		kfState,
	GTime           time)
{
	{
		//get current inertial states from the kfState
		for (auto& [kfKey, index] : kfState.kfIndexMap)
		{
			if	( kfKey.type	!= KF::SAT_POS
				||kfKey.num		!= 0)
			{
				continue;
			}

			auto& orbitPropagator = orbitPropagatorMap[kfKey.Sat];

			Vector6d rvECI;
			for (int i = 0; i < 3; i++)
			{
				KFKey posKey = kfKey;

				posKey.num = i;
				kfState.getKFValue(posKey, rvECI(0 + i));

				posKey.type = KF::SAT_POS_RATE;
				kfState.getKFValue(posKey, rvECI(3 + i));
			}

			double mjdUTC	= gpst2mjd(tsync);
			double leapSec	= nav.leaps;
			orbitPropagator.setPropOption(acsConfig.forceModels);
			orbitPropagator.init(rvECI, mjdUTC, leapSec, &nav.erp, nav.egm);
		}
	}

	for (auto& [satId, orbitPropagator] : orbitPropagatorMap)
	{
		SatSys Sat;
		Sat.fromHash(satId);

		double dt = time - kfState.time;			//time indicates the current epoch, kfState.time indicates the last epoch

		double t0 = gpst2mjd(kfState.time)	* 86400;
		double t1 = gpst2mjd(time)			* 86400;

		if (dt == 0)
		{
			return;
		}

// 		orbitPropagator.stateTime = time;

		VectorXd oldState = orbitPropagator.inertialState;

		double t_mid = (t0 + t1) / 2 / 86400;

	// 	orbitPropagator.updPropagator(t0);//time epoch before the orbital propagation

		std::cout << std::setprecision(2) << std::fixed;
		std::cout
		<< "ICRF coordinates before the orbital propagation step: "
		<< std::setprecision(14) << orbitPropagator.inertialState.transpose() <<  std::endl;

		orbitPropagator.update(t_mid); //time epoch inside the integrator

		//Run the propagator using the functor

		if (acsConfig.forceModels.ode_integrator == +E_Integrator::RKF78)
		{
			typedef runge_kutta_fehlberg78<Vector6d>	rkf78;			// Error stepper, used to create the controlled stepper
// 			typedef controlled_runge_kutta<rkf78>		ctrl_rkf78;		// Controlled stepper: it's built on an error stepper and allows us to have the output at each internally defined (refined) timestep, via integrate_adaptive call

			double errAbs = 1.0e-16; // Error bounds
			double errRel = 1.0e-13;

			//integrate_adaptive(ctrl_rkf78(), OrbitPropagator(), rvECI, t0, t1, dt);
			auto controller = make_controlled(errAbs, errRel, rkf78());
			integrate_adaptive(controller, orbitPropagator, orbitPropagator.inertialState, t0, t1, dt);
		}

		std::cout
		<< "ICRF coordinates after the orbital propagation step:  "
		<< std::setprecision(14) << orbitPropagator.inertialState.transpose() <<  std::endl;
	// 	VectorXSTMS	rvPhiS	= kfState.xSTMSM;

	// 	if (0)
	// 	{
	// 		// typedef runge_kutta4<VectorXSTMS> rk4;
	// 		// integrate_const(rk4(), odeVarEquation, rvPhiS, t0, t1, dt);
	// 		integrate(VarEqPropagator(), rvPhiS, t0, t1, dt);
	// 		// typedef runge_kutta_dopri5<Vector6d> rk5;
	// 		// typedef controlled_runge_kutta<rk5> ctrl_rk5;
	// 		// double errAbs = 1.0e-10; // Error bounds
	// 		// double errRel = 1.0e-8;
	// 		// integrate_adaptive(ctrl_rk5(), odeVarEquation, rvPhiS, t0, t1, dt);
	//
	// 		std::cout << "ICRF coordinates before the variatioanal equation propagation: " << std::setprecision(14) << rvPhiS.head(3).transpose() <<  std::endl;
	// 		kfState.xSTMSM = rvPhiS;
	// 	}

	// 	kfState.x		= rvECI;
	// 	kfState.time	= time;

		// ECI to ECEF transformation

		// double erpv[4] = {};
		// geterp(&nav.erp, time + dt, erpv);
		// double mjdUTC = gpst2mjd(time + dt);
		// double leapSec	= nav.leaps;
		// double dUT1_UTC = erpv[2];
		// double dUTC_TAI	= -(19 + orbitProp.mLeapSec);
		// double xp = erpv[0];
		// double yp = erpv[1];
		// double lod = erpv[3];
		// clIERS iersInstance;
		// iersInstance.Set(dUT1_UTC, dUTC_TAI, xp, yp, lod);
		// double mjdTT = mjdUTC + iersInstance.TT_UTC(mjdUTC) / 86400;
		// Matrix3d mECI2ECEF = Matrix3d::Identity();
		// Matrix3d mdECI2ECEF = Matrix3d::Identity();
		// Vector6d rvECEF;
		// eci2ecefVec_sofa(mjdUTC, iersInstance, rvECI, rvECEF);
		// std::cout << "ITRF coordinates: " << std::endl << std::setprecision(14) << rvECEF.transpose() <<  std::endl;

	// 	Matrix3d mECI2ECEF	= Matrix3d::Identity();
	// 	Matrix3d mdECI2ECEF	= Matrix3d::Identity();
		orbitPropagator.update(gpst2mjd(time));//time epoch after the orbital propagation;

		Vector3d rECI = orbitPropagator.inertialState.head(3);
		Vector3d vECI = orbitPropagator.inertialState.tail(3);
// 		std::cout
// 		<< "ICRF coordinates after the orbital propagation step:  "
// 		<< std::setprecision(14) << rECI.transpose() << " " << vECI.transpose() <<  std::endl;

		Vector3d rECEF2;
		Vector3d vECEF2;
		eci2ecef_sofa(orbitPropagator.mMJDUTC, orbitPropagator.iers, rECI, vECI, rECEF2, vECEF2);
// 		std::cout
// 		<< "ITRF coordinates after the orbital propagation step:  "
// 		<< std::setprecision(14) << rECEF2.transpose() << " " << vECEF2.transpose() <<  std::endl;

// 		std::ofstream fileOPResults("./ex01/OPResults.txt", std::ios_base::app | std::ios_base::in);
// 		if (!fileOPResults)
// 		{
// 			std::cout << "Error openinng results file!\n";
// 			return;
// 		}

// 		fileOPResults << std::setprecision(6) << std::fixed;

		// fileOPResults << setw(16) << orbitProp.mMJDUTC << setw(16) << rvECI.transpose() << setw(16) << rvECEF2.transpose() << endl;
// 		fileOPResults << std::setw(16) << orbitPropagator.mMJDUTC << " " << rECEF2.transpose() << vECEF2.transpose() << std::endl;

		//get the change in state ready for use in the kalman filter's state transition
// 		orbitPropagator.stateDelta		= orbitPropagator.inertialState - oldState;

		//get the noise in the state delta according to uncertainty in the inputs, and propagation uncertainty for use in the filters' covariance transition
// 		orbitPropagator.stateDeltaNoise	= Matrix6d::Zero();

		Matrix6d stateTransition = Matrix6d::Identity();
		stateTransition.topRightCorner(3,3) = Matrix3d::Identity() * dt;

		Vector6d transitionedState = stateTransition * oldState;

		Vector6d missingDynamics = orbitPropagator.inertialState - transitionedState;

		for (int i = 0; i < 3; i++)
		{
			KFKey oneKey = {.type = KF::ONE};

			KFKey kfKey;
			kfKey.Sat = Sat;
			kfKey.num = i;

			kfKey.type = KF::SAT_POS;

			kfState.setKFTrans(kfKey, oneKey, missingDynamics(0 + i));

			kfKey.type = KF::SAT_POS_RATE;

			kfState.setKFTrans(kfKey, oneKey, missingDynamics(3 + i));
		}
	}
}


/**
 * @file arma_extend_test.cpp
 * @author Ryan Curtin
 *
 * Test of the mlpack extensions to Armadillo.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

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

using namespace mlpack;
using namespace arma;

BOOST_AUTO_TEST_SUITE(ArmaExtendTest);

/**
 * Make sure we can reshape a matrix in-place without changing anything.
 */
BOOST_AUTO_TEST_CASE(InplaceReshapeColumnTest)
{
  arma::mat X;
  X.randu(1, 10);
  arma::mat oldX = X;

  arma::inplace_reshape(X, 2, 5);

  BOOST_REQUIRE_EQUAL(X.n_rows, 2);
  BOOST_REQUIRE_EQUAL(X.n_cols, 5);
  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_CLOSE(X[i], oldX[i], 1e-5); // Order should be preserved.
}

/**
 * Make sure we can reshape a large matrix.
 */
BOOST_AUTO_TEST_CASE(InplaceReshapeMatrixTest)
{
  arma::mat X;
  X.randu(8, 10);
  arma::mat oldX = X;

  arma::inplace_reshape(X, 10, 8);

  BOOST_REQUIRE_EQUAL(X.n_rows, 10);
  BOOST_REQUIRE_EQUAL(X.n_cols, 8);
  for (size_t i = 0; i < 80; ++i)
    BOOST_REQUIRE_CLOSE(X[i], oldX[i], 1e-5); // Order should be preserved.
}

/**
 * Test const_row_col_iterator for basic functionality.
 */
BOOST_AUTO_TEST_CASE(ConstRowColIteratorTest)
{
  mat X;
  X.zeros(5, 5);
  for (size_t i = 0; i < 5; ++i)
    X.col(i) += i;

  for (size_t i = 0; i < 5; ++i)
    X.row(i) += 3 * i;

  // Make sure default constructor works okay.
  mat::const_row_col_iterator it;
  // Make sure ++ operator, operator* and comparison operators work fine.
  size_t count = 0;
  for (it = X.begin_row_col(); it != X.end_row_col(); it++)
  {
    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);

    count++;
  }
  BOOST_REQUIRE_EQUAL(count, 25);
  it = X.end_row_col();
  do
  {
    it--;
    count--;

    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);
  } while (it != X.begin_row_col());

  BOOST_REQUIRE_EQUAL(count, 0);
}

/**
 * Test row_col_iterator for basic functionality.
 */
BOOST_AUTO_TEST_CASE(RowColIteratorTest)
{
  mat X;
  X.zeros(5, 5);
  for (size_t i = 0; i < 5; ++i)
    X.col(i) += i;

  for (size_t i = 0; i < 5; ++i)
    X.row(i) += 3 * i;

  // Make sure default constructor works okay.
  mat::row_col_iterator it;
  // Make sure ++ operator, operator* and comparison operators work fine.
  size_t count = 0;
  for (it = X.begin_row_col(); it != X.end_row_col(); it++)
  {
    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);

    count++;
  }
  BOOST_REQUIRE_EQUAL(count, 25);
  it = X.end_row_col();
  do
  {
    it--;
    count--;

    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);
  } while (it != X.begin_row_col());

  BOOST_REQUIRE_EQUAL(count, 0);
}

/**
 * Operator-- test for mat::row_col_iterator and mat::const_row_col_iterator
 */
BOOST_AUTO_TEST_CASE(MatRowColIteratorDecrementOperatorTest)
{
  mat test = ones<mat>(5, 5);

  mat::row_col_iterator it1 = test.begin_row_col();
  mat::row_col_iterator it2 = it1;

  // check that postfix-- does not decrement the position when position is
  // pointing to the begining
  it2--;
  BOOST_REQUIRE_EQUAL(it1.row(), it2.row());
  BOOST_REQUIRE_EQUAL(it1.col(), it2.col());

  // check that prefix-- does not decrement the position when position is
  // pointing to the begining
  --it2;
  BOOST_REQUIRE_EQUAL(it1.row(), it2.row());
  BOOST_REQUIRE_EQUAL(it1.col(), it2.col());
}

// These tests don't work when the sparse iterators hold references and not
// pointers internally because of the lack of default constructor.

/**
 * Test sparse const_row_col_iterator for basic functionality.
 */
BOOST_AUTO_TEST_CASE(ConstSpRowColIteratorTest)
{
  sp_mat X(5, 5);
  for (size_t i = 0; i < 5; ++i)
    X.col(i) += i;

  for (size_t i = 0; i < 5; ++i)
    X.row(i) += 3 * i;

  // Make sure default constructor works okay.
  sp_mat::const_row_col_iterator it;
  // Make sure ++ operator, operator* and comparison operators work fine.
  size_t count = 1;
  for (it = X.begin_row_col(); it != X.end_row_col(); it++)
  {
    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);

    count++;
  }
  BOOST_REQUIRE_EQUAL(count, 25);
  it = X.end_row_col();
  do
  {
    it--;
    count--;

    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);
  } while (it != X.begin_row_col());

  BOOST_REQUIRE_EQUAL(count, 1);
}

/**
 * Test sparse row_col_iterator for basic functionality.
 */
BOOST_AUTO_TEST_CASE(SpRowColIteratorTest)
{
  sp_mat X(5, 5);
  for (size_t i = 0; i < 5; ++i)
    X.col(i) += i;

  for (size_t i = 0; i < 5; ++i)
    X.row(i) += 3 * i;

  // Make sure default constructor works okay.
  sp_mat::row_col_iterator it;
  // Make sure ++ operator, operator* and comparison operators work fine.
  size_t count = 1;
  for (it = X.begin_row_col(); it != X.end_row_col(); it++)
  {
    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);

    count++;
  }
  BOOST_REQUIRE_EQUAL(count, 25);
  it = X.end_row_col();
  do
  {
    it--;
    count--;

    // Check iterator value.
    BOOST_REQUIRE_EQUAL(*it, (count % 5) * 3 + (count / 5));

    // Check iterator position.
    BOOST_REQUIRE_EQUAL(it.row(), count % 5);
    BOOST_REQUIRE_EQUAL(it.col(), count / 5);
  } while (it != X.begin_row_col());

  BOOST_REQUIRE_EQUAL(count, 1);
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <vector>
#include <algorithm>
#include <string>
#include <Eigen/Dense>

#include "Dark_Arts_Exception.h"

int main(int argc, char** argv)
{
  int val = 0;

  try{
    const int n_p = 4;
    std::vector< std::vector<double> > values_to_save(n_p, std::vector<double>(n_p,0.));

    Eigen::MatrixXd col_major_mat = Eigen::MatrixXd::Zero(n_p,n_p);
    for(int i=0; i <n_p ; i++)
    {
      for(int j=0; j<n_p ;j++)
      {
        values_to_save[i][j] = rand();
        col_major_mat(i,j) = values_to_save[i][j];
      }
    }

    std::cout << "This is col_major_mat:\n" << col_major_mat;
    std::cout << "\ncol_major_mat is: " << col_major_mat.IsRowMajor << " with respect to being row major\n";

    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> row_major_mat(n_p,n_p);
    std::cout << "This is the unitialized row_major_mat :\n" << row_major_mat;
    std::cout << "\n row_major_mat is: " << row_major_mat.IsRowMajor << " with respect to being row major\n";

    row_major_mat = Eigen::MatrixXd::Zero(n_p,n_p);
    std::cout << "Now I've zeroedrow_major_mat with MatrixXd.  \n row_major_mat is: " << row_major_mat.IsRowMajor << " with respect to being row major\n";

    row_major_mat = col_major_mat;
    std::cout << "Now I've set row_major_mat equal to a col_major_mat matrix.  \n row_major_mat is: " << row_major_mat.IsRowMajor << " with respect to being row major\n";

    std::cout << "This is row_major_mat now:\n" << row_major_mat << std::endl;

    double *col_maj_ptr = &col_major_mat(0,0);
    double *row_maj_ptr = &row_major_mat(0,0);

    for(int i = 0 ; i < n_p*n_p ; i++)
    {
      std::cout << "row_maj_ptr["<<i<<"]: " << row_maj_ptr[i] << std::endl;
    }
    std::cout << std::endl;
    for(int i = 0 ; i < n_p*n_p ; i++)
    {
      std::cout << "col_maj_ptr["<<i<<"]: " << col_maj_ptr[i] << std::endl;
    }
  }
  catch(const Dark_Arts_Exception& da)
  {
    val = -1;
    da.testing_message();
  }

  return val;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for various edge cases of round-tripping for fixed_point for 15 binary digits of precison.

#include <cmath>
#include <iomanip>
#include <sstream>
#include <string>

#define BOOST_TEST_MODULE test_negatable_round_trip_edge_cases
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename FixedPointType,
           typename FloatPointType>
  FixedPointType test_round_trip(const FloatPointType f)
  {
    const FixedPointType x(static_cast<FloatPointType>(f));

    std::stringstream stream1;

    // Here we use Kahan's formula for digits10.
    //       digits10 =  ((digits - 1) * 301) / 1000
    //   max_digits10 = (((digits - 0) * 301) / 1000) + 1
    stream1 << std::setprecision(std::numeric_limits<FixedPointType>::max_digits10)
            << std::fixed
            << x;

    std::stringstream stream2(stream1.str());

    FixedPointType y;
    stream2 >> y;

    return y;
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_round_trip_edge_cases)
{
  // Use round mode nearest_even becaust this is what
  // the compiler uses for the default rounding mode
  // of its built-in floating-point types.

  {
    // Define the smallest negatable type having 1 decimal digits10.
    typedef boost::fixed_point::negatable<0, -5, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                  float_point_type;

    const float_point_type x = static_cast<float_point_type>(0.9L);
    const fixed_point_type y = local::test_round_trip<fixed_point_type>(x);

    BOOST_CHECK_EQUAL(fixed_point_type(x), y);
  }

  {
    // Define the smallest negatable type having 2 decimal digits10.
    typedef boost::fixed_point::negatable<0, -8, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                  float_point_type;

    const float_point_type x = static_cast<float_point_type>(0.99L);
    const fixed_point_type y = local::test_round_trip<fixed_point_type>(x);

    BOOST_CHECK_EQUAL(fixed_point_type(x), y);
  }

  {
    // Define the smallest negatable type having 4 decimal digits10.
    typedef boost::fixed_point::negatable<0, -15, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                   float_point_type;

    const float_point_type x = static_cast<float_point_type>(0.9999L);
    const fixed_point_type y = local::test_round_trip<fixed_point_type>(x);

    BOOST_CHECK_EQUAL(fixed_point_type(x), y);
  }

  {
    // Define the smallest negatable type having 9 decimal digits10.
    typedef boost::fixed_point::negatable<0, -31, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                   float_point_type;

    const float_point_type x = static_cast<float_point_type>(0.999999999L);
    const fixed_point_type y = local::test_round_trip<fixed_point_type>(x);

    BOOST_CHECK_EQUAL(fixed_point_type(x), y);
  }

  {
    // Define the smallest negatable type having 37 decimal digits10.
    typedef boost::fixed_point::negatable<0, -124, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                    float_point_type;

    const float_point_type x = static_cast<float_point_type>("0.9999999999999999999999999999999999999");
    const fixed_point_type y = local::test_round_trip<fixed_point_type>(x);

    BOOST_CHECK_EQUAL(fixed_point_type(x), y);
  }

  {
    // Define the smallest negatable type having 100 decimal digits10.
    typedef boost::fixed_point::negatable<0, -334, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                    float_point_type;

    const float_point_type x = static_cast<float_point_type>("0.9999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999");
    const fixed_point_type y = local::test_round_trip<fixed_point_type>(x);

    BOOST_CHECK_EQUAL(fixed_point_type(x), y);
  }
}


//==============================================================================
//         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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 constants properties"

#include <boost/simd/include/constants/properties.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

#ifdef BOOST_MSVC
  #pragma warning(disable: 4146) // unary minus applied to unsigned
#endif

////////////////////////////////////////////////////////////////////////////////
// Test values for sigmask
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(signmask)
{
  NT2_TEST_EQUAL( boost::simd::Signmask<double>(), -0.   );
  NT2_TEST_EQUAL( boost::simd::Signmask<float>() , -0.f );

  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::uint64_t>(), 0u );
  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::uint32_t>(), 0u );
  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::uint16_t>(), 0u );
  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::uint8_t >(), 0u );

  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::int64_t>(), boost::simd::int64_t(0x8000000000000000LL) );
  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::int32_t>(), boost::simd::int32_t(0x80000000)            );
  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::int16_t>(), boost::simd::int16_t(0x8000)  );
  NT2_TEST_EQUAL( boost::simd::Signmask<boost::simd::int8_t >(), boost::simd::int8_t(0x80)    );
}

////////////////////////////////////////////////////////////////////////////////
// Test values for valmax
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(valmax)
{
  NT2_TEST_EQUAL( boost::simd::Valmax<double>() , 1.7976931348623157e+308 );
  NT2_TEST_EQUAL( boost::simd::Valmax<float>()  , float(3.4028235e+38)    );

  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::uint64_t>(), 0xFFFFFFFFFFFFFFFFULL  );
  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::uint32_t>(), 0xFFFFFFFFUL           );
  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::uint16_t>(), 0xFFFF                 );
  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::uint8_t >(), 0xFF                   );

  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::int64_t>(), 9223372036854775807LL);
  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::int32_t>(), 2147483647           );
  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::int16_t>(), 32767                );
  NT2_TEST_EQUAL( boost::simd::Valmax<boost::simd::int8_t >(), 127                  );
}

////////////////////////////////////////////////////////////////////////////////
// Test values for valmin
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(valmin)
{
  NT2_TEST_EQUAL( boost::simd::Valmin<double>() , -1.7976931348623157e+308);
  NT2_TEST_EQUAL( boost::simd::Valmin<float>()  , float(-3.4028235e+38)   );

  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::uint64_t>(), 0u );
  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::uint32_t>(), 0u  );
  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::uint16_t>(), 0u  );
  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::uint8_t >(), 0u  );

  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::int64_t>()+1, -9223372036854775807LL );
  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::int32_t>()+1, -2147483647L           );
  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::int16_t>(), -32768                  );
  NT2_TEST_EQUAL( boost::simd::Valmin<boost::simd::int8_t >(), -128                    );
}

////////////////////////////////////////////////////////////////////////////////
// Test values for maxleftshift
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(Maxleftshift)
{
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<double>()       , 63 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<float>()        , 31 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::uint64_t>(), 63 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::uint32_t>(), 31 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::uint16_t>(), 15 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::uint8_t >(), 7  );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::int64_t>() , 63 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::int32_t>() , 31 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::int16_t>() , 15 );
  NT2_TEST_EQUAL( boost::simd::Maxleftshift<boost::simd::int8_t >() , 7  );
}

////////////////////////////////////////////////////////////////////////////////
// Test values for IEEE specs
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(ieee)
{
  NT2_TEST_EQUAL( boost::simd::Nbmantissabits<double>() , 52);
  NT2_TEST_EQUAL( boost::simd::Nbmantissabits<float>()  , 23);

  NT2_TEST_EQUAL( boost::simd::Nbexponentbits<double>() , 11);
  NT2_TEST_EQUAL( boost::simd::Nbexponentbits<float>()  , 8  );

  NT2_TEST_EQUAL( boost::simd::Maxexponent<double>(), 1023  );
  NT2_TEST_EQUAL( boost::simd::Maxexponent<float>()  , 127    );

  NT2_TEST_EQUAL( boost::simd::Minexponent<double>(), -1022  );
  NT2_TEST_EQUAL( boost::simd::Minexponent<float>()  , -126  );

  NT2_TEST_EQUAL( boost::simd::Ldexpmask<double>(), 0x7FF0000000000000ll  );
  NT2_TEST_EQUAL( boost::simd::Ldexpmask<float>() , 0x7F800000            );

  NT2_TEST_EQUAL( boost::simd::Nbdigits<double>(), 53 );
  NT2_TEST_EQUAL( boost::simd::Nbdigits<float>() , 24 );
}
////////////////////////////////////////////////////////////////////////////////
// Test values for IEEE specs on integer
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL(ieee_ints, BOOST_SIMD_INTEGRAL_TYPES)
{
  NT2_TEST_EQUAL( boost::simd::Nbmantissabits<T>(), T(sizeof(T)*CHAR_BIT));
  NT2_TEST_EQUAL( boost::simd::Nbexponentbits<T>(), T(0) );
  NT2_TEST_EQUAL( boost::simd::Maxexponent<T>()   , T(0) );
  NT2_TEST_EQUAL( boost::simd::Minexponent<T>()   , T(0) );
  NT2_TEST_EQUAL( boost::simd::Ldexpmask<T>()     , T(0) );
  NT2_TEST_EQUAL( boost::simd::Nbdigits<T>()      , T(0) );
}


// Copyright © 2016 Venture Media Labs.
//
// This file is part of mxml. The full mxml copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include <mxml/Algorithm.h>
#include <boost/test/unit_test.hpp>

using namespace mxml;

BOOST_AUTO_TEST_CASE(integerSumUnderTest) {
    std::vector<double> reals(6, 2.3333);
    std::vector<int> integers;
    integerSum(reals.begin(), reals.end(), std::inserter(integers, integers.begin()));

    auto sum = std::accumulate(integers.begin(), integers.end(), 0, std::plus<int>());
    BOOST_CHECK_EQUAL(sum, 14);
    for (auto integer : integers)
        BOOST_CHECK(integer == 2 || integer == 3);
}

BOOST_AUTO_TEST_CASE(integerSumOverTest) {
    std::vector<double> reals(20, 2.9);
    std::vector<int> integers;
    integerSum(reals.begin(), reals.end(), std::inserter(integers, integers.begin()));

    auto sum = std::accumulate(integers.begin(), integers.end(), 0, std::plus<int>());
    BOOST_CHECK_EQUAL(sum, 58);
    for (auto integer : integers)
        BOOST_CHECK(integer == 2 || integer == 3);
}

BOOST_AUTO_TEST_CASE(integerSumInaccurateTotalTest) {
    double reals[] = {2.9, 2.9, 2.9};
    int integers[3];
    integerSum(reals, reals + 3, integers);

    auto sum = std::accumulate(integers, integers + 3, 0, std::plus<int>());
    BOOST_CHECK_EQUAL(sum, 9);
    for (auto integer : integers)
        BOOST_CHECK(integer == 2 || integer == 3);
}


//  (C) Copyright Eric Niebler 2005.
//  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)

#include <iostream>
#include <valarray>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename T>
typename boost::enable_if<is_floating_point<T> >::type is_equal_or_close(T const &left, T const &right)
{
    BOOST_CHECK_CLOSE(left, right, 1e-5);
}

template<typename T>
typename boost::disable_if<is_floating_point<T> >::type is_equal_or_close(T const &left, T const &right)
{
    BOOST_CHECK_EQUAL(left, right);
}

template<typename T>
void is_equal(std::valarray<T> const &left, std::valarray<T> const &right)
{
    BOOST_CHECK_EQUAL(left.size(), right.size());
    if(left.size() == right.size())
    {
        for(std::size_t i = 0; i < left.size(); ++i)
        {
            is_equal_or_close(left[i], right[i]);
        }
    }
}

namespace std
{
    template<typename T>
    inline std::ostream &operator <<(std::ostream &sout, std::valarray<T> const &arr)
    {
        sout << '(';
        for(std::size_t i = 0; i < arr.size(); ++i)
        {
            sout << arr[i] << ',';
        }
        sout << ')' << std::endl;
        return sout;
    }
}

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    typedef std::valarray<int> sample_t;

    // test sum
    {
        accumulator_set<sample_t, stats<tag::sum> > acc(sample = sample_t(0,3));

        acc(sample_t(1,3));
        acc(sample_t(2,3));
        acc(sample_t(3,3));

        is_equal(sample_t(6,3), sum(acc));
    }

    // test min and max
    {
        int s1[] = {1,2,3}, s2[] = {0,3,4}, s3[] = {2,1,4}, min_res[] = {0,1,3}, max_res[] = {2,3,4};
        accumulator_set<sample_t, stats<tag::min, tag::max> > acc(sample = sample_t(0,3));

        acc(sample_t(s1,3));
        acc(sample_t(s2,3));
        acc(sample_t(s3,3));

        is_equal(sample_t(min_res,3), (min)(acc));
        is_equal(sample_t(max_res,3), (max)(acc));
    }

    // test mean(lazy) and mean(immediate)
    {
        accumulator_set<sample_t, stats<tag::mean> > acc(sample = sample_t(0,3));

        acc(sample_t(1,3));
        is_equal(std::valarray<double>(1., 3), mean(acc));
        BOOST_CHECK_EQUAL(1u, count(acc));
        is_equal(sample_t(1, 3), sum(acc));

        acc(sample_t(0,3));
        is_equal(std::valarray<double>(0.5, 3), mean(acc));
        BOOST_CHECK_EQUAL(2u, count(acc));
        is_equal(sample_t(1, 3), sum(acc));

        acc(sample_t(2,3));
        is_equal(std::valarray<double>(1., 3), mean(acc));
        BOOST_CHECK_EQUAL(3u, count(acc));
        is_equal(sample_t(3, 3), sum(acc));

        accumulator_set<sample_t, stats<tag::mean(immediate)> > acc2(sample = sample_t(0,3));

        acc2(sample_t(1,3));
        is_equal(std::valarray<double>(1., 3), mean(acc2));
        BOOST_CHECK_EQUAL(1u, count(acc2));

        acc2(sample_t(0,3));
        is_equal(std::valarray<double>(0.5, 3), mean(acc2));
        BOOST_CHECK_EQUAL(2u, count(acc2));

        acc2(sample_t(2,3));
        is_equal(std::valarray<double>(1., 3), mean(acc2));
        BOOST_CHECK_EQUAL(3u, count(acc2));
    }

    // test weighted_mean
    {
        accumulator_set<sample_t, stats<tag::weighted_mean>, int> acc(sample = sample_t(0,3));

        acc(sample_t(10,3), weight = 2);            //  20
        BOOST_CHECK_EQUAL(2, sum_of_weights(acc));  //
                                                    //
        acc(sample_t(6,3), weight = 3);             //  18
        BOOST_CHECK_EQUAL(5, sum_of_weights(acc));  //
                                                    //
        acc(sample_t(4,3), weight = 4);             //  16
        BOOST_CHECK_EQUAL(9, sum_of_weights(acc));  //
                                                    //
        acc(sample_t(6,3), weight = 5);             //+ 30
        BOOST_CHECK_EQUAL(14, sum_of_weights(acc)); //
                                                    //= 84  / 14 = 6

        is_equal(std::valarray<double>(6.,3), weighted_mean(acc));

        accumulator_set<sample_t, stats<tag::weighted_mean(immediate)>, int> acc2(sample = sample_t(0,3));

        acc2(sample_t(10,3), weight = 2);           //  20
        BOOST_CHECK_EQUAL(2, sum_of_weights(acc2)); //
                                                    //
        acc2(sample_t(6,3), weight = 3);            //  18
        BOOST_CHECK_EQUAL(5, sum_of_weights(acc2)); //
                                                    //
        acc2(sample_t(4,3), weight = 4);            //  16
        BOOST_CHECK_EQUAL(9, sum_of_weights(acc2)); //
                                                    //
        acc2(sample_t(6,3), weight = 5);            //+ 30
        BOOST_CHECK_EQUAL(14, sum_of_weights(acc2));//
                                                    //= 84  / 14 = 6

        is_equal(std::valarray<double>(6.,3), weighted_mean(acc2));

    }
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("valarray test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include <boost/test/unit_test.hpp>
#include "outliers_input.h"
#include "utils/math_utils.h"
#include "utils/algorithms.hpp"
#include "inputs/outlier_data.h"
#include "utils/algorithms.hpp"
#include <iostream>

void check_outlier(double expected, std::vector<double>& founds)
{
    auto i = std::find(std::begin(founds), std::end(founds), expected);
    if (i == std::end(founds)) {
        BOOST_TEST(false, "failed to find outliers "<<expected<<" in the list of founds outliers");
    } else {
        founds.erase(i);
    }
}

BOOST_AUTO_TEST_CASE(test_outliers)
{
    // please note that you have a python program to find the outliers for verifiction
    static const double alpha_val = 0.1;
    std::vector<double> samples(std::begin(outliers_data), std::end(outliers_data));
    auto outliers = utils::grubbstest(samples, alpha_val);
    BOOST_TEST(samples.size() != outliers.size());
    BOOST_TEST(samples.size() > outliers.size());
    BOOST_TEST(outliers.size() > 0u);
    // make sure that we dont have any duplications in the outliers
    BOOST_TEST_REQUIRE(utils::size(expected_outliers) == outliers.size());
    //std::cout<<"we have "<<outliers.size()<<" outliers that we found in the test out of "<<samples.size()<<std::endl;
    std::sort(std::begin(outliers), std::end(outliers));
    for (auto i = 0u; i < outliers.size(); ++i) {
        BOOST_TEST(outliers[i] == expected_outliers[i]);
        //auto r = verifier.insert(val);
        //BOOST_TEST(r.second, "we are not expecting that the value "<<val<<" would already be inside");
    }
}

BOOST_AUTO_TEST_CASE(test_outliers_with_expected)
{
    static const double alpha_val = 0.1;
    const auto data_size = utils::size(data_outlier);
    const auto number_expected_outliers = utils::size(expected_outlier);
    auto outliers{utils::grubbstest(std::begin(data_outlier), std::end(data_outlier), alpha_val)};
    BOOST_TEST_REQUIRE(number_expected_outliers == outliers.size(),
            "the number of outliers we have "<<outliers.size()<<" != "<<number_expected_outliers);
    //std::cout<<"we found outliers\n";
    //out_range(std::cout, outliers);
    //std::cout<<"\nexpected outliers:"<<std::endl;
    //for (auto index : expected_outlier) {
    //    if (index < data_size) {
    //        std::cout<<data_outlier[index]<<", ";
    //    } else {
    //        std::cout<<" out of range: "<<index<<", ";
    //    }
    //}
    //std::cout<<std::endl;
    for (auto index : expected_outlier) {   // these are the locations where we expecting to find outliers
        if (index < data_size) {
            auto outlier = data_outlier[index];
            check_outlier(outlier, outliers);
        } else {
            BOOST_TEST_REQUIRE(false, "we have invalid index "<<index<<" out of range for data "<<data_size);
        }
    }
    //std::cout<<std::endl;
}


#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <iterator>
#include <map>
#include <set>
#include <sstream>
#include <string>
#include <vector>

#include <Eigen/Core>

#include "igl/AABB.h"
#include "igl/adjacency_list.h"
#include "igl/adjacency_matrix.h"
#include "igl/ambient_occlusion.h"
#include "igl/components.h"
#include "igl/cotmatrix.h"
#include "igl/decimate.h"
#include "igl/dijkstra.h"
#include "igl/embree/ambient_occlusion.h"
#include "igl/embree/EmbreeIntersector.h"
#include "igl/embree/unproject_onto_mesh.h"
#include "igl/gaussian_curvature.h"
#include "igl/invert_diag.h"
#include "igl/jet.h"
#include "igl/per_vertex_attribute_smoothing.h"
#include "igl/per_vertex_normals.h"
#include "igl/point_mesh_squared_distance.h"
#include "igl/polygon_mesh_to_triangle_mesh.h"
#include "igl/principal_curvature.h"
#include "igl/ray_mesh_intersect.h"
#include "igl/readOFF.h"
#include "igl/unproject_onto_mesh.h"
#include "igl/viewer/Viewer.h"
#include "igl/writeOFF.h"

#include "GLFW/glfw3.h"

#include "dijkstra.hxx"

const double mOCC_MAX_RAY_DIST = 10.0;
const int mOCC_NUM_RAYS = 20;
const double mDECIMATE_PERC = 0.6;

void getMeanCurvature(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
                      Eigen::VectorXd &VCs) {
  Eigen::MatrixXd PD1,PD2;
  Eigen::VectorXd PV1,PV2;
  //igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 5);
  igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 8);
  //mean_curve = 0.5 * (PV1 + PV2);
  VCs = PV1.cwiseProduct(PV1) + PV2.cwiseProduct(PV2);
  VCs = VCs.cwiseMin(0.1);
  VCs /= VCs.maxCoeff();

  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  VCs = ones - VCs;

  /*
  // Get the laplacian
  Eigen::SparseMatrix<double> L,M,Minv;
  //   from the cotangent matrix
  igl::cotmatrix(V,F, L);
  //   and the mass matrix
  igl::massmatrix(V,F, igl::MASSMATRIX_TYPE_VORONOI, M);
  igl::invert_diag(M, Minv);

  Eigen::MatrixXd HN = -Minv*(L * V);
  // Mean curviture is this value.
  Eigen::VectorXd VC = HN.rowwise().norm();

  // Smooth this out a bit.
  igl::per_vertex_attribute_smoothing(VC,F, VCs);
  // Cutoff at 1 (to avoid overpowering by high curvature)
  VCs = VCs.cwiseMin(1.0);

  // Normalize
  VCs /= VCs.maxCoeff();
  // Invert so high curvature has low weight.
  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  // Sqrt before switching
  VCs = VCs.cwiseSqrt();
  VCs = ones - VCs;
  //std::cout << VCs;
  */

}

void printHelp() {
  printf("Press 'U' to undo last point\n");
  printf("Press 'C' to show colors from curvature\n");
  printf("Press 'D' to show colors from ambient occlusion\n");
}

bool file_exists(const char* fn) {
  FILE* ifs = fopen(fn, "r");
  if (ifs) {
    fclose(ifs);
    return true;
  }
  return false;
}

int main(int argc, char* argv[]) {
  if (argc < 3) {
    fprintf(stderr, "Used for selecting points on the surface of a mesh. Will write the\n"
                    "corresponding vertices to output.off\n");
    fprintf(stderr, "usage: %s <input.off> <output.off> [nowrite]\n",
            argv[0]);
    return -1;
  }

  bool writeable = true;
  if (argc == 4) {
    writeable = false;
  }

  Eigen::MatrixXd V,V2;
  Eigen::MatrixXi F,F2;
  igl::embree::EmbreeIntersector ei;

  printf("Reading in mesh...\n");
  // Read in the input file.
  igl::readOFF(argv[1], V, F);

  // Colors are all white to start.
  Eigen::MatrixXd VC = Eigen::MatrixXd::Constant(V.rows(),3,1);
  // Initialize this thing so we can get the vertices.
  ei.init(V.cast<float>(), F);

  // Ambient occlusion
  Eigen::VectorXd AO;
  Eigen::MatrixXd N;
  igl::per_vertex_normals(V,F,N);
  // Compute ambient occlusion factor using embree
  printf("Building AABB tree...\n");
  igl::AABB<Eigen::MatrixXd, 3> aabb;
  aabb.init(V, F);
  const auto & shoot_ray = [&aabb,&V,&F](
      const Eigen::Vector3f& _s,
      const Eigen::Vector3f& dir)->bool
  {
    Eigen::Vector3f s = _s+1e-4*dir;
    igl::Hit hit;
    if (aabb.intersect_ray(V,F, s.cast<double>().eval(),dir.cast<double>().eval(), hit)) {
      return hit.t < mOCC_MAX_RAY_DIST;
    }
    return false;
  };

  printf("Now setting ambient occlusion vertices to improve contrast (patience)...\n");
  //igl::embree::ambient_occlusion(V,F,V,N,300,AO);
  igl::ambient_occlusion(shoot_ray, V,N, mOCC_NUM_RAYS,AO);
  AO = 1.0 - AO.array();
  for (int i = 0; i < V.rows(); ++i) {
    VC.row(i) *= AO(i);
  }

  printf("Constructing adjacency list...\n");
  // Need the adjacency list
  std::vector<std::vector<int> > VV;
  igl::adjacency_list(F, VV);

  std::vector<std::vector<double> > darkWeight;
  darkWeight.resize(VV.size());
  //std::vector<std::vector<double> > curveWeight;
  //curveWeight.resize(VV.size());
  for (int i = 0; i < VV.size(); ++i) {
    for (int j = 0; j < VV[i].size(); ++j) {
      // Just use the value of the ambient occlusion.
      darkWeight[i].push_back(AO(VV[i][j]));
      //curveWeight[i].push_back(1.0 - AO(VV[i][j]));
    }
  }

  // Also caluculate the curvature values.
  printf("Calculating mean curvature...\n");
  Eigen::VectorXd mean_curve;
  getMeanCurvature(V,F, mean_curve);
  std::vector<std::vector<double> > curveWeight;
  curveWeight.resize(VV.size());
  double maxCurve = 0;
  double minCurve = 0;
  for (int i = 0; i < VV.size(); ++i) {
    for (int j = 0; j < VV[i].size(); ++j) {
      // Difference in gausssian curvature
      //double diff = std::abs(VCs(i) - VCs(j));
      double diff = mean_curve(VV[i][j]);
      curveWeight[i].push_back(diff);
      maxCurve = std::max(diff, maxCurve);
      minCurve = std::min(diff, minCurve);
    }
  }

  // Create the colors for mean curviture to use
  Eigen::MatrixXd curve_colors;
  igl::jet(mean_curve, true, curve_colors);

  std::vector<int> all_points;  // will contain all selected points.

  igl::viewer::Viewer viewer;
  viewer.data.set_mesh(V, F);
  viewer.data.set_colors(curve_colors);
  viewer.core.show_lines = false;

  if (file_exists(argv[2])) {
    printf("File exists! Adding points from previous run!\n");
    Eigen::MatrixXd sp;
    Eigen::MatrixXd spf;
    igl::readOFF(argv[2], sp, spf);

    // Create indices.
    Eigen::VectorXi ids(V.rows());
    for (int i = 0; i < V.rows(); ++i) {
      ids(i) = i;
    }

    Eigen::VectorXd D;
    Eigen::VectorXi I;
    Eigen::MatrixXd CP;
    igl::point_mesh_squared_distance(sp, V,ids, D,I,CP);

    // Add them to the viewer.
    for (int i = 0; i < sp.rows(); ++i) {
      all_points.push_back(I(i));
      VC.row(I(i)) << 1,0,0;

      std::stringstream label;
      label << all_points.size();
      viewer.data.add_label(sp.row(i), label.str());
    }
  }

  viewer.callback_key_down = [&](igl::viewer::Viewer& viewer, unsigned char key, int modifier)->bool {
    switch(key) {
      case 'U':
      {
        printf("Undoing last point...\n");
        // Undo the last choice
        int last = all_points.back();
        printf("Undoing last choice... %d\n", last);
        all_points.pop_back();
        VC.row(last) << 1,1,1;
        VC.row(last) *= AO(last);

        // Set the colors back to what they were.
        viewer.data.set_colors(VC);
        viewer.data.labels_positions.conservativeResize(all_points.size(), 3);
        viewer.data.labels_strings.pop_back();
        break;
      }
      case 'C':
        printf("Setting curvature colors...\n");
        viewer.data.set_colors(curve_colors);
        break;
      case 'D':
        printf("Setting colors to highlight darkness...\n");
        viewer.data.set_colors(VC);
        break;
      case 'W':
      {
        if (!writeable) {
          return false;
        }
        printf("Writing output to %s\n", argv[2]);
        // Write to output file
        FILE* ofs = fopen(argv[2], "w");
        fprintf(ofs, "OFF\n");
        fprintf(ofs, "%d 0 0\n", all_points.size());
        for (int idx : all_points) {
          fprintf(ofs, "%f %f %f\n", V(idx, 0), V(idx, 1), V(idx, 2));
        }
        fclose(ofs);
        // You can quit now.
        printf("Finished! Please close the program.\n");
        break;
      }
      default:
        // Question mark ('?')
        if (key == '/' && modifier == GLFW_MOD_SHIFT) {
          printHelp();
        }
    }

    // Make sure this stays the same.
    viewer.core.lighting_factor =
        std::min(std::max(viewer.core.lighting_factor,0.f),1.f);
    return false;
  };
  viewer.callback_mouse_down =
      [&](igl::viewer::Viewer &viewer, int, int)->bool
      {
        if (!writeable) {
          return false;
        }
        // fid will be the face ID. Need to get the vertex ID
        int fid, vid;
        // Cast a ray in the view direction starting from the mouse position
        double x = viewer.current_mouse_x;
        double y = viewer.core.viewport(3) - viewer.current_mouse_y;
        if(igl::embree::unproject_onto_mesh(Eigen::Vector2f(x,y), F,
                                            viewer.core.view * viewer.core.model,
                                            viewer.core.proj,
                                            viewer.core.viewport,
                                            ei,
                                            fid, vid)) {
          // If it already exists, ignore it.
          for (int i : all_points) {
            if (vid == i) return false;
          }

          all_points.push_back(vid);
          VC.row(vid) << 1,0,0;
          viewer.data.set_colors(VC);
          std::stringstream label;
          label << all_points.size();
          viewer.data.add_label(V.row(vid), label.str());

          return true;
        }
        return false;
      };

  printHelp();
  viewer.launch();
}