#include "tf/transform_listener.h"
#include <Eigen/Geometry>
#include <geometry_msgs/Twist.h>
#include <grid_map_msgs/GridMap.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <ros/topic.h>
#include <srrg2_navigation_2d_msgs/PathFollowerStatus.h>
#include <srrg2_navigation_2d_ros/instances.h>
#include <srrg2_navigation_2d_ros/path_follower.h>
#include <srrg2_navigation_2d_ros/tf_helpers.h>
#include <srrg_config/configurable_manager.h>
#include <srrg_converters/converter.h>
#include <srrg_geometry/geometry2d.h>
#include <srrg_pcl/point_types.h>
#include <srrg_qgl_viewport/viewer_core_shared_qgl.h>
#include <srrg_system_utils/parse_command_line.h>
#include <srrg_system_utils/system_utils.h>
#include <thread>

using namespace srrg2_core;
using namespace srrg2_navigation_2d;
using namespace srrg2_navigation_2d_ros;
using srrg2_core_ros::Converter;

ros::Publisher cmd_vel_publisher;

std::shared_ptr<tf::TransformListener> listener;
ObstacleAvoidanceBase::Isometry2fList path_poses;
Point2fVectorCloud path_points;
GridMap2D grid_map;

// hardcoded parameters
std::string map_frame_id       = "map";
std::string cmd_vel_topic      = "/cmd_vel";
std::string path_topic         = "/path";
std::string grid_map_topic     = "grid_map";
std::string base_link_frame_id = "base_link";
std::string path_follower_name = "path_follower";

// ia nice logging :)
size_t total_path_size = 0;
const std::string exe_name("planning_robot");

#define LOG std::cerr << exe_name + "|"
#define DEBUG(var) \
  if (var)         \
  std::cerr

static bool planning_debug = false;

void generateConfig(const std::string& config_file_) {
  ConfigurableManager manager;
  const std::string scan_topic = "/base_scan";
  auto path_follower           = manager.create<PathFollower>(path_follower_name);
  path_follower->param_scan_handler->param_raw_data_preprocessor->param_scan_topic.setValue(
    scan_topic);
  manager.write(config_file_);

  LOG << "default config saved in [ " << config_file_ << " ]\n";
}

void pathCallback(const nav_msgs::Path& path_) {
  // ia cache and log
  total_path_size = path_.poses.size();
  std::cerr << "Got path -- path size [ " << total_path_size << " ]\n";
  path_poses.clear();
  path_points.clear();
  for (size_t i = 0; i < path_.poses.size(); ++i) {
    Point2f current_point;
    Isometry2f iso;
    getIsometry(iso, path_.poses[i].pose);
    path_poses.push_back(iso);
    current_point.coordinates() = iso.translation();
    path_points.emplace_back(std::move(current_point));
  }
}

void gridMapCallback(const grid_map_msgs::GridMapConstPtr& grid_map_) {
  // bb convert to BaseSensorMessagePtr
  BaseSensorMessagePtr grid_map_msg = Converter::convert(grid_map_);
  if (!grid_map_msg) {
    std::cerr << __PRETTY_FUNCTION__ << FG_BCYAN("Was not able to convert the grid map message!!!")
              << std::endl;
    return;
  }
  // bb cast BaseSensorMessagePtr to GridMapMessagePtr
  GridMapMessagePtr grid_map_ptr = std::dynamic_pointer_cast<GridMapMessage>(grid_map_msg);
  grid_map                       = grid_map_ptr->grid_map_struct.value();
  DEBUG(planning_debug) << FG_BCYAN("GRID MAP SIZE: ") << grid_map.GridMap2DHeader::size().x()
                        << "x" << grid_map.GridMap2DHeader::size().y() << std::endl;
}

void node(ViewerCanvasPtr canvas_, std::shared_ptr<PathFollower> path_follower_) {
  ros::NodeHandle nh;
  ros::NodeHandle _nh("~");
  _nh.getParam("map_frame_id", map_frame_id);
  _nh.getParam("base_link_frame_id", base_link_frame_id);
  _nh.getParam("cmd_vel_topic", cmd_vel_topic);
  _nh.getParam("path_topic", path_topic);
  std::cerr << exe_name << ": running with these parameters" << std::endl;
  std::cerr << "_map_frame_id:=" << map_frame_id << std::endl;
  std::cerr << "_base_link_frame_id:=" << base_link_frame_id << std::endl;
  std::cerr << "_cmd_vel_topic:=" << cmd_vel_topic << std::endl;
  std::cerr << "_path_topic:=" << path_topic << std::endl;

  cmd_vel_publisher                   = nh.advertise<geometry_msgs::Twist>(cmd_vel_topic, 10);
  ros::Subscriber path_subscriber     = nh.subscribe("path", 10, pathCallback);
  ros::Subscriber grid_map_subscriber = nh.subscribe(grid_map_topic, 10, gridMapCallback);
  path_follower_->setCanvas(canvas_);
  path_follower_->advertiseTopics(nh);
  ros::Subscriber scan_subscriber = nh.subscribe(
    path_follower_->param_scan_handler->param_raw_data_preprocessor->param_scan_topic.value(),
    10,
    &ScanHandler::scanCallback,
    path_follower_->param_scan_handler.value().get());

  ros::Rate loop_rate(50);
  listener.reset(new tf::TransformListener);

  // bb first of all check that the map is valid
  grid_map_msgs::GridMapConstPtr sharedPtr;
  sharedPtr = ros::topic::waitForMessage<grid_map_msgs::GridMap>(grid_map_topic, nh);
  if (sharedPtr == nullptr) {
    ROS_INFO("No grid map message received!!!!!");
  } else {
    std::cerr << FG_BMAGENTA("GRID MAP MESSAGE RECEIVED") << std::endl;
    gridMapCallback(sharedPtr);
  }
  while (ros::ok()) {
    if (!path_poses.size()) {
      DEBUG(planning_debug) << "Waiting for next path to be received!" << std::endl;
      std::flush(std::cerr);
    } else {
      Isometry2f robot_in_world;
      if (!getTfTransform(
            robot_in_world, *listener, map_frame_id, base_link_frame_id, ros::Time(0))) {
        break;
      }
      geometry_msgs::Twist twist;
      // bb set twist to zero
      // bb path_follower_ will modify it if possible
      twist.linear.x  = 0.f;
      twist.angular.z = 0.f;
      path_follower_->compute(grid_map, robot_in_world, path_points, path_poses, twist);
      // cmd_vel_publisher.publish(twist);
      // bb if goal has been reached within a given tolerance clear path_poses and path_points
      // bb get ready to receive next path
      if (path_follower_->status() == PathFollower::Status::End) {
        path_poses.clear();
        path_points.clear();
        std::cerr << "Path poses size: " << path_poses.size() << std::endl;
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

int main(int argc, char** argv) {
  static const std::string canvas_name("my_canvas");
  ros::init(argc, argv, "path_follower", ros::init_options::NoSigintHandler);
  srrg2_navigation_2d_ros_registerTypes();
  QApplication qapp(argc, argv);
  srrg2_qgl_viewport::ViewerCoreSharedQGL viewer_core(argc, argv, &qapp);
  const ViewerCanvasPtr& canvas = viewer_core.getCanvas(canvas_name);
  srrg2_qgl_viewport::ViewerCoreSharedQGL::stop();
  srrgInit(argc, argv, exe_name.c_str());
  ParseCommandLine cmd_line(argv);

  ArgumentFlag arg_generate_config(&cmd_line, "j", "generate-config", "generates a config file");
  ArgumentString arg_config_file(
    &cmd_line, "c", "config-file", "path to config file", "conf_" + exe_name + ".json");
  cmd_line.parse();

  const std::string& config_file = arg_config_file.value();

  // srrg check for options and params
  if (arg_generate_config.isSet()) {
    LOG << "generating default config\n";
    generateConfig(config_file);
    return 0;
  }
  ConfigurableManager config_manager;
  config_manager.read(config_file);
  std::cerr << "reading config from [" << config_file << "]" << std::endl;

  auto path_follower = config_manager.getByName<PathFollower>(path_follower_name);
  if (!path_follower) {
    throw std::runtime_error("no path follower in configuration");
  }
  auto path_planner = config_manager.getByName<LocalPathPlanner>("local_path_planner");
  if (!path_planner) {
    throw std::runtime_error("no path follower in configuration");
  }
  std::cerr << "vx res: " << path_planner->param_voxelize_res.value() << std::endl;

  node(canvas, path_follower);
  //  std::thread node_t(node, canvas, path_follower);
  //  viewer_core.startViewerServer();
  //  node_t.join();
  return 0;
}


// This file is modified from libigl, a simple c++ geometry processing library.
// libigl is under the Mozilla Public License v. 2.0.
// According to the license restriction, we release this code including the changes
// we made to libigl code under the same license.
//
// Copyright of our changes: (C) 2018 USC
// Code authors of our changes: Yijing Li, Jernej Barbic
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "iglRemeshSelfIntersection.h"

#include <Eigen/Core>
#include "igl/copyleft/cgal/RemeshSelfIntersectionsParam.h"
#include "igl/copyleft/cgal/remesh_self_intersections.h"
#include "igl/remove_unreferenced.h"
#include "igl/unique_edge_map.h"
#include "igl/extract_manifold_patches.h"
#include "igl/copyleft/cgal/extract_cells.h"
#include "igl/copyleft/cgal/propagate_winding_numbers.h"
#include "igl/copyleft/cgal/mesh_boolean.h"
#include "igl/copyleft/cgal/assign_scalar.h"

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <iostream>
#include <algorithm>

namespace iglInterface
{

SelfCutMeshData remeshSelfIntersection(TriMeshRef mesh, bool stitch,
    bool computeWindingNumbers, bool computeDoublePrecisionPos, bool computePatchAndCells)
{
  using namespace Eigen;
  using namespace std;
  SelfCutMeshData ret;

  Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>::MapType VV((double*)mesh.positions(), mesh.numVertices(), 3);
  Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor>::MapType FF((int*)mesh.triangles(), mesh.numTriangles(), 3);

  typedef double Scalar;
  typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
  typedef Kernel::FT ExactScalar;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,3> MatrixX3S;
  typedef Eigen::Matrix<int,Eigen::Dynamic,1> VectorXJ;
  typedef Eigen::Matrix<
      ExactScalar,
      Eigen::Dynamic,
      Eigen::Dynamic,
      Eigen::RowMajor> MatrixXES;
  MatrixXES V;
  Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor> F;
  VectorXJ  CJ;
  Matrix<int, Eigen::Dynamic, 2, Eigen::RowMajor> IF;

  MatrixXES Vr; // unstitched vertices
  Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor>  Fr; // unstitched faces
  Eigen::VectorXi I; // vtxStitchIDs
  {

    igl::copyleft::cgal::RemeshSelfIntersectionsParam params;
    params.stitch_all = stitch;
    //    params.stitch_all = false;

    igl::copyleft::cgal::remesh_self_intersections(
        VV, FF, params, Vr, Fr, IF, CJ, I);
    for(int i = 0; i < Fr.rows(); i++)
    {
      assert(Fr(i,0) != Fr(i, 1) && Fr(i, 0) != Fr(i,2) && Fr(i,1) != Fr(i,2));
    }
    assert(I.size() == Vr.rows());
    if (IF.size() > 0) assert(IF.cols() == 2);
    assert(CJ.size() == Fr.rows());
//    cout << "After remesh, " << endl;
//    cout << "#v " << Vr.rows() << " #f: " << Fr.rows() << endl;

    // Merge coinciding vertices into non-manifold vertices.
    Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor>  Ftmp = Fr;
    std::for_each(Ftmp.data(), Ftmp.data()+Ftmp.size(),
        [&I](typename Matrix<int, Eigen::Dynamic, 3, Eigen::RowMajor>::Scalar& a) { a=I[a]; });
    // Remove unreferenced vertices.
    Eigen::VectorXi UIM;
    igl::remove_unreferenced(Vr, Ftmp, V, F, UIM);
//    cout << "After remove, " << endl;
//    cout << "#v " << V.rows() << " #f: " << F.rows() << endl;
//    cout << "#UIM size " << UIM.size() << endl;
    // assert(UIM.size() == Fr.rows());
    assert(F.rows() == Fr.rows());

    // Eigen::MatrixX3i Ftmp;
    // for(int i = 0; i < Fr.rows(); i++)
    //   Ftmp.row(UIM(i)) = Fr.row(i);
  }

  // Compute edges of (F) --> (E,uE,EMAP,uE2E)
  Eigen::MatrixXi E, uE;
  Eigen::VectorXi EMAP;
  std::vector<std::vector<size_t> > uE2E;
  Eigen::VectorXi P;
  size_t num_patches = 0, num_cells = 0;
  Eigen::MatrixXi per_patch_cells;

  if (computePatchAndCells)
  {
    igl::unique_edge_map(F, E, uE, EMAP, uE2E);
    // Compute patches (F,EMAP,uE2E) --> (P)
    num_patches = igl::extract_manifold_patches(F, EMAP, uE2E, P);
    // Compute cells (V,F,P,E,uE,EMAP) -> (per_patch_cells)
    num_cells = igl::copyleft::cgal::extract_cells(V, F, P, E, uE, uE2E, EMAP, per_patch_cells);
  }

  bool valid = true;
  const size_t num_faces = F.rows();
  Eigen::MatrixXi W;
  Eigen::VectorXi labels = Eigen::VectorXi::Constant(num_faces, 0);
  if (num_faces > 0 && computeWindingNumbers)
  {
    valid = valid &
        igl::copyleft::cgal::propagate_winding_numbers(
            V, F, uE, uE2E, num_patches, P, num_cells, per_patch_cells, labels, W);
    assert((size_t)W.rows() == num_faces);
  } else
  {
    W.resize(0, 2);
  }

  // If W doesn't have enough columns, pad with zeros
  if (W.cols() <= 2)
  {
    const int old_ncols = W.cols();
    W.conservativeResize(num_faces,2);
    W.rightCols(2-old_ncols).setConstant(0);
  }
  assert((size_t)W.cols() == 2);

  // begin exporting data

  vector<Vec3d> Vd(Vr.size(), Vec3d(0.0));
  ret.cutPosExact.resize(Vr.rows());
  for(int i = 0; i < Vr.rows(); i++)
  {
    for(int j = 0; j < 3; j++)
    {
      if (computeDoublePrecisionPos)
        igl::copyleft::cgal::assign_scalar(Vr(i,j), Vd[i][j]); // assign CGAL exact numbers to double
      ret.cutPosExact[i][j] = assignCGALToER(Vr(i,j));
    }
  }

  ret.cutMesh = TriMeshGeo(Vr.rows(), (double*)Vd.data(), Fr.rows(), Fr.data());

  ret.interTriPairs.resize(IF.rows(), pair<int,int>(0,0));
  for(int i = 0; i < IF.rows(); i++)
  {
    ret.interTriPairs[i] = { IF(i,0), IF(i,1) };
  }
  ret.oldTriIDs.assign(CJ.data(), CJ.data() + CJ.size());
  ret.triPatchIDs.assign(P.data(), P.data() + P.size());
  ret.vtxStitchIDs.assign(I.data(), I.data() + I.size());
  ret.cellIDsAtPatch.resize(per_patch_cells.rows(), pair<int,int>(0,0));
  for(int i = 0; i < per_patch_cells.rows(); i++)
  {
    ret.cellIDsAtPatch[i] = { per_patch_cells(i, 0), per_patch_cells(i, 1) };
  }

  ret.windAroundTri.resize(W.rows(), pair<int,int>(0,0));
  for(int i = 0; i < W.rows(); i++)
  {
    ret.windAroundTri[i] = { W(i, 0), W(i, 1) };
  }

  return ret;
}

}


/*
 * Calibrator.h
 *
 *  Created on: Dec 13, 2016
 *      Author: nima
 */

#ifndef SRC_UTILS_CALIBBOARDROBOT_HPP_
#define SRC_UTILS_CALIBBOARDROBOT_HPP_

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <tf_conversions/tf_kdl.h>

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/SVD>

#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>

#include "src/ac_overlay_cv/Drawings.h"
#include "utils/Conversions.hpp"

class RobotToCameraAruco {
public:
    RobotToCameraAruco(std::string node_name);

    void DrawToolTarget(cv::String &instructions, cv::Mat img);

    void Calib2DrawTarget(cv::String &instructions, cv::Mat img);
    void Calib2SaveCalibrationPoint();
    void Calib2CalculateTransformation();

    void Calib1DrawCalibrationAxis(cv::String &instructions, cv::Mat img);
    void Calib1SaveCalibrationPoint();
    void Calib1CalculateTransformation(const std::vector<Eigen::Vector3d> axis_points,
                                       KDL::Frame &transformation);

    void SetROSParameters();

    std::pair<Eigen::Vector3d, Eigen::Vector3d>
    FindAxis(Eigen::MatrixXd axis_points);

    void Reset();

    bool IsCalibrated() { return calib_finished; }

    // ROS Callbacks
    void CameraImageCallback(const sensor_msgs::ImageConstPtr &msg);

    void RobotPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);

    void LeftCameraPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);

    void RightCameraPoseCallback(const geometry_msgs::PoseStamped::ConstPtr
                                &msg);

    cv::Mat &Image(ros::Duration timeout);

    // Eddy Andre addition
    const double length_x = 0.0782, length_y = 0.0514;
    const double  aruco_marker_length_in_meters = 0.023, aruco_marker_separation_in_meters = 0.0;
    std::vector< Eigen::Vector3d> meas_points;
    //std::vector< Eigen::Vector3d> points_on_camera;
    Eigen::Matrix<double, 3, 6> target_mat;
    Eigen::Matrix<double, 3, 6> meas_points_mat;
    Eigen::Matrix4d camera_to_robot;
    //

public:
    CameraIntrinsics camera_intrinsics;
    int cam_id; // 0 for left, 1 for right
    double ros_freq = 0.0;
    KDL::Frame task_frame_to_cam_frame[2];
    KDL::Frame tool_pose_in_robot_frame;
    KDL::Frame tool_pose_in_task_frame;
    KDL::Frame task_frame_to_robot_frame;
    bool task_frame_to_robot_frame_param_present;

    cv::Vec3d task_frame_to_cam_rvec[2], task_frame_to_cam_tvec[2];

private:
    void SetupROS();

    void ReadCameraParameters(const std::string file_path,
                              CameraIntrinsics & camera);

    // MUST BE EVEN NUMBER. The number of points recorded for calibration. default value of 5 is used
    // if no parameter is found on the ros server.
    int num_calib_points = 6;

    // the length of the axis line in [m] shown on the image
    double visual_axis_length = 0.1;

    std::vector<Eigen::Vector3d> measured_points;

    bool calib_finished;

    ros::NodeHandle n;
    ros::Subscriber left_camera_pose_subscriber;
    ros::Subscriber right_camera_pose_subscriber;
    ros::Subscriber robot_pose_subscriber;

    image_transport::Subscriber camera_image_subscriber;
    image_transport::ImageTransport *it;

    // Eddy Andre addition
    //
    //
    //
    std::vector<cv::Point3d> target;
    std::vector<cv::Point2d> calib_points_screen;
    //
    //
    //
    std::vector<cv::Point3d> calib_points_in_board_frame;

    cv::Mat image_msg;

};

///////// Defining the templates

// taken from https://gist.github.com/ialhashim/0a2554076a6cf32831ca
template <class Vector3>
std::pair<Vector3, Vector3> FitLineToPoints(const std::vector<Vector3> &c) {
    // copy coordinates to  matrix in Eigen format
    size_t num_atoms = c.size();
    Eigen::Matrix<typename Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic>
            centers(num_atoms, 3);
    for (size_t i = 0; i < num_atoms; ++i)
        centers.row(i) = c[i];

    Vector3 origin = centers.colwise().mean();
    Eigen::MatrixXd centered = centers.rowwise() - origin.transpose();
    Eigen::MatrixXd cov = centered.adjoint() * centered;
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);
    Vector3 axis = eig.eigenvectors().col(2).normalized();

    return std::make_pair(origin, axis);
}

template <class Vector3>
std::pair<Vector3, Vector3> FitPlaneToPoints(const std::vector<Vector3> &c) {
    // copy coordinates to  matrix in Eigen format
    size_t num_atoms = c.size();
    Eigen::Matrix<typename Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic>
            coord(3, num_atoms);
    for (size_t i = 0; i < num_atoms; ++i)
        coord.col(i) = c[i];

    // calculate centroid
    Vector3 centroid(coord.row(0).mean(), coord.row(1).mean(),
                     coord.row(2).mean());

    // subtract centroid
    coord.row(0).array() -= centroid(0);
    coord.row(1).array() -= centroid(1);
    coord.row(2).array() -= centroid(2);

    // we only need the left-singular matrix here
    //  http://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
    auto svd = coord.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

    Vector3 plane_normal = svd.matrixU().rightCols(1);
    return std::make_pair(centroid, plane_normal);
}

// operator overload to print out vectors
std::ostream &operator<<(std::ostream &out, const std::vector<double> &vect);

#endif /* SRC_UTILS_CALIBBOARDROBOT_HPP_ */


/*
This file is a part of Raman-Scattering-Code-Conversion.
<https://github.com/Kirbologist/Raman-Scattering-Code-Conversion>

Written by Siwan Li for the UQ School of Maths and Physics.
Based on the SMARTIES MATLAB package by W.R.C. Somerville, B. Auguié, E.C. Le Ru
Copyright (C) 2021-2022 Siwan Li

This source code form is subject to the terms of the MIT License.
If a copy of the MIT License was not distributed with this file,
you can obtain one at <https://opensource.org/licenses/MIT>.

This code contains all common or basic header files and typedefs used for arbitrary precision functionality.
*/

#ifndef CORE_MP_HPP
#define CORE_MP_HPP

#include <boost/multiprecision/mpfr.hpp>

using namespace boost::multiprecision;

typedef number<mpfr_float_backend<PRECISION, mpfr_allocation_type::allocate_stack>> RamanFloat;

#endif


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random_spanning_tree.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/graph/named_function_params.hpp>
#include <iostream>
#include <boost/array.hpp>
#include <string>
#include <boost/foreach.hpp>
#include <vector>
#include <boost/unordered_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/property_map/shared_array_property_map.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>

#ifdef DEBUG
#define DEBUG_MSG(str) do { std::cout << str << std::endl; } while( false )
#else
#define DEBUG_MSG(str) do { } while ( false )
#endif

//Defining types
typedef boost::property<boost::edge_weight_t, double> EdgeWeightProperty;
typedef boost::adjacency_list <
    boost::vecS, boost::vecS, boost::directedS,
    boost::no_property,EdgeWeightProperty > digraph_t;

// typedef boost::property<boost::edge_index_t, int> EdgeWeightProperty;
// typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::directedS,
// boost::no_property,boost::property< boost::edge_index_t, std::size_t > > digraph_t;

typedef boost::unordered_map<std::string,double> unordered_map;
boost::random::mt19937 rng;

#ifdef DEBUG_L2
int MAXIT = 10;
#else
int MAXIT = 10000;
#endif

/*
Macro to return "v1->v2"
(Used to avoid implementing a hash function for tuples)
*/
#define getKey(v1,v2) std::to_string((long long unsigned int)v1)+"->"+std::to_string((long long unsigned int)v2)

/*
Initialize a graph using the edge list
*/
void initializeGraph(std::vector<boost::tuple<int,int,double> > edgeList,
                    digraph_t* g,
                    unordered_map* map)
{
    int v1,v2;
    double wt;
    boost::tuple<int,int,double> t;
    BOOST_FOREACH ( t, edgeList)
    {
        v1 = boost::get<0>(t);
        v2 = boost::get<1>(t);
        wt = boost::get<2>(t);
        DEBUG_MSG("Loading: "<<v1<<"->"<<v2<<" : "<<wt);
        add_edge(v1,v2,wt,*g);
        map->insert(unordered_map::value_type(getKey(v1,v2),0));
    }
}

/*
Given an edgeList, run the test case
*/
void runTest(int n_vertices,std::vector<boost::tuple<int,int,double> > edgeList)
{
    digraph_t g;

    unordered_map edgeProb;
    initializeGraph(edgeList,&g,&edgeProb);
    for (unordered_map::iterator it = edgeProb.begin(); it != edgeProb.end(); ++it)
        DEBUG_MSG(it->first << ", " << it->second);

    // boost::shared_array_property_map
    // <
    // double, boost::property_map<digraph_t, boost::edge_weight_t>
    // > weight(num_edges(g), get(edge_weight, g));

    //BGL_FORALL_EDGES(e, g, digraph_t) {put(weight, e, (1. + get(edge_index, g, e)) / num_edges(g));}

    BGL_FORALL_EDGES(e, g, digraph_t)
    {
        std::cout<<e<<std::endl;
        //put(weight, e, (1. + get(edge_index, g, e)) / num_edges(g));
    }

    std::vector<int> predecessors (n_vertices);
    std::vector<double> root_prob (n_vertices);
    for(int i=0;i<n_vertices;i++)
    {
        predecessors[i]=0;
        root_prob[i] = 0;
    }
    int root;
    boost::random::uniform_int_distribution<> dist(0, n_vertices-1);
    for(int i=1;i<=MAXIT;i++)
    {
        //Sample root uniformly
        root = dist(rng);
        #ifdef DEBUG_L2 //Since the DEBUG_MSG macro prints newline
            std::cout<<i<<"|"<<root<<"|,"<<std::flush;
        #endif
        boost::random_spanning_tree(g,rng,
            boost::predecessor_map(
                boost::make_iterator_property_map(
                    predecessors.begin(), get(boost::vertex_index, g))).
            root_vertex(root));

        // boost::random_spanning_tree(g,rng,(boost::graph_traits<digraph_t>::vertex_descriptor)root,
        //     boost::predecessor_map(
        //         boost::make_iterator_property_map(
        //             predecessors.begin(), get(boost::vertex_index, g))));

        //Update counts
        root_prob[root]+=1;
        #ifdef DEBUG_L2
        std::cout<<"Tree Found"<<std::endl;
        #endif

        for(int i=0;i<n_vertices;i++)
        {
            if(predecessors[i]!=-1)
            {
                edgeProb.at(getKey(predecessors[i],i)) +=1;
                #ifdef DEBUG_L2
                std::cout<<predecessors[i]<<"->"<<i<<std::endl;
                #endif
            }
            else
            {
            	#ifdef DEBUG_L2
            	std::cout<<"r<"<<i<<">"std::endl;;
            	#endif
                if(root!=i)
                {
                    std::cout<<"Error. root="<<root<<" i="<<i<<std::endl;
                    exit(1);
                }

            }

        }

    }
    DEBUG_MSG("");

    std::cout<<"---RESULT---"<<std::endl;
    //Normalize root and edge probabilities
    for (int i=0;i<n_vertices;i++)
    {
        root_prob[i]/=MAXIT;
        std::cout<<"Node "<<i<<" : " << root_prob[i] << std::endl;
    }
    for (unordered_map::iterator it = edgeProb.begin(); it != edgeProb.end(); ++it)
    {
        std::cout << it->first << ": " << (it->second/MAXIT) << std::endl;
    }
}

/*
4 node grid

Expected Result (Uniform) : The edges in grid should all have 1/4 probabilities, the edge
moving outside should have
*/
void tc1()
{
    int n_vertices = 4;
    std::cout<<"----------- 4 Node Grid (Uniform) ------------"<<std::endl;
    std::vector<boost::tuple<int,int,double> > edgeList =
    {
        boost::make_tuple(0,1,1),
        boost::make_tuple(1,0,1),
        boost::make_tuple(3,1,100),
        boost::make_tuple(1,3,1),
        boost::make_tuple(2,3,100),
        boost::make_tuple(3,2,1),
        boost::make_tuple(0,2,100),
        boost::make_tuple(2,0,1)
    };
    runTest(n_vertices,edgeList);
    std::cout<<"----------- Done (Check Results Visually) ------------"<<std::endl;
}

void tc2()
{
    int n_vertices = 4;
    std::cout<<"----------- 4 Node Grid (Non Uniform) ------------"<<std::endl;
    std::cout<<"----------- 0-2-3-1 has highest probability ------------"<<std::endl;
    std::cout<<"----------- 2-0-1-3 has second highest probability ------------"<<std::endl;
    std::vector<boost::tuple<int,int,double> > edgeList =
    {
        boost::make_tuple(0,1,50),
        boost::make_tuple(1,0,10),
        boost::make_tuple(3,1,100),
        boost::make_tuple(1,3,50),
        boost::make_tuple(2,3,100),
        boost::make_tuple(3,2,10),
        boost::make_tuple(0,2,100),
        boost::make_tuple(2,0,50)
    };
    runTest(n_vertices,edgeList);
    std::cout<<"----------- Done (Check Results Visually) ------------"<<std::endl;
}

/*
4 node fully connected graph

Expected Result (Uniform) : The edges in grid should all have 1/12 probabilities, the edge
moving outside should have

*/
void tc3()
{
    int n_vertices = 4;
    std::cout<<"----------- 4 Node Fully Connected (Uniform) ------------"<<std::endl;
    std::vector<boost::tuple<int,int,double> > edgeList =
    {
        boost::make_tuple(0,1,10),
        boost::make_tuple(1,0,10),
        boost::make_tuple(3,1,10),
        boost::make_tuple(1,3,10),
        boost::make_tuple(2,3,10),
        boost::make_tuple(3,2,10),
        boost::make_tuple(0,2,10),
        boost::make_tuple(2,0,10),
        boost::make_tuple(0,3,10),
        boost::make_tuple(3,0,10),
        boost::make_tuple(1,2,10),
        boost::make_tuple(2,1,10)
    };
    runTest(n_vertices,edgeList);
    std::cout<<"----------- Done (Check Results Visually) ------------"<<std::endl;

}

/*
Graph with a single edge that appears in all
undirected spanning trees. (2x2 grid graph + 1 extra outgrid edge)

Expected Result (Uniform) : The edges in grid should all have equal probabilities,
the edges moving outside should have probability 0.5 for either direction
*/
void tc4()
{
    int n_vertices = 5;
    std::cout<<"----------- 4 Node Grid (Uniform) +1 ------------"<<std::endl;
    std::vector<boost::tuple<int,int,double> > edgeList =
    {
        boost::make_tuple(0,1,10),
        boost::make_tuple(1,0,10),
        boost::make_tuple(3,1,10),
        boost::make_tuple(1,3,10),
        boost::make_tuple(2,3,10),
        boost::make_tuple(3,2,10),
        boost::make_tuple(0,2,10),
        boost::make_tuple(2,0,10),
        boost::make_tuple(0,4,10),
        boost::make_tuple(4,0,10)
    };
    runTest(n_vertices,edgeList);
    std::cout<<"----------- Done (Check Results Visually) ------------"<<std::endl;

}

int main()
{
    tc1();
    tc2();
    tc3();
    tc4();
    return EXIT_SUCCESS;
}

/*

    digraph_t g;
    add_edge(0, 1,10, g);
    add_edge(0, 2,10, g);
    add_edge(1, 3,10, g);
    add_edge(0, 4,10, g);
    boost::array<int, 5> predecessors;

    add_edge(1, 0,10, g);
    add_edge(2, 0,10, g);
    add_edge(3, 1,10, g);
    add_edge(4, 0,10, g);

    boost::random_spanning_tree(g,rng,
    boost::predecessor_map(predecessors.begin()).root_vertex(0));
    std::cout<<"Spanning Tree"<<std::endl;
    int p = 0;
    while (p != 5)
    {
    std::cout << predecessors[p++] << '\n';
    //p = predecessors[p];
    }
    // You should expect to see 0 1 3 2 4
    return EXIT_SUCCESS;
*/


//==============================================================================
//          Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_COMPLEX_GENERIC_SIGNIFICANTS_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_COMPLEX_GENERIC_SIGNIFICANTS_HPP_INCLUDED

#include <nt2/exponential/functions/significants.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/bitwise_cast.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( significants_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< complex_<floating_<A0> > >)
                              (generic_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return result_type(significants(nt2::real(a0), a1),significants(nt2::imag(a0), a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( significants_, tag::cpu_
                            , (A0)(A1)
                            , (generic_< dry_<floating_<A0> > >)
                              (generic_< integer_<A1> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return bitwise_cast<result_type>(nt2::significants(nt2::real(a0), a1));
    }
  };
} }

#endif


//
// Copyright (c) 2020 CNRS INRIA
//

#include "pinocchio/codegen/cppadcg.hpp"
#include "pinocchio/codegen/code-generator-algo.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "utils/model-generator.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_joint_configuration_code_generation)
{
  typedef double Scalar;

  typedef pinocchio::ModelTpl<Scalar> Model;

  Model model; buildAllJointsModel(model);
  Eigen::VectorXd q1 = Eigen::VectorXd::Random(model.nq);
  Eigen::VectorXd q2 = Eigen::VectorXd::Random(model.nq);
  normalize(model,q1);
  normalize(model,q2);

  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  std::vector<Eigen::VectorXd> results_q(2,Eigen::VectorXd::Zero(model.nq));
  std::vector<Eigen::VectorXd> results_v(2,Eigen::VectorXd::Zero(model.nv));

  //Integrate
  CodeGenIntegrate<double> cg_integrate(model);
  cg_integrate.initLib();
  cg_integrate.loadLib();

  cg_integrate.evalFunction(q1,v, results_q[0]);
  pinocchio::integrate(model, q1,v,results_q[1]);
  BOOST_CHECK(results_q[1].isApprox(results_q[0]));

  cg_integrate.evalFunction(q1,0*v,results_q[0]);
  BOOST_CHECK(results_q[0].isApprox(q1));

  //Difference
  CodeGenDifference<double> cg_difference(model);
  cg_difference.initLib();
  cg_difference.loadLib();

  cg_difference.evalFunction(q1,q2, results_v[0]);
  pinocchio::difference(model,q1,q2,results_v[1]);
  BOOST_CHECK(results_v[1].isApprox(results_v[0]));

  cg_difference.evalFunction(q1,q1,results_v[0]);
  BOOST_CHECK(results_v[0].isZero());

  //dDifference
  CodeGenDDifference<double> cg_dDifference(model);
  cg_dDifference.initLib();
  cg_dDifference.loadLib();

  //ARG0
  std::vector<Eigen::MatrixXd> results_J(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
  cg_dDifference.evalFunction(q1,q2, results_J[0], pinocchio::ARG0);
  pinocchio::dDifference(model,q1,q2,results_J[1], pinocchio::ARG0);
  BOOST_CHECK(results_J[1].isApprox(results_J[0]));

  //ARG1
  cg_dDifference.evalFunction(q1,q2, results_J[0], pinocchio::ARG1);
  pinocchio::dDifference(model,q1,q2,results_J[1], pinocchio::ARG1);
  BOOST_CHECK(results_J[1].isApprox(results_J[0]));

  //ARG0
  cg_dDifference.evalFunction(q1,q1,results_J[0],pinocchio::ARG0);
  BOOST_CHECK((-results_J[0]).isIdentity());

  //ARG1
  cg_dDifference.evalFunction(q1,q1,results_J[0],pinocchio::ARG1);
  BOOST_CHECK(results_J[0].isIdentity());

}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by sam on 11/12/17.
//

#ifndef PERCEPTRON_ITEMNN_HPP
#define PERCEPTRON_ITEMNN_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <opencv2/core.hpp>

template<typename typeEntry>
class ItemNN {
private:
    /**
     * vector who contains all the set of the image
     */
    std::vector<boost::numeric::ublas::vector<typeEntry>> entries;
    cv::Mat const mat_image;
    typeEntry const expected;

    boost::numeric::ublas::vector<typeEntry> (*f_itemToVec)(const cv::Mat &);

public:
    ItemNN(const double expected, const cv::Mat item, boost::numeric::ublas::vector<typeEntry> (*f_itemToVec)(const cv::Mat &)):expected( expected), mat_image(item), f_itemToVec(f_itemToVec) {
        entries.push_back(f_itemToVec(item));
        //To present to the perceptron the images in all the orientations size uncomment this
//        for (int i = 0; i < 3; ++i) {
//            cv::Mat dst;
//            cv::rotate(item, dst, cv::ROTATE_90_CLOCKWISE + 1);
//            entries.push_back(f_itemToVec(dst));
//        }
//        cv::Mat dst;
//        cv::flip(item, dst, 1);
//        entries.push_back(f_itemToVec(dst));
    };

    /**
     * function that return the image without bieng rotated or flipped
     * @return the vector of elements
     */
    const boost::numeric::ublas::vector<typeEntry> &getEntry() const {
        return entries[0];
    }
    const cv::Mat &getItem() const {
        return mat_image;
    }
    const std::vector<boost::numeric::ublas::vector<typeEntry>> &getVecEntries() const {
        return this->entries;
    }

    typeEntry const getExpected() const {
        return expected;
    }
};

#endif //PERCEPTRON_ITEMNN_HPP


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to
/// deal in the Software without restriction, including without limitation the
/// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
/// sell copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions ofthe Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
/// IN THE SOFTWARE.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <cmath>
#include <utility>
#include <vector>
#include <index/pq.hpp>
#include <boost/math/special_functions/beta.hpp>
#include "query/sorter/bucket_sorter.hpp"

#include "parameters.hpp"
#include "query.hpp"
#include "index/transformer/norm_range.hpp"

namespace ss {

    template<
            class DataType,
            class KeyType,
            class HashingIndexType,
            class Transformer
    >
    class RationalALSHRanking :
            public Query <DataType> {
        using MapType   = unordered_map<KeyType, vector<int>, ss::SSHasher<KeyType> >;
        using IndexType = NormRangeIndex<DataType, KeyType, HashingIndexType, Transformer >;
    private:
        const MapType &                          _index_map;
        BucketSorter<DataType, KeyType > *       _sorter;
    public:
        ~RationalALSHRanking() {}

        explicit RationalALSHRanking(
                IndexType *                        index,
                const DataType *                   query,
                const Metric<DataType > &          metric,
                const Matrix<DataType > &          data,
                const parameter &                  para)
                : Query<DataType >(index, query, metric, data, para),
                  _index_map(index->getIndexMap()){

            { /// construction here,

                /// calculate query's hash value by hash function
                KeyType query_hash = index->HashQuery(query, -1);

                /// distance function from query to bucket(designated by index_key)
                auto distor = [this, &query_hash, &para, index](const KeyType & index_key) {

                    std::pair<int, DataType > hash_dist_and_norm
                            = index->HashDistAndPercentile(index_key, query_hash);
                    int num_same_bit = para.num_bit - hash_dist_and_norm.first;
                    DataType u = hash_dist_and_norm.second;
                    return this->distance(num_same_bit, u);
                };
                _sorter = new BucketSorter<DataType, KeyType>(_index_map, distor);

            } /// end construction
        }

        double NormalCFD(double value) {
            return 0.5 * erfc(-value * M_SQRT1_2);
        }

        double Fr(double d) {
            double r_d = this->_para.r / d;
            return 1.0 - 2.0*NormalCFD(-r_d)
                   - 2.0 * (1 - std::pow(M_E, -r_d*r_d/2.0)) / std::sqrt(2.0 * M_PI ) / r_d;
        }

        double DistanceByProbability(double p) {
            double mid;
            double a = 0.0;
            int m = this->_para.transformed_dim;
            double u = 0.83;
            double b = std::sqrt(1+m/4.0+std::pow(u, 1<<(m+1)) + 2 * u); //upper bound of distance

            int i=0;
            while(i < 500) {
                mid=(a+b)/2;
                if(std::abs(p - Fr(mid)) < 0.00001)
                    return mid;
                if((Fr(mid)-p)*(Fr(a)-p)>0)
                    a=mid;
                else
                    b=mid;
                i++;
            }
            return mid;
        }

        DataType distance(int num_same_bit, DataType u)  {
            DataType probability = num_same_bit / static_cast<DataType>(this->_para.num_bit);
            DataType shift       = 0.40f;
            probability          = shift + (1 - shift) * probability;
            double d = DistanceByProbability(probability);
            DataType dist = - u * (1 + this->_para.transformed_dim/4 - d * d);
            return dist;
        }

        const vector<int> &  NextBucket() override {
            typename MapType::const_iterator bucket;
            while ( (bucket = _index_map.find(_sorter->NextBucket()) ) ==_index_map.end()) {}
            return bucket->second;
        }

        bool NextBucketExisted() const override {
            return _sorter->NextBucketExisted();
        }
    };
}  //end namespace ss


#include <igl/circulation.h>
#include <igl/collapse_edge.h>
#include <igl/edge_flaps.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/read_triangle_mesh.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Core>
#include <iostream>
#include <set>

#include "tutorial_shared_path.h"

int main(int argc, char * argv[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  cout<<"Usage: ./703_Decimation_bin [filename.(off|obj|ply)]"<<endl;
  cout<<"  [space]  toggle animation."<<endl;
  cout<<"  'r'  reset."<<endl;
  // Load a closed manifold mesh
  string filename(TUTORIAL_SHARED_PATH "/fertility.off");
  if(argc>=2)
  {
    filename = argv[1];
  }
  MatrixXd V,OV;
  MatrixXi F,OF;
  read_triangle_mesh(filename,OV,OF);

  igl::opengl::glfw::Viewer viewer;

  // Prepare array-based edge data structures and priority queue
  VectorXi EMAP;
  MatrixXi E,EF,EI;
  typedef std::set<std::pair<double,int> > PriorityQueue;
  PriorityQueue Q;
  std::vector<PriorityQueue::iterator > Qit;
  // If an edge were collapsed, we'd collapse it to these points:
  MatrixXd C;
  int num_collapsed;

  // Function to reset original mesh and data structures
  const auto & reset = [&]()
  {
    F = OF;
    V = OV;
    edge_flaps(F,E,EMAP,EF,EI);
    Qit.resize(E.rows());

    C.resize(E.rows(),V.cols());
    VectorXd costs(E.rows());
    Q.clear();
    for(int e = 0;e<E.rows();e++)
    {
      double cost = e;
      RowVectorXd p(1,3);
      shortest_edge_and_midpoint(e,V,F,E,EMAP,EF,EI,cost,p);
      C.row(e) = p;
      Qit[e] = Q.insert(std::pair<double,int>(cost,e)).first;
    }
    num_collapsed = 0;
    viewer.data().clear();
    viewer.data().set_mesh(V,F);
    viewer.data().set_face_based(true);
  };

  const auto &pre_draw = [&](igl::opengl::glfw::Viewer & viewer)->bool
  {
    // If animating then collapse 10% of edges
    if(viewer.core().is_animating && !Q.empty())
    {
      bool something_collapsed = false;
      // collapse edge
      const int max_iter = std::ceil(0.01*Q.size());
      for(int j = 0;j<max_iter;j++)
      {
        if(!collapse_edge(
          shortest_edge_and_midpoint, V,F,E,EMAP,EF,EI,Q,Qit,C))
        {
          break;
        }
        something_collapsed = true;
        num_collapsed++;
      }

      if(something_collapsed)
      {
        viewer.data().clear();
        viewer.data().set_mesh(V,F);
        viewer.data().set_face_based(true);
      }
    }
    return false;
  };

  const auto &key_down =
    [&](igl::opengl::glfw::Viewer &viewer,unsigned char key,int mod)->bool
  {
    switch(key)
    {
      case ' ':
        viewer.core().is_animating ^= 1;
        break;
      case 'R':
      case 'r':
        reset();
        break;
      default:
        return false;
    }
    return true;
  };

  reset();
  viewer.core().background_color.setConstant(1);
  viewer.core().is_animating = true;
  viewer.callback_key_down = key_down;
  viewer.callback_pre_draw = pre_draw;
  return viewer.launch();
}


#ifdef _MSC_VER
#define _CRT_SECURE_NO_WARNINGS
#endif

#define _USE_MATH_DEFINES
#include "gate_factory.hpp"

#include <Eigen/QR>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <iostream>

#include "exception.hpp"
#include "gate.hpp"
#include "gate_matrix.hpp"
#include "gate_matrix_diagonal.hpp"
#include "gate_matrix_sparse.hpp"
#include "gate_merge.hpp"
#include "gate_named_one.hpp"
#include "gate_named_pauli.hpp"
#include "gate_named_two.hpp"
#include "gate_noisy_evolution.hpp"
#include "gate_reflect.hpp"
#include "gate_reversible.hpp"
#include "type.hpp"

namespace gate {
ComplexMatrix get_IBMQ_matrix(double theta, double phi, double lambda);

QuantumGateBase* Identity(UINT qubit_index) {
    return new ClsIGate(qubit_index);
}
QuantumGateBase* X(UINT qubit_index) { return new ClsXGate(qubit_index); }
QuantumGateBase* Y(UINT qubit_index) { return new ClsYGate(qubit_index); }
QuantumGateBase* Z(UINT qubit_index) { return new ClsZGate(qubit_index); }
QuantumGateBase* H(UINT qubit_index) { return new ClsHGate(qubit_index); }
QuantumGateBase* S(UINT qubit_index) { return new ClsSGate(qubit_index); }
QuantumGateBase* Sdag(UINT qubit_index) { return new ClsSdagGate(qubit_index); }
QuantumGateBase* T(UINT qubit_index) { return new ClsTGate(qubit_index); }
QuantumGateBase* Tdag(UINT qubit_index) { return new ClsTdagGate(qubit_index); }
QuantumGateBase* sqrtX(UINT qubit_index) {
    return new ClsSqrtXGate(qubit_index);
}
QuantumGateBase* sqrtXdag(UINT qubit_index) {
    return new ClsSqrtXdagGate(qubit_index);
}
QuantumGateBase* sqrtY(UINT qubit_index) {
    return new ClsSqrtYGate(qubit_index);
}
QuantumGateBase* sqrtYdag(UINT qubit_index) {
    return new ClsSqrtYdagGate(qubit_index);
}
QuantumGateBase* P0(UINT qubit_index) { return new ClsP0Gate(qubit_index); }
QuantumGateBase* P1(UINT qubit_index) { return new ClsP1Gate(qubit_index); }
QuantumGateBase* RX(UINT qubit_index, double angle) {
    return new ClsRXGate(qubit_index, angle);
}
QuantumGateBase* RY(UINT qubit_index, double angle) {
    return new ClsRYGate(qubit_index, angle);
}
QuantumGateBase* RZ(UINT qubit_index, double angle) {
    return new ClsRZGate(qubit_index, angle);
}

ComplexMatrix get_IBMQ_matrix(double theta, double phi, double lambda) {
    CPPCTYPE im(0, 1);
    CPPCTYPE exp_val1 = exp(im * phi);
    CPPCTYPE exp_val2 = exp(im * lambda);
    CPPCTYPE cos_val = cos(theta / 2);
    CPPCTYPE sin_val = sin(theta / 2);

    ComplexMatrix matrix(2, 2);
    matrix(0, 0) = cos_val;
    matrix(0, 1) = -exp_val2 * sin_val;
    matrix(1, 0) = exp_val1 * sin_val;
    matrix(1, 1) = exp_val1 * exp_val2 * cos_val;
    return matrix;
}
QuantumGateBase* U1(UINT qubit_index, double lambda) {
    ComplexMatrix matrix = get_IBMQ_matrix(0, 0, lambda);
    std::vector<UINT> vec;
    vec.push_back(qubit_index);
    return new QuantumGateMatrix(vec, matrix);
}
QuantumGateBase* U2(UINT qubit_index, double phi, double lambda) {
    ComplexMatrix matrix = get_IBMQ_matrix(M_PI / 2, phi, lambda);
    std::vector<UINT> vec;
    vec.push_back(qubit_index);
    return new QuantumGateMatrix(vec, matrix);
}
QuantumGateBase* U3(UINT qubit_index, double theta, double phi, double lambda) {
    ComplexMatrix matrix = get_IBMQ_matrix(theta, phi, lambda);
    std::vector<UINT> vec;
    vec.push_back(qubit_index);
    return new QuantumGateMatrix(vec, matrix);
}

QuantumGateBase* CNOT(UINT control_qubit_index, UINT target_qubit_index) {
    if (control_qubit_index == target_qubit_index) {
        throw InvalidControlQubitException(
            "Error: gate::CNOT(UINT, UINT): control_qubit_index and "
            "target_qubit_index has the same value."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new ClsCNOTGate(control_qubit_index, target_qubit_index);
}
QuantumGateBase* CZ(UINT control_qubit_index, UINT target_qubit_index) {
    if (control_qubit_index == target_qubit_index) {
        throw InvalidControlQubitException(
            "Error: gate::CZ(UINT, UINT): control_qubit_index and "
            "target_qubit_index has the same value."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new ClsCZGate(control_qubit_index, target_qubit_index);
}
QuantumGateBase* SWAP(UINT qubit_index1, UINT qubit_index2) {
    if (qubit_index1 == qubit_index2) {
        throw DuplicatedQubitIndexException(
            "Error: gate::SWAP(UINT, UINT): two indices have the same value."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new ClsSWAPGate(qubit_index1, qubit_index2);
}

QuantumGateBase* Pauli(std::vector<UINT> target, std::vector<UINT> pauli_id) {
    if (!check_is_unique_index_list(target)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::Pauli(std::vector<UINT> target, "
            "std::vector<UINT>pauli_id): target list contains "
            "duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    auto pauli = new PauliOperator(target, pauli_id);
    return new ClsPauliGate(pauli);
}
QuantumGateBase* PauliRotation(
    std::vector<UINT> target, std::vector<UINT> pauli_id, double angle) {
    if (!check_is_unique_index_list(target)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::PauliRotation(std::vector<UINT> target, "
            "std::vector<UINT>pauli_id, double angle): target list "
            "contains duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    auto pauli = new PauliOperator(target, pauli_id, angle);
    return new ClsPauliRotationGate(angle, pauli);
}

QuantumGateMatrix* DenseMatrix(UINT target_index, ComplexMatrix matrix) {
    std::vector<UINT> target_list(1, target_index);
    return new QuantumGateMatrix(target_list, matrix);
}
QuantumGateMatrix* DenseMatrix(
    std::vector<UINT> target_list, ComplexMatrix matrix) {
    if (!check_is_unique_index_list(target_list)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::DenseMatrix(std::vector<UINT> target_list, "
            "ComplexMatrix matrix): target list contains duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new QuantumGateMatrix(target_list, matrix);
}

QuantumGateBase* SparseMatrix(
    std::vector<UINT> target_list, SparseComplexMatrix matrix) {
    if (!check_is_unique_index_list(target_list)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::SparseMatrix(std::vector<UINT> target_list, "
            "SparseComplexMatrix matrix): target list contains duplicated "
            "values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new QuantumGateSparseMatrix(target_list, matrix);
}
QuantumGateBase* DiagonalMatrix(
    std::vector<UINT> target_list, ComplexVector diagonal_element) {
    if (!check_is_unique_index_list(target_list)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::DiagonalMatrix(std::vector<UINT> target_list, "
            "ComplexVector diagonal_element): target list contains "
            "duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new QuantumGateDiagonalMatrix(target_list, diagonal_element);
}

QuantumGateMatrix* RandomUnitary(std::vector<UINT> target_list) {
    if (!check_is_unique_index_list(target_list)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::RandomUnitary(std::vector<UINT> target_list): "
            "target list contains duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    Random random;
    UINT qubit_count = (UINT)target_list.size();
    ITYPE dim = 1ULL << qubit_count;
    ComplexMatrix matrix(dim, dim);
    for (ITYPE i = 0; i < dim; ++i) {
        for (ITYPE j = 0; j < dim; ++j) {
            matrix(i, j) = (random.normal() + 1.i * random.normal()) / sqrt(2.);
        }
    }
    Eigen::HouseholderQR<ComplexMatrix> qr_solver(matrix);
    ComplexMatrix Q = qr_solver.householderQ();
    // actual R matrix is upper-right triangle of matrixQR
    auto R = qr_solver.matrixQR();
    for (ITYPE i = 0; i < dim; ++i) {
        CPPCTYPE phase = R(i, i) / abs(R(i, i));
        for (ITYPE j = 0; j < dim; ++j) {
            Q(j, i) *= phase;
        }
    }
    return new QuantumGateMatrix(target_list, Q);
}
QuantumGateMatrix* RandomUnitary(std::vector<UINT> target_list, UINT seed) {
    if (!check_is_unique_index_list(target_list)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::RandomUnitary(std::vector<UINT> target_list): "
            "target list contains duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    Random random;
    random.set_seed(seed);
    UINT qubit_count = (UINT)target_list.size();
    ITYPE dim = 1ULL << qubit_count;
    ComplexMatrix matrix(dim, dim);
    for (ITYPE i = 0; i < dim; ++i) {
        for (ITYPE j = 0; j < dim; ++j) {
            matrix(i, j) = (random.normal() + 1.i * random.normal()) / sqrt(2.);
        }
    }
    Eigen::HouseholderQR<ComplexMatrix> qr_solver(matrix);
    ComplexMatrix Q = qr_solver.householderQ();
    // actual R matrix is upper-right triangle of matrixQR
    auto R = qr_solver.matrixQR();
    for (ITYPE i = 0; i < dim; ++i) {
        CPPCTYPE phase = R(i, i) / abs(R(i, i));
        for (ITYPE j = 0; j < dim; ++j) {
            Q(j, i) *= phase;
        }
    }
    return new QuantumGateMatrix(target_list, Q);
}
QuantumGateBase* ReversibleBoolean(std::vector<UINT> target_qubit_index_list,
    std::function<ITYPE(ITYPE, ITYPE)> function_ptr) {
    if (!check_is_unique_index_list(target_qubit_index_list)) {
        throw DuplicatedQubitIndexException(
            "Error: gate::ReversibleBoolean(std::vector<UINT> "
            "target_qubit_index_list, std::function<ITYPE(ITYPE,ITYPE)> "
            "function_ptr): target list contains duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    return new ClsReversibleBooleanGate(target_qubit_index_list, function_ptr);
}
QuantumGateBase* StateReflection(const QuantumStateBase* reflection_state) {
    return new ClsStateReflectionGate(reflection_state);
}

QuantumGateBase* BitFlipNoise(UINT target_index, double prob) {
    auto gate0 = X(target_index);
    auto gate1 = Identity(target_index);
    auto new_gate =
        new QuantumGate_Probabilistic({prob, 1 - prob}, {gate0, gate1});
    delete gate0;
    delete gate1;
    return new_gate;
}
QuantumGateBase* DephasingNoise(UINT target_index, double prob) {
    auto gate0 = Z(target_index);
    auto gate1 = Identity(target_index);
    auto new_gate =
        new QuantumGate_Probabilistic({prob, 1 - prob}, {gate0, gate1});
    delete gate0;
    delete gate1;
    return new_gate;
}
QuantumGateBase* IndependentXZNoise(UINT target_index, double prob) {
    auto gate0 = X(target_index);
    auto gate1 = Z(target_index);
    auto gate2 = Y(target_index);
    auto gate3 = Identity(target_index);
    double p1 = prob * (1 - prob);
    double p2 = prob * prob;
    auto new_gate = new QuantumGate_Probabilistic(
        {p1, p1, p2, 1 - 2 * p1 - p2}, {gate0, gate1, gate2, gate3});
    delete gate0;
    delete gate1;
    delete gate2;
    return new_gate;
}
QuantumGateBase* DepolarizingNoise(UINT target_index, double prob) {
    auto gate0 = X(target_index);
    auto gate1 = Z(target_index);
    auto gate2 = Y(target_index);
    auto gate3 = Identity(target_index);
    auto new_gate = new QuantumGate_Probabilistic(
        {prob / 3, prob / 3, prob / 3, 1 - prob}, {gate0, gate1, gate2, gate3});
    delete gate0;
    delete gate1;
    delete gate2;
    delete gate3;
    return new_gate;
}
QuantumGateBase* TwoQubitDepolarizingNoise(
    UINT target_index1, UINT target_index2, double prob) {
    if (target_index1 == target_index2) {
        throw DuplicatedQubitIndexException(
            "Error: gate::TwoQubitDepolarizingNoise(UINT, UINT, double): "
            "target list contains duplicated values."
            "\nInfo: NULL used to be returned, "
            "but it changed to throw exception.");
    }
    std::vector<QuantumGateBase*> gate_list;
    for (int i = 0; i < 16; ++i) {
        if (i != 0) {
            UINT pauli_qubit1 = i % 4;
            UINT pauli_qubit2 = i / 4;
            auto gate_pauli = Pauli(
                {target_index1, target_index2}, {pauli_qubit1, pauli_qubit2});
            auto gate_dense = gate::to_matrix_gate(gate_pauli);
            gate_list.push_back(gate_dense);
        } else {
            gate_list.push_back(Identity(target_index1));
        }
    }
    std::vector<double> probabilities(16, prob / 15);
    probabilities[0] = 1 - prob;
    auto new_gate = new QuantumGate_Probabilistic(probabilities, gate_list);
    for (UINT gate_index = 0; gate_index < 15; ++gate_index) {
        delete gate_list[gate_index];
    }
    return new_gate;
}
QuantumGateBase* AmplitudeDampingNoise(UINT target_index, double prob) {
    ComplexMatrix damping_matrix_0(2, 2), damping_matrix_1(2, 2);
    damping_matrix_0 << 1, 0, 0, sqrt(1 - prob);
    damping_matrix_1 << 0, sqrt(prob), 0, 0;
    auto gate0 = DenseMatrix({target_index}, damping_matrix_0);
    auto gate1 = DenseMatrix({target_index}, damping_matrix_1);
    auto new_gate = new QuantumGate_CPTP({gate0, gate1});
    delete gate0;
    delete gate1;
    return new_gate;
}
QuantumGateBase* Measurement(
    UINT target_index, UINT classical_register_address) {
    auto gate0 = P0(target_index);
    auto gate1 = P1(target_index);
    auto new_gate =
        new QuantumGate_Instrument({gate0, gate1}, classical_register_address);
    delete gate0;
    delete gate1;
    return new_gate;
}

QuantumGateBase* NoisyEvolution(Observable* hamiltonian,
    std::vector<GeneralQuantumOperator*> c_ops, double time, double dt) {
    return new ClsNoisyEvolution(hamiltonian, c_ops, time, dt);
}

QuantumGateBase* create_quantum_gate_from_string(std::string gate_string) {
    const char* gateString = gate_string.c_str();
    char* sbuf;
    const char delim[] = " ";
    std::vector<UINT> targets;
    QuantumGateBase* gate = NULL;
    char* buf = (char*)calloc(strlen(gateString) + 1, sizeof(char));
    strcpy(buf, gateString);
    sbuf = strtok(buf, delim);

    if (strcasecmp(sbuf, "I") == 0)
        gate = gate::Identity(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "X") == 0)
        gate = gate::X(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "Y") == 0)
        gate = gate::Y(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "Z") == 0)
        gate = gate::Z(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "H") == 0)
        gate = gate::H(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "S") == 0)
        gate = gate::S(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "Sdag") == 0)
        gate = gate::Sdag(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "T") == 0)
        gate = gate::T(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "Tdag") == 0)
        gate = gate::Tdag(atoi(strtok(NULL, delim)));
    else if (strcasecmp(sbuf, "CNOT") == 0 || strcasecmp(sbuf, "CX") == 0) {
        unsigned int control = atoi(strtok(NULL, delim));
        unsigned int target = atoi(strtok(NULL, delim));
        gate = gate::CNOT(control, target);
    } else if (strcasecmp(sbuf, "CZ") == 0) {
        unsigned int control = atoi(strtok(NULL, delim));
        unsigned int target = atoi(strtok(NULL, delim));
        gate = gate::CZ(control, target);
    } else if (strcasecmp(sbuf, "SWAP") == 0) {
        unsigned int target1 = atoi(strtok(NULL, delim));
        unsigned int target2 = atoi(strtok(NULL, delim));
        gate = gate::SWAP(target1, target2);
    } else if (strcasecmp(sbuf, "U1") == 0) {
        unsigned int target = atoi(strtok(NULL, delim));
        double theta1 = atof(strtok(NULL, delim));
        gate = gate::U1(target, theta1);
    } else if (strcasecmp(sbuf, "U2") == 0) {
        unsigned int target = atoi(strtok(NULL, delim));
        double theta1 = atof(strtok(NULL, delim));
        double theta2 = atof(strtok(NULL, delim));
        gate = gate::U2(target, theta1, theta2);
    } else if (strcasecmp(sbuf, "U3") == 0) {
        unsigned int target = atoi(strtok(NULL, delim));
        double theta1 = atof(strtok(NULL, delim));
        double theta2 = atof(strtok(NULL, delim));
        double theta3 = atof(strtok(NULL, delim));
        gate = gate::U3(target, theta1, theta2, theta3);
    } else if (strcasecmp(sbuf, "RX") == 0) {
        unsigned int target = atoi(strtok(NULL, delim));
        double theta = atof(strtok(NULL, delim));
        gate = gate::RX(target, theta);
    } else if (strcasecmp(sbuf, "RY") == 0) {
        unsigned int target = atoi(strtok(NULL, delim));
        double theta = atof(strtok(NULL, delim));
        gate = gate::RY(target, theta);
    } else if (strcasecmp(sbuf, "RZ") == 0) {
        unsigned int target = atoi(strtok(NULL, delim));
        double theta = atof(strtok(NULL, delim));
        gate = gate::RZ(target, theta);
    } else if (strcasecmp(sbuf, "RM") == 0) {
        char* pauliStr = strtok(NULL, delim);
        unsigned int targetCount = (UINT)strlen(pauliStr);

        std::vector<UINT> pauli(targetCount, 0);
        for (unsigned int i = 0; i < targetCount; i++) {
            if (pauliStr[i] == 'x' || pauliStr[i] == 'X')
                pauli[i] = 1;
            else if (pauliStr[i] == 'y' || pauliStr[i] == 'Y')
                pauli[i] = 2;
            else if (pauliStr[i] == 'z' || pauliStr[i] == 'Z')
                pauli[i] = 3;
        }

        targets = std::vector<UINT>(targetCount, 0);
        for (unsigned int i = 0; i < targetCount; i++) {
            targets[i] = atoi(strtok(NULL, delim));
        }

        double theta = atof(strtok(NULL, delim));
        gate = gate::PauliRotation(targets, pauli, theta);
    } else if (strcasecmp(sbuf, "U") == 0) {
        unsigned int targetCount = atoi(strtok(NULL, delim));

        targets = std::vector<UINT>(targetCount, 0);
        for (unsigned int i = 0; i < targetCount; i++) {
            targets[i] = atoi(strtok(NULL, delim));
        }
        ITYPE dim = 1ULL << targetCount;
        ComplexMatrix matrix(dim, dim);

        for (ITYPE i = 0; i < dim * dim; i++) {
            char* token;
            token = strtok(NULL, delim);
            matrix(i / dim, i % dim) = atof(token);
            token = strtok(NULL, delim);
            matrix(i / dim, i % dim) += CPPCTYPE(0, 1) * atof(token);
        }
        gate = gate::DenseMatrix(targets, matrix);
    } else {
        throw InvalidGateIdentifierException(
            "Error: "
            "gate::create_quantum_gate_from_string(string): invalid gate "
            "name " +
            std::string(sbuf));
    }
    free(buf);
    return gate;
}

}  // namespace gate


/**
 * \file
 * \author Thomas Fischer
 * \date   2010-03-17
 * \brief  Implementation of analytical geometry functions.
 *
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AnalyticalGeometry.h"

#include <algorithm>
#include <cmath>
#include <limits>

#include <Eigen/Dense>

#include "BaseLib/StringTools.h"

#include "Polyline.h"
#include "PointVec.h"

#include "MathLib/GeometricBasics.h"

extern double orient2d(double *, double *, double *);
extern double orient2dfast(double*, double*, double*);

namespace ExactPredicates
{
double getOrientation2d(MathLib::Point3d const& a,
    MathLib::Point3d const& b, MathLib::Point3d const& c)
{
    return orient2d(const_cast<double*>(a.getCoords()),
        const_cast<double*>(b.getCoords()),
        const_cast<double*>(c.getCoords()));
}

double getOrientation2dFast(MathLib::Point3d const& a,
                            MathLib::Point3d const& b,
                            MathLib::Point3d const& c)
{
    return orient2dfast(const_cast<double*>(a.getCoords()),
                        const_cast<double*>(b.getCoords()),
                        const_cast<double*>(c.getCoords()));
}
}  // namespace ExactPredicates

namespace GeoLib
{
Orientation getOrientation(MathLib::Point3d const& p0,
                           MathLib::Point3d const& p1,
                           MathLib::Point3d const& p2)
{
    double const orientation = ExactPredicates::getOrientation2d(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

Orientation getOrientationFast(MathLib::Point3d const& p0,
                               MathLib::Point3d const& p1,
                               MathLib::Point3d const& p2)
{
    double const orientation =
        ExactPredicates::getOrientation2dFast(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

bool parallel(MathLib::Vector3 v, MathLib::Vector3 w)
{
    const double eps(std::numeric_limits<double>::epsilon());

    // check degenerated cases
    if (v.getLength() < eps)
    {
        return false;
    }

    if (w.getLength() < eps)
    {
        return false;
    }

    v.normalize();
    w.normalize();

    bool parallel(true);
    if (std::abs(v[0] - w[0]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[1] - w[1]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[2] - w[2]) > eps)
    {
        parallel = false;
    }

    if (! parallel) {
        parallel = true;
        // change sense of direction of v_normalised
        v *= -1.0;
        // check again
        if (std::abs(v[0] - w[0]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[1] - w[1]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[2] - w[2]) > eps)
        {
            parallel = false;
        }
    }

    return parallel;
}

bool lineSegmentIntersect(GeoLib::LineSegment const& s0,
                          GeoLib::LineSegment const& s1,
                          GeoLib::Point& s)
{
    GeoLib::Point const& a{s0.getBeginPoint()};
    GeoLib::Point const& b{s0.getEndPoint()};
    GeoLib::Point const& c{s1.getBeginPoint()};
    GeoLib::Point const& d{s1.getEndPoint()};

    if (!isCoplanar(a, b, c, d))
    {
        return false;
    }

    // handle special cases here to avoid computing intersection numerical
    if (MathLib::sqrDist(a, c) < std::numeric_limits<double>::epsilon() ||
        MathLib::sqrDist(a, d) < std::numeric_limits<double>::epsilon()) {
        s = a;
        return true;
    }
    if (MathLib::sqrDist(b, c) < std::numeric_limits<double>::epsilon() ||
        MathLib::sqrDist(b, d) < std::numeric_limits<double>::epsilon()) {
        s = b;
        return true;
    }

    MathLib::Vector3 const v(a, b);
    MathLib::Vector3 const w(c, d);
    MathLib::Vector3 const qp(a, c);
    MathLib::Vector3 const pq(c, a);

    auto isLineSegmentIntersectingAB = [&v](MathLib::Vector3 const& ap,
                                            std::size_t i)
    {
        // check if p is located at v=(a,b): (ap = t*v, t in [0,1])
        return 0.0 <= ap[i] / v[i] && ap[i] / v[i] <= 1.0;
    };

    if (parallel(v,w)) { // original line segments (a,b) and (c,d) are parallel
        if (parallel(pq,v)) { // line segment (a,b) and (a,c) are also parallel
            // Here it is already checked that the line segments (a,b) and (c,d)
            // are parallel. At this point it is also known that the line
            // segment (a,c) is also parallel to (a,b). In that case it is
            // possible to express c as c(t) = a + t * (b-a) (analog for the
            // point d). Since the evaluation of all three coordinate equations
            // (x,y,z) have to lead to the same solution for the parameter t it
            // is sufficient to evaluate t only once.

            // Search id of coordinate with largest absolute value which is will
            // be used in the subsequent computations. This prevents division by
            // zero in case the line segments are parallel to one of the
            // coordinate axis.
            std::size_t i_max(std::abs(v[0]) <= std::abs(v[1]) ? 1 : 0);
            i_max = std::abs(v[i_max]) <= std::abs(v[2]) ? 2 : i_max;
            if (isLineSegmentIntersectingAB(qp, i_max)) {
                s = c;
                return true;
            }
            MathLib::Vector3 const ad(a, d);
            if (isLineSegmentIntersectingAB(ad, i_max)) {
                s = d;
                return true;
            }
            return false;
        }
        return false;
    }

    // general case
    const double sqr_len_v(v.getSqrLength());
    const double sqr_len_w(w.getSqrLength());

    Eigen::Matrix2d mat;
    mat(0,0) = sqr_len_v;
    mat(0,1) = -1.0 * MathLib::scalarProduct(v,w);
    mat(1,1) = sqr_len_w;
    mat(1,0) = mat(0,1);

    Eigen::Vector2d rhs;
    rhs << MathLib::scalarProduct(v, qp), MathLib::scalarProduct(w, pq);

    rhs = mat.partialPivLu().solve(rhs);

    // no theory for the following tolerances, determined by testing
    // lower tolerance: little bit smaller than zero
    const double l(-1.0*std::numeric_limits<float>::epsilon());
    // upper tolerance a little bit greater than one
    const double u(1.0+std::numeric_limits<float>::epsilon());
    if (rhs[0] < l || u < rhs[0] || rhs[1] < l || u < rhs[1]) {
        return false;
    }

    // compute points along line segments with minimal distance
    GeoLib::Point const p0(a[0]+rhs[0]*v[0], a[1]+rhs[0]*v[1], a[2]+rhs[0]*v[2]);
    GeoLib::Point const p1(c[0]+rhs[1]*w[0], c[1]+rhs[1]*w[1], c[2]+rhs[1]*w[2]);

    double const min_dist(sqrt(MathLib::sqrDist(p0, p1)));
    double const min_seg_len(std::min(sqrt(sqr_len_v), sqrt(sqr_len_w)));
    if (min_dist < min_seg_len * 1e-6) {
        s[0] = 0.5 * (p0[0] + p1[0]);
        s[1] = 0.5 * (p0[1] + p1[1]);
        s[2] = 0.5 * (p0[2] + p1[2]);
        return true;
    }

    return false;
}

bool lineSegmentsIntersect(const GeoLib::Polyline* ply,
                           GeoLib::Polyline::SegmentIterator &seg_it0,
                           GeoLib::Polyline::SegmentIterator &seg_it1,
                           GeoLib::Point& intersection_pnt)
{
    std::size_t const n_segs(ply->getNumberOfSegments());
    // Neighbouring segments always intersects at a common vertex. The algorithm
    // checks for intersections of non-neighbouring segments.
    for (seg_it0 = ply->begin(); seg_it0 != ply->end() - 2; ++seg_it0)
    {
        seg_it1 = seg_it0+2;
        std::size_t const seg_num_0 = seg_it0.getSegmentNumber();
        for ( ; seg_it1 != ply->end(); ++seg_it1) {
            // Do not check first and last segment, because they are
            // neighboured.
            if (!(seg_num_0 == 0 && seg_it1.getSegmentNumber() == n_segs - 1)) {
                if (lineSegmentIntersect(*seg_it0, *seg_it1, intersection_pnt)) {
                    return true;
                }
            }
        }
    }
    return false;
}

void rotatePoints(MathLib::DenseMatrix<double> const& rot_mat, std::vector<GeoLib::Point*> &pnts)
{
    rotatePoints(rot_mat, pnts.begin(), pnts.end());
}

MathLib::DenseMatrix<double> rotatePointsToXY(std::vector<GeoLib::Point*>& pnts)
{
    return rotatePointsToXY(pnts.begin(), pnts.end(), pnts.begin(), pnts.end());
}

std::unique_ptr<GeoLib::Point> triangleLineIntersection(
    MathLib::Point3d const& a, MathLib::Point3d const& b,
    MathLib::Point3d const& c, MathLib::Point3d const& p,
    MathLib::Point3d const& q)
{
    const MathLib::Vector3 pq(p, q);
    const MathLib::Vector3 pa(p, a);
    const MathLib::Vector3 pb(p, b);
    const MathLib::Vector3 pc(p, c);

    double u (MathLib::scalarTriple(pq, pc, pb));
    if (u < 0)
    {
        return nullptr;
    }
    double v (MathLib::scalarTriple(pq, pa, pc));
    if (v < 0)
    {
        return nullptr;
    }
    double w (MathLib::scalarTriple(pq, pb, pa));
    if (w < 0)
    {
        return nullptr;
    }

    const double denom (1.0/(u+v+w));
    u*=denom;
    v*=denom;
    w*=denom;
    return std::make_unique<GeoLib::Point>(u * a[0] + v * b[0] + w * c[0],
                                           u * a[1] + v * b[1] + w * c[1],
                                           u * a[2] + v * b[2] + w * c[2]);
}

void computeAndInsertAllIntersectionPoints(GeoLib::PointVec &pnt_vec,
    std::vector<GeoLib::Polyline*> & plys)
{
    auto computeSegmentIntersections = [&pnt_vec](GeoLib::Polyline& poly0,
                                                  GeoLib::Polyline& poly1)
    {
        for (auto seg0_it(poly0.begin()); seg0_it != poly0.end(); ++seg0_it)
        {
            for (auto seg1_it(poly1.begin()); seg1_it != poly1.end(); ++seg1_it)
            {
                GeoLib::Point s(0.0, 0.0, 0.0, pnt_vec.size());
                if (lineSegmentIntersect(*seg0_it, *seg1_it, s))
                {
                    std::size_t const id(
                        pnt_vec.push_back(new GeoLib::Point(s)));
                    poly0.insertPoint(seg0_it.getSegmentNumber() + 1, id);
                    poly1.insertPoint(seg1_it.getSegmentNumber() + 1, id);
                }
            }
        }
    };

    for (auto it0(plys.begin()); it0 != plys.end(); ++it0) {
        auto it1(it0);
        ++it1;
        for (; it1 != plys.end(); ++it1) {
            computeSegmentIntersections(*(*it0), *(*it1));
        }
    }
}

GeoLib::Polygon rotatePolygonToXY(GeoLib::Polygon const& polygon_in,
    MathLib::Vector3 & plane_normal)
{
    // 1 copy all points
    auto* polygon_pnts(new std::vector<GeoLib::Point*>);
    for (std::size_t k(0); k < polygon_in.getNumberOfPoints(); k++)
    {
        polygon_pnts->push_back(new GeoLib::Point(*(polygon_in.getPoint(k))));
    }

    // 2 rotate points
    double d_polygon (0.0);
    GeoLib::getNewellPlane (*polygon_pnts, plane_normal, d_polygon);
    MathLib::DenseMatrix<double> rot_mat(3,3);
    GeoLib::computeRotationMatrixToXY(plane_normal, rot_mat);
    GeoLib::rotatePoints(rot_mat, *polygon_pnts);

    // 3 set z coord to zero
    std::for_each(polygon_pnts->begin(), polygon_pnts->end(),
        [] (GeoLib::Point* p) { (*p)[2] = 0.0; }
    );

    // 4 create new polygon
    GeoLib::Polyline rot_polyline(*polygon_pnts);
    for (std::size_t k(0); k < polygon_in.getNumberOfPoints(); k++)
    {
        rot_polyline.addPoint(k);
    }
    rot_polyline.addPoint(0);
    return GeoLib::Polygon(rot_polyline);
}

std::vector<MathLib::Point3d> lineSegmentIntersect2d(
    GeoLib::LineSegment const& ab, GeoLib::LineSegment const& cd)
{
    GeoLib::Point const& a{ab.getBeginPoint()};
    GeoLib::Point const& b{ab.getEndPoint()};
    GeoLib::Point const& c{cd.getBeginPoint()};
    GeoLib::Point const& d{cd.getEndPoint()};

    double const orient_abc(getOrientation(a, b, c));
    double const orient_abd(getOrientation(a, b, d));

    // check if the segment (cd) lies on the left or on the right of (ab)
    if ((orient_abc > 0 && orient_abd > 0) || (orient_abc < 0 && orient_abd < 0)) {
        return std::vector<MathLib::Point3d>();
    }

    // check: (cd) and (ab) are on the same line
    if (orient_abc == 0.0 && orient_abd == 0.0) {
        double const eps(std::numeric_limits<double>::epsilon());
        if (MathLib::sqrDist2d(a, c) < eps && MathLib::sqrDist2d(b, d) < eps)
        {
            return {{a, b}};
        }
        if (MathLib::sqrDist2d(a, d) < eps && MathLib::sqrDist2d(b, c) < eps)
        {
            return {{a, b}};
        }

        // Since orient_ab and orient_abd vanish, a, b, c, d are on the same
        // line and for this reason it is enough to check the x-component.
        auto isPointOnSegment = [](double q, double p0, double p1)
        {
            double const t((q - p0) / (p1 - p0));
            return 0 <= t && t <= 1;
        };

        // check if c in (ab)
        if (isPointOnSegment(c[0], a[0], b[0])) {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0])) {
                return {{a, c}};
            }
            // check b == c
            if (MathLib::sqrDist2d(b,c) < eps) {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0])) {
                return {{b, c}};
            }
            // check d in (ab)
            if (isPointOnSegment(d[0], a[0], b[0])) {
                return {{c, d}};
            }
            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) is not handled yet. "
                "Aborting.",
                err.str());
        }

        // check if d in (ab)
        if (isPointOnSegment(d[0], a[0], b[0])) {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0])) {
                return {{a, d}};
            }
            // check if b==d
            if (MathLib::sqrDist2d(b, d) < eps) {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0])) {
                return {{b, d}};
            }
            // d in (ab), b not in (cd): check c in (ab)
            if (isPointOnSegment(c[0], a[0], b[0])) {
                return {{c, d}};
            }

            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) "
                "is not handled yet. Aborting.",
                err.str());
        }
        return std::vector<MathLib::Point3d>();
    }

    // precondition: points a, b, c are collinear
    // the function checks if the point c is onto the line segment (a,b)
    auto isCollinearPointOntoLineSegment = [](MathLib::Point3d const& a,
                                              MathLib::Point3d const& b,
                                              MathLib::Point3d const& c) {
        if (b[0] - a[0] != 0)
        {
            double const t = (c[0] - a[0]) / (b[0] - a[0]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[1] - a[1] != 0)
        {
            double const t = (c[1] - a[1]) / (b[1] - a[1]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[2] - a[2] != 0)
        {
            double const t = (c[2] - a[2]) / (b[2] - a[2]);
            return 0.0 <= t && t <= 1.0;
        }
        return false;
    };

    if (orient_abc == 0.0) {
        if (isCollinearPointOntoLineSegment(a, b, c))
        {
            return {{c}};
        }
        return std::vector<MathLib::Point3d>();
    }

    if (orient_abd == 0.0) {
        if (isCollinearPointOntoLineSegment(a, b, d))
        {
            return {{d}};
        }
        return std::vector<MathLib::Point3d>();
    }

    // check if the segment (ab) lies on the left or on the right of (cd)
    double const orient_cda(getOrientation(c, d, a));
    double const orient_cdb(getOrientation(c, d, b));
    if ((orient_cda > 0 && orient_cdb > 0) || (orient_cda < 0 && orient_cdb < 0)) {
        return std::vector<MathLib::Point3d>();
    }

    // at this point it is sure that there is an intersection and the system of
    // linear equations will be invertible
    // solve the two linear equations (b-a, c-d) (t, s)^T = (c-a) simultaneously
    Eigen::Matrix2d mat;
    mat(0,0) = b[0]-a[0];
    mat(0,1) = c[0]-d[0];
    mat(1,0) = b[1]-a[1];
    mat(1,1) = c[1]-d[1];
    Eigen::Vector2d rhs{c[0] - a[0], c[1] - a[1]};

    rhs = mat.partialPivLu().solve(rhs);
    if (0 <= rhs[1] && rhs[1] <= 1.0) {
        return { MathLib::Point3d{std::array<double,3>{{
                c[0]+rhs[1]*(d[0]-c[0]), c[1]+rhs[1]*(d[1]-c[1]),
                c[2]+rhs[1]*(d[2]-c[2])}} } };
    }
    return std::vector<MathLib::Point3d>();  // parameter s not in the valid
                                             // range
}

void sortSegments(
    MathLib::Point3d const& seg_beg_pnt,
    std::vector<GeoLib::LineSegment>& sub_segments)
{
    double const eps(std::numeric_limits<double>::epsilon());

    auto findNextSegment = [&eps](
                               MathLib::Point3d const& seg_beg_pnt,
                               std::vector<GeoLib::LineSegment>& sub_segments,
                               std::vector<GeoLib::LineSegment>::iterator&
                                   sub_seg_it) {
        if (sub_seg_it == sub_segments.end())
        {
            return;
        }
        // find appropriate segment for the given segment begin point
        auto act_beg_seg_it = std::find_if(
            sub_seg_it, sub_segments.end(),
            [&seg_beg_pnt, &eps](GeoLib::LineSegment const& seg)
            {
                return MathLib::sqrDist(seg_beg_pnt, seg.getBeginPoint()) < eps ||
                       MathLib::sqrDist(seg_beg_pnt, seg.getEndPoint()) < eps;
            });
        if (act_beg_seg_it == sub_segments.end())
        {
            return;
        }
        // if necessary correct orientation of segment, i.e. swap beg and end
        if (MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getEndPoint()) <
            MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getBeginPoint()))
        {
            std::swap(act_beg_seg_it->getBeginPoint(),
                      act_beg_seg_it->getEndPoint());
        }
        assert(sub_seg_it != sub_segments.end());
        // exchange segments within the container
        if (sub_seg_it != act_beg_seg_it)
        {
            std::swap(*sub_seg_it, *act_beg_seg_it);
        }
    };

    // find start segment
    auto seg_it = sub_segments.begin();
    findNextSegment(seg_beg_pnt, sub_segments, seg_it);

    while (seg_it != sub_segments.end())
    {
        MathLib::Point3d & new_seg_beg_pnt(seg_it->getEndPoint());
        seg_it++;
        if (seg_it != sub_segments.end())
        {
            findNextSegment(new_seg_beg_pnt, sub_segments, seg_it);
        }
    }
}

} // end namespace GeoLib


#include <stdio.h>
#include <stdlib.h>

#include <math.h>
#include <fftw3.h>
#include <complex>
#include <boost/program_options.hpp>
#include <iostream>
#include <iomanip>
#include "sdlplot.h"
#include "TutorialConfig.h"

#define N 1024

using namespace std::complex_literals;
using namespace std;
using namespace boost::program_options;

void on_input(std::string s)
{
  std::cout << "On input: " << s << '\n';
}

int main(int argc, char *argv[])
{
  fprintf(stdout, "Version %d.%d\n", Tutorial_VERSION_MAJOR, Tutorial_VERSION_MINOR);
  try
  {
    options_description desc{"Options"};
    desc.add_options()("help,h", "Help screen")("fs", value<float>()->default_value(1e6f), "Sample Rate")("input", value<std::string>()->notifier(on_input), "Input Filename");

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

    if (vm.count("help"))
      std::cout << desc << '\n';
    else if (vm.count("fs"))
      std::cout << "fs specified as : " << vm["fs"].as<float>() << '\n';
    else if (vm.count("input"))
      std::cout << "input file: " << vm["input"].as<std::string>() << '\n';
  }
  catch (const error &ex)
  {
    std::cerr << ex.what() << '\n';
  }

  fftw_complex *in, *out;
  fftw_plan p;

  for (int x = 0; x < argc; x++)
  {
    printf("\n[%d] == %s", x, argv[x]);
  }

  in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
  out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
  p = fftw_plan_dft_1d(N, in, out, FFTW_FORWARD, FFTW_ESTIMATE);

//  complex<double> bbb = 1i + 1;
//  in[0][0] = reinterpret_cast<double(&)[2]>(bbb)[0];
//  in[0][1] = reinterpret_cast<double(&)[2]>(bbb)[1];
  srand(1);

  for (int n = 0; n < N; n++)
  {
    in[n][0] = ((float)rand()) / RAND_MAX;
    in[n][1] = ((float)rand()) / RAND_MAX;
  }
  fftw_execute(p);

  sdlplot pp;
  pp.axis.xmax = M_PI;
  pp.axis.xmin = -M_PI;
  pp.axis.ymin = -1.2;
  pp.axis.ymax = 1.2;
  pp.line.connect_the_dots = true;

  for(int n=0;n<100;n++){
    datapoint pt;
    pt.x = ((double)n-50.0)/100.0 * 2*M_PI;
    pt.y = sin(pt.x);
    pp.line.points.push_back(pt);

  }

  pp.makewindow();

  while(1){
    if(pp.run()){
      cout << "\nquit!";
      break;
    }

  }

  fftw_destroy_plan(p);
  fftw_free(in);
  fftw_free(out);

  return 0;
}


#include <state_estimation/filters/ukf_vs.h>
#include <state_estimation/utilities/data_subset_utilities.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

UKFVS::UKFVS(system_models::NonlinearSystemModel* system_model)
    : FilterBase::FilterBase(system_model) {
    initializeSigmaPointParameters();
}

UKFVS::UKFVS(system_models::NonlinearSystemModel* system_model, const Eigen::VectorXd& x,
             const Eigen::MatrixXd& cov, double timestamp)
    : FilterBase::FilterBase(system_model, x, cov, timestamp) {
    initializeSigmaPointParameters();
}

void UKFVS::setSigmaPointParameters(double alpha, double kappa, double beta) {
    uint32_t n = system_model_->activeStateSize();
    num_sigma_pts_ = 2 * n + 1;

    // Compute our lambda value
    lambda_ = pow(alpha, 2) * (n + kappa) - n;

    // Compute our weight vectors
    w_mean_.resize(num_sigma_pts_);
    w_cov_.resize(num_sigma_pts_);

    const double init_w = 0.5 / (n + lambda_);
    w_mean_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);
    w_cov_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);

    w_mean_(0) = lambda_ / (n + lambda_);
    w_cov_(0) = w_mean_(0) + 1 - pow(alpha, 2) + beta;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF sigma point initialization" << std::endl
              << "alpha=" << alpha << std::endl
              << "kappa=" << kappa << std::endl
              << "lambda=" << lambda_ << std::endl
              << "Initialized mean weights to [" << w_mean_.transpose() << "]" << std::endl
              << "Initialized covariance weights to [" << w_cov_.transpose() << "]" << std::endl;
#endif
}

void UKFVS::initializeSigmaPointParameters() {
    setSigmaPointParameters(0.001, 0, 2);
}

void UKFVS::myPredict(const Eigen::VectorXd& u, double dt) {
    // We will only be updating a subset of the state. All the computationally expensive linear
    // algebra operations will happen on the subset of the state for efficiency. Since the models
    // still operate on the full state vector the sigma points vectors will be of the full
    // dimensionality, but the number of sigma points will still be determined by the number of
    // active states. This only adds a slight additional memory cost.

    // Generate the sigma offsets only using the subset of the covariance matrix we are inerested
    // for efficiency
    const Eigen::MatrixXd cov_subset = getSubset(
        filter_state_.covariance, system_model_->activeStates(), system_model_->activeStates());
    const Eigen::MatrixXd sigma_offset_subset =
        ((system_model_->activeStateSize() + lambda_) * cov_subset).llt().matrixL();

    Eigen::MatrixXd sigma_pts(system_model_->stateSize(), num_sigma_pts_);

    // Run all the sigma points through the model
    system_model_->update(filter_state_.x, u, dt);
    sigma_pts.col(0) = system_model_->g();

    Eigen::VectorXd offset = Eigen::VectorXd::Zero(system_model_->stateSize());
    for (uint32_t i = 0; i < system_model_->activeStateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->activeStateSize();
        convertSubsetToFull(sigma_offset_subset.col(i), &offset, system_model_->activeStates());

        const Eigen::VectorXd x_high = system_model_->addVectors(filter_state_.x, offset);
        system_model_->update(x_high, u, dt);
        sigma_pts.col(i_high) = system_model_->g();

        const Eigen::VectorXd x_low = system_model_->subtractVectors(filter_state_.x, offset);
        system_model_->update(x_low, u, dt);
        sigma_pts.col(i_low) = system_model_->g();
    }

    // Compute the weighted mean
    filter_state_.x = system_model_->weightedSum(w_mean_, sigma_pts);

    // Initialze the covariance with the process and control noise
    const Eigen::MatrixXd Rc_subset =
        getSubset(system_model_->Rc(), system_model_->activeControls());
    const Eigen::MatrixXd P_subset =
        getSubset(system_model_->P(), system_model_->activeStates(), {});
    const Eigen::MatrixXd V_subset = getSubset(system_model_->V(), system_model_->activeStates(),
                                               system_model_->activeControls());

    Eigen::MatrixXd cov_prime_subset = P_subset * system_model_->Rp() * P_subset.transpose() +
                                       V_subset * Rc_subset * V_subset.transpose();

    // Add the weighted sample covariance
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx_full =
            system_model_->subtractVectors(sigma_pts.col(i), filter_state_.x);
        const Eigen::VectorXd dx_subset = getSubset(dx_full, system_model_->activeStates());

        cov_prime_subset += w_cov_(i) * dx_subset * dx_subset.transpose();
    }

    // Update the full covariance matrix with the subset we calculated
    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    Eigen::MatrixXd sigma_offset =
        Eigen::MatrixXd::Zero(system_model_->stateSize(), system_model_->stateSize());
    convertSubsetToFull(sigma_offset_subset, &sigma_offset, system_model_->activeStates());

    std::cout << "UKF predicition update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "Sigma points=" << std::endl
              << printMatrix(sigma_pts) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void UKFVS::myCorrect(const Eigen::VectorXd& z,
                      measurement_models::NonlinearMeasurementModel* model, double dt) {
    // We will only be updating a subset of the state. All the computationally expensive linear
    // algebra operations will happen on the subset of the state for efficiency. Since the models
    // still operate on the full state vector the sigma points vectors will be of the full
    // dimensionality, but the number of sigma points will still be determined by the number of
    // active states. This only adds a slight additional memory cost.

    // Generate the sigma offsets only using the subset of the covariance matrix we are inerested
    // for efficiency
    const Eigen::MatrixXd cov_subset =
        getSubset(filter_state_.covariance, system_model_->activeStates());
    const Eigen::MatrixXd sigma_offset_subset =
        ((system_model_->activeStateSize() + lambda_) * cov_subset).llt().matrixL();

    Eigen::MatrixXd state_sigma_pts(system_model_->stateSize(), num_sigma_pts_);
    Eigen::MatrixXd meas_sigma_pts(model->measurementSize(), num_sigma_pts_);

    // Run all the sigma points through the model
    state_sigma_pts.col(0) = filter_state_.x;
    model->update(filter_state_.x, dt);
    meas_sigma_pts.col(0) = model->h();

    Eigen::VectorXd offset = Eigen::VectorXd::Zero(system_model_->stateSize());
    for (uint32_t i = 0; i < system_model_->activeStateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->activeStateSize();
        convertSubsetToFull(sigma_offset_subset.col(i), &offset, system_model_->activeStates());

        state_sigma_pts.col(i_high) = system_model_->addVectors(filter_state_.x, offset);
        model->update(state_sigma_pts.col(i_high), dt);
        meas_sigma_pts.col(i_high) = model->h();

        state_sigma_pts.col(i_low) = system_model_->subtractVectors(filter_state_.x, offset);
        model->update(state_sigma_pts.col(i_low), dt);
        meas_sigma_pts.col(i_low) = model->h();
    }

    // Compute the weighted mean for the predicted measurement
    Eigen::VectorXd z_pred = model->weightedSum(w_mean_, meas_sigma_pts);

    // Compute the gain (only using the subset of the states)
    Eigen::MatrixXd S = getSubset(model->covariance(), model->activeMeasurements());
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dz_full = model->subtractVectors(meas_sigma_pts.col(i), z_pred);
        const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
        S += w_cov_(i) * dz_subset * dz_subset.transpose();
    }

    Eigen::MatrixXd cross_covariance =
        Eigen::MatrixXd::Zero(system_model_->activeStateSize(), model->activeMeasurementSize());
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx_full =
            system_model_->subtractVectors(state_sigma_pts.col(i), state_sigma_pts.col(0));
        const Eigen::VectorXd dx_subset = getSubset(dx_full, system_model_->activeStates());

        const Eigen::VectorXd dz_full = model->subtractVectors(meas_sigma_pts.col(i), z_pred);
        const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());

        cross_covariance += w_cov_(i) * dx_subset * dz_subset.transpose();
    }

    const Eigen::MatrixXd K = cross_covariance * S.inverse();

    // Perform the mean update
    const Eigen::VectorXd dz_full = model->subtractVectors(z, z_pred);
    const Eigen::VectorXd dz_subset = getSubset(dz_full, model->activeMeasurements());
    const Eigen::VectorXd dx_subset = K * dz_subset;
    const Eigen::VectorXd dx_full = convertSubsetToFullZeroed(
        dx_subset, system_model_->activeStates(), system_model_->stateSize());
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx_full);

    // Perform the covariance update
    const Eigen::MatrixXd cov_prime_subset = cov_subset - K * S * K.transpose();
    convertSubsetToFull(cov_prime_subset, &filter_state_.covariance, system_model_->activeStates());

#ifdef DEBUG_STATE_ESTIMATION
    const Eigen::MatrixXd sigma_offset = convertSubsetToFullZeroed(
        sigma_offset_subset, system_model_->activeStates(), system_model_->stateSize());

    const Eigen::MatrixXd S_full =
        convertSubsetToFullZeroed(S, model->activeMeasurements(), model->measurementSize());

    const Eigen::MatrixXd cross_full = convertSubsetToFullZeroed(
        cross_covariance, system_model_->activeStates(), model->activeMeasurements(),
        system_model_->stateSize(), model->measurementSize());

    const Eigen::MatrixXd K_full =
        convertSubsetToFullZeroed(K, system_model_->activeStates(), model->activeMeasurements(),
                                  system_model_->stateSize(), model->measurementSize());

    std::cout << "UKF measurement update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "State sigma points=" << std::endl
              << printMatrix(state_sigma_pts) << std::endl
              << "Measurement sigma points=" << std::endl
              << printMatrix(meas_sigma_pts) << std::endl
              << "z_pred=" << printMatrix(z_pred) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "S=" << std::endl
              << printMatrix(S_full) << std::endl
              << "Cross Covariance=" << std::endl
              << printMatrix(cross_full) << std::endl
              << "K=" << std::endl
              << printMatrix(K_full) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation