// An example showing TEASER++ registration with the Stanford bunny model
#include <chrono>
#include <iostream>
#include <random>

#include <Eigen/Core>

#include <teaser/log.h>
#include <teaser/ply_io.h>
#include <teaser/registration.h>

// Macro constants for generating noise and outliers
#define NOISE_BOUND 0.05
#define N_OUTLIERS 1700
#define OUTLIER_TRANSLATION_LB 5
#define OUTLIER_TRANSLATION_UB 10

inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) {
  return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0)));
}

void addNoiseAndOutliers(Eigen::Matrix<double, 3, Eigen::Dynamic>& tgt) {
  //int N_OUTLIERS = tgt.size() * 90 / 100;
  // Add uniform noise
  Eigen::Matrix<double, 3, Eigen::Dynamic> noise =
      Eigen::Matrix<double, 3, Eigen::Dynamic>::Random(3, tgt.cols()) * NOISE_BOUND;
  NOISE_BOUND / 2;
  tgt = tgt + noise;

  // Add outliers
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); // pos of outliers
  std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB,
                                       OUTLIER_TRANSLATION_UB); // random translation
  std::vector<bool> expected_outlier_mask(tgt.cols(), false);
  for (int i = 0; i < N_OUTLIERS; ++i) {
    int c_outlier_idx = dis2(gen);
    assert(c_outlier_idx < expected_outlier_mask.size());
    expected_outlier_mask[c_outlier_idx] = true;
    tgt.col(c_outlier_idx).array() += dis3(gen); // random translation
  }
}

int main() {
  // Load the .ply file
  teaser::PLYReader reader;
  teaser::PointCloud src_cloud;
  auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud);
  int N = src_cloud.size();

  // Convert the point cloud to Eigen
  Eigen::Matrix<double, 3, Eigen::Dynamic> src(3, N);
  for (size_t i = 0; i < N; ++i) {
    src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z;
  }

  // Homogeneous coordinates
  Eigen::Matrix<double, 4, Eigen::Dynamic> src_h;
  src_h.resize(4, src.cols());
  src_h.topRows(3) = src;
  src_h.bottomRows(1) = Eigen::Matrix<double, 1, Eigen::Dynamic>::Ones(N);

  // Apply an arbitrary SE(3) transformation
  Eigen::Matrix4d T;
  // clang-format off
  T << 9.96926560e-01,  6.68735757e-02, -4.06664421e-02, -1.15576939e-01,
      -6.61289946e-02, 9.97617877e-01,  1.94008687e-02, -3.87705398e-02,
      4.18675510e-02, -1.66517807e-02,  9.98977765e-01, 1.14874890e-01,
      0,              0,                0,              1;
  // clang-format on

  // Apply transformation
  Eigen::Matrix<double, 4, Eigen::Dynamic> tgt_h = T * src_h;
  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt = tgt_h.topRows(3);

  // Add some noise & outliers
  addNoiseAndOutliers(tgt);

  // Run TEASER++ registration
  // Prepare solver parameters
  teaser::RobustRegistrationSolver::Params params;
  params.noise_bound = NOISE_BOUND;
  params.cbar2 = 1;
  params.estimate_scaling = false;
  params.rotation_max_iterations = 100;
  params.rotation_gnc_factor = 1.4;
  params.rotation_estimation_algorithm =
      teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
  params.rotation_cost_threshold = 0.005;

  // Solve with TEASER++
  teaser::RobustRegistrationSolver solver(params);
  std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
  solver.solve(src, tgt);
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

  auto solution = solver.getSolution();

  // Compare results
  std::cout << "=====================================" << std::endl;
  std::cout << "          TEASER++ Results           " << std::endl;
  std::cout << "=====================================" << std::endl;
  std::cout << "Expected rotation: " << std::endl;
  std::cout << T.topLeftCorner(3, 3) << std::endl;
  std::cout << "Estimated rotation: " << std::endl;
  std::cout << solution.rotation << std::endl;
  std::cout << "Error (deg): " << getAngularError(T.topLeftCorner(3, 3), solution.rotation)
            << std::endl;
  std::cout << std::endl;
  std::cout << "Expected translation: " << std::endl;
  std::cout << T.topRightCorner(3, 1) << std::endl;
  std::cout << "Estimated translation: " << std::endl;
  std::cout << solution.translation << std::endl;
  std::cout << "Error (m): " << (T.topRightCorner(3, 1) - solution.translation).norm() << std::endl;
  std::cout << std::endl;
  std::cout << "Number of correspondences: " << N << std::endl;
  std::cout << "Number of outliers: " << N_OUTLIERS << std::endl;
  std::cout << "Time taken (s): "
            << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() /
                   1000000.0
            << std::endl;
}


/*
 * resizing_lattice.cpp
 *
 * Demonstrates the usage of resizing of the state type during integration.
 * Examplary system is a strongly nonlinear, disordered Hamiltonian lattice
 * where the spreading of energy is invastigated
 *
 * Copyright 2009-2012 Karsten Ahnert and Mario Mulansky.
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <utility>

#include <boost/numeric/odeint.hpp>

#include <boost/ref.hpp>
#include <boost/random.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ resizing_lattice_system_class
typedef vector< double > coord_type;
typedef pair< coord_type , coord_type > state_type;

struct compacton_lattice
{
    const int m_max_N;
    const double m_beta;
    int m_pot_start_index;
    vector< double > m_pot;

    compacton_lattice( int max_N , double beta , int pot_start_index )
        : m_max_N( max_N ) , m_beta( beta ) , m_pot_start_index( pot_start_index ) , m_pot( max_N )
    {
        srand( time( NULL ) );
        // fill random potential with iid values from [0,1]
        boost::mt19937 rng;
        boost::uniform_real<> unif( 0.0 , 1.0 );
        boost::variate_generator< boost::mt19937&, boost::uniform_real<> > gen( rng , unif );
        generate( m_pot.begin() , m_pot.end() , gen );
    }

    void operator()( const coord_type &q , coord_type &dpdt )
    {
        // calculate dpdt = -dH/dq of this hamiltonian system
        // dp_i/dt = - V_i * q_i^3 - beta*(q_i - q_{i-1})^3 + beta*(q_{i+1} - q_i)^3
        const int N = q.size();
        double diff = q[0] - q[N-1];
        for( int i=0 ; i<N ; ++i )
        {
            dpdt[i] = - m_pot[m_pot_start_index+i] * q[i]*q[i]*q[i] -
                    m_beta * diff*diff*diff;
            diff = q[(i+1) % N] - q[i];
            dpdt[i] += m_beta * diff*diff*diff;
        }
    }

    void energy_distribution( const coord_type &q , const coord_type &p , coord_type &energies )
    {
        // computes the energy per lattice site normalized by total energy
        const size_t N = q.size();
        double en = 0.0;
        for( size_t i=0 ; i<N ; i++ )
        {
            const double diff = q[(i+1) % N] - q[i];
            energies[i] = p[i]*p[i]/2.0
                + m_pot[m_pot_start_index+i]*q[i]*q[i]*q[i]*q[i]/4.0
                + m_beta/4.0 * diff*diff*diff*diff;
            en += energies[i];
        }
        en = 1.0/en;
        for( size_t i=0 ; i<N ; i++ )
        {
            energies[i] *= en;
        }
    }

    double energy( const coord_type &q , const coord_type &p )
    {
        // calculates the total energy of the excitation
        const size_t N = q.size();
        double en = 0.0;
        for( size_t i=0 ; i<N ; i++ )
        {
            const double diff = q[(i+1) % N] - q[i];
            en += p[i]*p[i]/2.0
                + m_pot[m_pot_start_index+i]*q[i]*q[i]*q[i]*q[i] / 4.0
                + m_beta/4.0 * diff*diff*diff*diff;
        }
        return en;
    }

    void change_pot_start( const int delta )
    {
        m_pot_start_index += delta;
    }
};
//]

//[ resizing_lattice_resize_function
void do_resize( coord_type &q , coord_type &p , coord_type &distr , const int N )
{
    q.resize( N );
    p.resize( N );
    distr.resize( N );
}
//]

const int max_N = 1024;
const double beta = 1.0;

int main()
{
    //[ resizing_lattice_initialize
    //start with 60 sites
    const int N_start = 60;
    coord_type q( N_start , 0.0 );
    q.reserve( max_N );
    coord_type p( N_start , 0.0 );
    p.reserve( max_N );
    // start with uniform momentum distribution over 20 sites
    fill( p.begin()+20 , p.end()-20 , 1.0/sqrt(20.0) );

    coord_type distr( N_start , 0.0 );
    distr.reserve( max_N );

    // create the system
    compacton_lattice lattice( max_N , beta , (max_N-N_start)/2 );

    //create the stepper, note that we use an always_resizer because state size might change during steps
    typedef symplectic_rkn_sb3a_mclachlan< coord_type , coord_type , double , coord_type , coord_type , double ,
            range_algebra , default_operations , always_resizer > hamiltonian_stepper;
    hamiltonian_stepper stepper;
    hamiltonian_stepper::state_type state = make_pair( q , p );
    //]

    //[ resizing_lattice_steps_loop
    double t = 0.0;
    const double dt = 0.1;
    const int steps = 10000;
    for( int step = 0 ; step < steps ; ++step )
    {
        stepper.do_step( boost::ref(lattice) , state , t , dt );
        lattice.energy_distribution( state.first , state.second , distr );
        if( distr[10] > 1E-150 )
        {
            do_resize( state.first , state.second , distr , state.first.size()+20 );
            rotate( state.first.begin() , state.first.end()-20 , state.first.end() );
            rotate( state.second.begin() , state.second.end()-20 , state.second.end() );
            lattice.change_pot_start( -20 );
            cout << t << ": resized left to " << distr.size() << ", energy = " << lattice.energy( state.first , state.second ) << endl;
        }
        if( distr[distr.size()-10] > 1E-150 )
        {
            do_resize( state.first , state.second , distr , state.first.size()+20 );
            cout << t << ": resized right to " << distr.size() << ", energy = " << lattice.energy( state.first , state.second ) << endl;
        }
        t += dt;
    }
    //]

    cout << "final lattice size: " << distr.size() << ", final energy: " << lattice.energy( state.first , state.second ) << endl;
}


#include <iostream>
#include <fstream>
#include <string>
#include <tuple>
#include <vector>
#include <queue>
#include <boost/algorithm/string.hpp>
#include <math.h>
#include <chrono>
#include <stdlib.h>

using namespace std;

typedef std::chrono::high_resolution_clock Clock;
typedef std::vector<vector<int>> AdjList;

double distance(const tuple<int, int> &p1, const tuple<int, int> &p2) {
  return sqrt( (std::get<0>(p1) - std::get<0>(p2)) * (std::get<0>(p1) - std::get<0>(p2)) + (std::get<1>(p1) - std::get<1>(p2)) * (std::get<1>(p1) - std::get<1>(p2)) );
}

std::vector<std::vector<double>> createAdjMatrix (const std::vector<std::tuple<int, int>> &nodes){
  std::vector<std::vector<double>> adjMatrix(nodes.size(), vector<double>(nodes.size()));
  for (int i = 0; i < nodes.size(); i++) {
    for (int j = 0; j < nodes.size(); j++){
      adjMatrix[i][j] = distance(nodes[i], nodes[j]);
    }
  }
  return adjMatrix;
}

double pathWeightVsReverse(std::vector<int> &path, int i, int j, const std::vector<std::vector<double>> &adjMatrix){
  if (j-1 < 0)
    return ( adjMatrix[path[i]][path[(i+1)%path.size()]] + adjMatrix[path[path.size()-1]][path[j]] ) - ( adjMatrix[path[i]][path[path.size()-1]] + adjMatrix[path[(i+1)%path.size()]][path[j]] );
  else
    return ( adjMatrix[path[i]][path[(i+1)%path.size()]] + adjMatrix[path[j-1]][path[j]] ) - ( adjMatrix[path[i]][path[j-1]] + adjMatrix[path[(i+1)%path.size()]][path[j]] );
}

void printTour(std::vector<int> &tour, const std::vector<tuple<int,int>> &nodes, std::string name){
  //cout << name << "  TOUR: ";
  for ( const auto &i : tour) {
     cout << "(" << get<0>(nodes[i]) << ", " << get<1>(nodes[i]) << ")" << endl;
  }
}

void printTour(std::vector<int> &tour){
  cout << "TOUR: ";
  for ( const auto &i : tour) {
    cout << i << " ";
  }
  cout << endl;
}

void reverseSubtour (std::vector<int> &tour, int &i, int &j) {
  if (j <= i) {
    /**/
    vector <int> subtour = vector<int>(tour.begin() + i + 1, tour.end());
    subtour.insert(subtour.end(),  tour.begin(), tour.begin() + j);

    int index = i+1;
    for (std::vector<int>::reverse_iterator it = subtour.rbegin(); it != subtour.rend(); it++) {
      tour[index%tour.size()] = *it;
      index += 1;
    }

    //cout << "REVERSING SUBOTUR: " << i << ", " << j << endl;
    /*
    int tourSize = tour.size();
    int a = i + 1;
    int b = (j-1) < 0 ? tourSize - 1 : j -1;
    int tmp;
    for (int c = 0; c < (tourSize - i + j - 2)/2; c++) {
        //printTour(tour);
        //cout << tourSize - i + j - 1 - c << "\tswapping a: " << a << ", b: " << b << endl;
        //cout << "\tswapping tour[a]: " << tour[a] << ", tour[b]: " << tour[b] << endl;
        tmp = tour[a];
        tour[a] = tour[b];
        tour[b] = tmp;
        a = (a + 1) % tourSize;
        b = (b - 1) < 0 ? tourSize - 1 : b - 1;
        //cout <<"\t\t" << a << ", " << b << endl;
    }
   */
  } else {
    std::reverse(tour.begin() + i + 1, tour.begin() + j);
  }
}

double tourWeight(std::vector<int> tour, std::vector<std::vector<double>> adjMatrix) {
  double tourWeight = 0;
  for (int i = 0; i < tour.size(); i++)
    tourWeight += adjMatrix[tour[i]][tour[(i+1)%tour.size()]];
  return tourWeight;
}

std::vector<int> reverseTours (std::vector<int> &tour, const std::vector<std::tuple<int, int>> &nodes, const std::vector<std::vector<double>> &adjMatrix){
  int counter = 0;
  int tourSize = tour.size();
  int tourSizeSq = tourSize*tourSize;
  int subtourSize;

  while (counter < tourSizeSq) {
    for (int i = 0; i < tourSize; i++) {
      for (int j = 0; j < tourSize; j++){

        subtourSize = ((j <= i) ? ((tourSize - i) + j) : (j - i));

        if (subtourSize > 3) {
           if (pathWeightVsReverse(tour, i, j, adjMatrix) > 0) {
             reverseSubtour(tour, i, j);
             counter = 0;
           }
        }
        counter += 1;
        if (counter > tourSizeSq) { goto label2; }
      }
    }
  }
label2:

  return tour;
}

std::vector<std::tuple<int,int>> readPipedInput() {
  std::string line;
  std::vector<std::string> strs;
  std::vector<std::tuple<int, int>> nodes;
  int i = 0; int j = 0;
  std::string black_dot = "0";
  while (std::getline(std::cin, line))  {
    boost::split(strs, line, boost::is_any_of(" \n"));
    for (std::vector<std::string>::const_iterator it = strs.begin(); it != strs.end(); ++it) {
        if (*it == black_dot){nodes.push_back(std::make_tuple(i, j));}
        j += 1;
     }
    i += 1;
    j = 0;
  }
  return nodes;
}
std::vector<std::tuple<int, int>> readInput(string const &str1) {
  string line;
  std::vector<std::string> strs;
  std::vector<std::tuple<int, int>> nodes;
  ifstream myfile (str1);
  int i = 0; int j = 0;
  std::basic_string<char> black_dot = "0";
  if (myfile.is_open())
  {
    while ( getline (myfile,line) )
    {
      boost::split(strs, line, boost::is_any_of(" \n"));
      for (std::vector<std::string>::const_iterator it = strs.begin(); it != strs.end(); ++it) {
          if (*it == black_dot){nodes.push_back(std::make_tuple(i, j));}
          j += 1;
       }
      i += 1;
      j = 0;
    }
    myfile.close();
  }
  else cout << "Unable to open file";
  return nodes;
}

struct compareNodeWeight {
  bool operator()(const std::tuple<double, int, int> &lhs, std::tuple<double,int,int> &rhs) {
    return std::get<0>(lhs) >  std::get<0>(rhs);
  }
};

int searchHeap(std::vector<std::tuple<double,int,int>> &heap, int vertex) {
  for (int i = 0; i < heap.size(); i++) {
    if (std::get<1>(heap[i]) == vertex) {
      return i;
    }
  }
  return -1;
}

void siftUp(std::vector<std::tuple<double,int,int>> &heap, int index) {
  int parent = (index - 1) / 2;
  double indexValue = std::get<0>(heap[parent]);
  double parentValue = std::get<0>(heap[index]);
  std::tuple<double,int,int> tmp;

  while (index != 0 && std::get<0>(heap[(index-1)/2]) > std::get<0>(heap[index]) ) {
    parent = (index - 1) / 2;
    indexValue = std::get<0>(heap[index]);
    parentValue = std::get<0>(heap[parent]);
    tmp = heap[index];
    heap[index] = heap[parent];
    heap[parent] = tmp;
    index = parent;
  }
}

std::vector<std::tuple<int,int>> prims (std::vector<std::vector<double>> adjMatrix) {
  std::vector<int> knownNodes(adjMatrix.size(), 0); // knownNodes[i] = 0 iff i is not known, else 1

  std::vector<std::tuple<double,int,int>> heap;
  for (int i = 1; i < adjMatrix.size(); i++ ){
    heap.push_back(std::make_tuple(INT_MAX, i, -1));
  }

  heap.push_back(std::make_tuple(0, 0, -1));
  std::make_heap(heap.begin(), heap.end(), compareNodeWeight());

  std::tuple<double, int, int> minVal;
  double weight; int u; int u_prev;
  std::vector<std::tuple<int,int>> edges;
  int index;

  while (heap.size() > 0) {
    std::pop_heap(heap.begin(), heap.end(), compareNodeWeight());
    minVal = heap.back();
    heap.pop_back();
    weight = std::get<0>(minVal);
    u = std::get<1>(minVal);
    u_prev = std::get<2>(minVal);

    if (knownNodes[u] == 0) {
      knownNodes[u] = 1;
      if (u_prev != -1) {
        edges.push_back(std::make_tuple(u, u_prev));
      }
    }
    for (int v = 0; v < adjMatrix.size(); v++ ) {
      if (knownNodes[v] == 0) {
        index = searchHeap(heap, v);
        if (index != -1) {
          if (weight + adjMatrix[u][v] < std::get<0>(heap[index])) {
            heap[index] = std::make_tuple(weight + adjMatrix[u][v], v, u);
            siftUp(heap, index);
          }
        }
      }
    }
  }
  return edges;
}

std::vector<vector<int>> constructAdjList(std::vector<std::tuple<int,int>> &MST, const std::vector<std::tuple<int,int>> &nodes) {
  std::vector<std::vector<int>> adjList(nodes.size(), std::vector<int>());

  for (int i = 0; i < MST.size(); i++) {
    std::tuple<int,int> edge = MST[i];
    adjList[std::get<0>(edge)].push_back(std::get<1>(edge));
    adjList[std::get<1>(edge)].push_back(std::get<0>(edge));
  }
  return adjList;
}

std::vector<int> DFSUtil(int v, bool visited[], const std::vector<std::vector<int>> &adjList, std::vector<int> &tour) {
    visited[v] = true;
    tour.push_back(v);

    for (std::vector<int>::const_iterator i = adjList[v].begin(); i != adjList[v].end(); ++i)
        if (!visited[*i])
            DFSUtil(*i, visited, adjList, tour);
    return tour;
}

std::vector<int> DFS(int v, const std::vector<std::vector<int>> adjList) {
    bool *visited = new bool[adjList.size()];
    for (int i = 0; i < adjList.size(); i++)
        visited[i] = false;
    std::vector<int> dfsTour;
    std::vector<int> tour = DFSUtil(v, visited, adjList, dfsTour);
    return tour;
}

std::vector<int> nearestNeighbor(std::vector<int> &tour, const std::vector<std::vector<double>> &adjMatrix) {
  int visitedVertices[adjMatrix.size()] = {0};
  int visitedCount = 0;

  int currentVertex = 0;
  int nextVertex;
  double minEdgeWeight;

  std::vector<int> nnTour;

  while(visitedCount < adjMatrix.size()) {
    nnTour.push_back(currentVertex);
    minEdgeWeight = INT_MAX;
    for (int v = 0; v < adjMatrix.size(); v++){
      if (!visitedVertices[v] && currentVertex != v && adjMatrix[currentVertex][v] < minEdgeWeight) {
        minEdgeWeight = adjMatrix[currentVertex][v];
        nextVertex = v;
      }
    }
    visitedVertices[currentVertex] = 1;
    currentVertex = nextVertex;
    visitedCount+=1;
  }
  return nnTour;

}

int main (int argc, char *argv[]) {
  /*
  std::string input_file = argv[1];
  cout << "INPUT FILE: " << input_file << endl;
  const std::basic_string<char> T = input_file;
  */
  const std::vector<std::tuple<int, int>> nodes = readPipedInput();

  std::vector<int> tour;
  for (int i=0; i<nodes.size(); ++i) tour.push_back(i);
  //printTour(tour, nodes, "ORIGINAL");

  //cout << "Number of cities: " << tour.size() << endl;
  auto t1 = Clock::now();
  const std::vector<std::vector<double>> adjMatrix = createAdjMatrix(nodes);
  auto t2 = Clock::now();
  /*std::cout << "Create AdjMatrix Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */
//  tour = nearestNeighbor(tour, adjMatrix);
//  printTour(tour, nodes, "NEAREST NEIGHBOR");

  t1 = Clock::now();
  std::vector<tuple<int,int>> MST = prims(adjMatrix);
  t2 = Clock::now();
  /*std::cout << "PRIMS Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */

  t1 = Clock::now();
  const std::vector<vector<int>> adjList = constructAdjList(MST, nodes);
  t2 = Clock::now();
  /*std::cout << "Construct AdjList  Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */
  t1 = Clock::now();
  tour = DFS(0, adjList);
  t2 = Clock::now();
  //printTour(tour, nodes, "DFS");
  /*std::cout << "DFS  Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " seconds" << std::endl;
  */

  t1 = Clock::now();
  std::vector<int> reversedTour = reverseTours(tour, nodes, adjMatrix);
  t2 = Clock::now();
  printTour(tour, nodes, "REVERSED");
  /*cout << "Number of cities: " << tour.size() << endl;
  std::cout << "Reverse Tour Delta t2-t1: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count()
            << " milliseconds" << std::endl;
  */
  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/optimal_bounding_box.h>
#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/IO/read_off_points.h>

#include <fstream>
#include <iostream>
#include <utility>
#include <limits>

#ifdef BUILD_PY

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/list.hpp>

#endif

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point;
typedef Kernel::Aff_transformation_3                        Aff_transformation;
typedef CGAL::Surface_mesh<Point>                           Surface_mesh;

/**
 * returns the transformation matrix and the
 */
std::pair<std::array<double, 16>, std::array<double,3>> createOptimalBoundingBox(std::string pathToSTL)
{
  std::ifstream stream(pathToSTL);
  std::list<Point> p;
  if (!stream || !CGAL::read_off_points(stream, std::back_inserter(p)))
  {
      std::cerr << "Error: cannot read file data" << std::endl;
      throw std::exception();
  }
  // compute rotated optimal bounding box
  std::array<Point, 8> obb_points;
  CGAL::oriented_bounding_box(p, obb_points, CGAL::parameters::use_convex_hull(true));

  /* untested */
  Aff_transformation tf;
  CGAL::oriented_bounding_box(p, tf, CGAL::parameters::use_convex_hull(true));

  std::array<double, 6> bb_min_max;
  for(int i = 0; i < 3; i++)
  {
    bb_min_max[i*2+0] = DBL_MIN;
    bb_min_max[i*2+1] = DBL_MAX;

  }
  // rotate points such that the bounding is axis aligned
  // and get the minimum and maximum values for each axis
  std::array<Point, 8> obb_points_rotated;
  for(int i = 0; i < obb_points.size(); i++)
  {
    obb_points_rotated[i] = tf.transform(obb_points[i]);
    bb_min_max[0] = std::max(bb_min_max[0], obb_points_rotated[i].x());
    bb_min_max[1] = std::min(bb_min_max[1], obb_points_rotated[i].x());
    bb_min_max[2] = std::max(bb_min_max[2], obb_points_rotated[i].y());
    bb_min_max[3] = std::min(bb_min_max[3], obb_points_rotated[i].y());
    bb_min_max[4] = std::max(bb_min_max[4], obb_points_rotated[i].z());
    bb_min_max[5] = std::min(bb_min_max[5], obb_points_rotated[i].z());
  }

  std::array<double,3> bb_size;
  bb_size[0] =  bb_min_max[0] - bb_min_max[1];
  bb_size[1] =  bb_min_max[2] - bb_min_max[3];
  bb_size[2] =  bb_min_max[4] - bb_min_max[5];
  //std::cout << "size xyz: " << bb_size[0] <<  " " << bb_size[1] << " " << bb_size[1] << std::endl;

  double offset_x = bb_size[0]/2 - bb_min_max[0];
  double offset_y = bb_size[1]/2 - bb_min_max[2];
  double offset_z = bb_size[2]/2 - bb_min_max[4];
  //std::cout << "x_off: " << offset_x << " y_off: "  << offset_y << " z_off: " << offset_z << " " << std::endl;

  // apply offset to each point of the bounding box for when it is written to file
  Kernel::Vector_3 off(offset_x, offset_y, offset_z);
  for(int i = 0; i<8; i++)
  {
    obb_points_rotated[i] += off;
  }

  // Create mesh, both rotated and not rotated and write each to a file
  Surface_mesh obb_sm;
  CGAL::make_hexahedron(obb_points[0], obb_points[1], obb_points[2], obb_points[3],
                        obb_points[4], obb_points[5], obb_points[6], obb_points[7], obb_sm);
  std::ofstream("box.off") << obb_sm;
  Surface_mesh obb_sm_rotated;
  CGAL::make_hexahedron(obb_points_rotated[0], obb_points_rotated[1], obb_points_rotated[2], obb_points_rotated[3],
                        obb_points_rotated[4], obb_points_rotated[5], obb_points_rotated[6], obb_points_rotated[7], obb_sm_rotated);
  std::ofstream("box_rotated.off") << obb_sm_rotated;

  // put the values from the affine transformation matrix into a standard cpp array
  std::array<double,16> tf_array;
  for(int i = 0; i < 4; i++)
  {
    for(int j = 0; j < 4; j++)
    {
      tf_array[i*4+j] = tf.m(i,j);
    }
  }
  // translation of the bounding box is not included in the affine transformation matrix
  // therefore we add it here
  Kernel::Vector_3 off_rotated = tf.inverse().transform(off);
  tf_array[3] = -off_rotated.x();
  tf_array[7] = -off_rotated.y();
  tf_array[11] = -off_rotated.z();

  return std::make_pair(tf_array, bb_size);
}

#ifdef BUILD_PY
boost::python::list optimalBoundingBoxWrapper(std::string filename)
{
  std::pair<std::array<double, 16>, std::array<double,3>> bb_tf_and_size = createOptimalBoundingBox(filename);
  boost::python::list return_value;

  boost::python::list py_tf;
  for (double d : bb_tf_and_size.first)
  {
    py_tf.append(d);
  }
  boost::python::list py_bb_size;
  for (double limit : bb_tf_and_size.second)
  {
    py_bb_size.append(limit);
  }
  return_value.append(py_tf);
  return_value.append(py_bb_size);

  return return_value;
}
BOOST_PYTHON_MODULE(py_optimal_bounding_box)
{
    using namespace boost::python;

    def("create_optimal_bounding_box", &optimalBoundingBoxWrapper);
}
#endif

// main function if it is called by itself, takes filename as cli input
// prints resulting bounding box and saves it as .off file
int main(int argc, char** argv)
{
  if(argc != 2)
  {
      std::cerr << "Usage: " << argv[0] << " <input file.off>" << std::endl;
      return EXIT_FAILURE;
  }
  auto a = createOptimalBoundingBox(argv[1]);
  // TODO do something with result
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

void linalg_invert( ub::matrix<double> &A, ub::matrix<double> &V){
        // matrix inversion using gsl

        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();
	// signum s (for LU decomposition)
	int s;
        //make copy of A as A is destroyed by GSL
        ub::matrix<double> work=A;
        V.resize(N, N, false);

	// Define all the used matrices
        gsl_matrix_view A_view = gsl_matrix_view_array(&work(0,0), N, N);
        gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_permutation * perm = gsl_permutation_alloc (N);

	// Make LU decomposition of matrix A_view
	gsl_linalg_LU_decomp (&A_view.matrix, perm, &s);

	// Invert the matrix A_view
	(void)gsl_linalg_LU_invert (&A_view.matrix, perm, &V_view.matrix);

        gsl_set_error_handler(handler);

	// return (status != 0);
}

}}


#include <iostream>
#include <utility>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace std;
using namespace boost;
typedef adjacency_list<listS, vecS, directedS, no_property, property<edge_weight_t, int>> Graph;
typedef Graph::vertex_descriptor Vertex;
typedef pair<int, int> Edge;

int main(int argc, char* argv[])
{
  enum { A, B, C, D, E, N };
  const int num_vertices = N;
  Edge edge_array[] = { Edge(A,B), Edge(A,D), Edge(C,A), Edge(D,C), Edge(C,E), Edge(B,D), Edge(D,E) };
  const int num_edges = sizeof(edge_array)/sizeof(Edge);
  int weights[] = { 1, 2, 1, 2, 7, 3, 1};
  Graph g(edge_array, edge_array + num_vertices, weights, num_vertices);
  cout << "edges(g) = ";
  Graph::edge_iterator EdgeI, EdgeLast;
  for (tie(EdgeI, EdgeLast) = edges(g); EdgeI != EdgeLast; ++EdgeI)
    cout << "(" << source(*EdgeI, g) << "," << target(*EdgeI, g) << ") ";
  cout << endl;
  vector<int> distances(num_vertices);
  Vertex start = *(vertices(g).first);
  dijkstra_shortest_paths(g, start, distance_map(&distances[0]));
  cout << "distances from start vertex:" << endl;
  Graph::vertex_iterator VertexI, VertexLast;

  for(tie(VertexI, VertexLast) = vertices(g); VertexI != VertexLast; ++VertexI)
    cout << "distance(" << *VertexI << ") = " << distances[*VertexI] << endl;
  cout << endl;
  return 0;
}


#ifndef COORDINATE_CALCULATION
#define COORDINATE_CALCULATION

#include "util/coordinate.hpp"

#include <boost/optional.hpp>

#include <utility>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

namespace detail
{
const constexpr long double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr long double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856;
}

//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);

double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate)
{
    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
    // dot product of two un-normed vectors
    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
    // squared length of the slope vector
    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
                                static_cast<double>(slope_vector.lat * slope_vector.lat);

    if (squared_length < std::numeric_limits<double>::epsilon())
    {
        return {0, source};
    }

    const double normed_ratio = unnormed_ratio / squared_length;
    double clamped_ratio = normed_ratio;
    if (clamped_ratio > 1.)
    {
        clamped_ratio = 1.;
    }
    else if (clamped_ratio < 0.)
    {
        clamped_ratio = 0.;
    }

    return {clamped_ratio,
            {
                FloatLongitude(1.0 - clamped_ratio) * source.lon +
                    target.lon * FloatLongitude(clamped_ratio),
                FloatLatitude(1.0 - clamped_ratio) * source.lat +
                    target.lat * FloatLatitude(clamped_ratio),
            }};
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio);

Coordinate centroid(const Coordinate lhs, const Coordinate rhs);

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);

// Get angle of line segment (A,C)->(C,B)
double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third);

// find the center of a circle through three coordinates
boost::optional<Coordinate> circleCenter(const Coordinate first_coordinate,
                                         const Coordinate second_coordinate,
                                         const Coordinate third_coordinate);

// find the radius of a circle through three coordinates
double circleRadius(const Coordinate first_coordinate,
                    const Coordinate second_coordinate,
                    const Coordinate third_coordinate);

// factor in [0,1]. Returns point along the straight line between from and to. 0 returns from, 1
// returns to
Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to);

} // ns coordinate_calculation
} // ns util
} // ns osrm

#endif // COORDINATE_CALCULATION


//
// Created by Amir Masoud Abdol on 2020-04-11
//

#include "TestStrategy.h"

#include <boost/math/distributions/students_t.hpp>

using namespace sam;
using boost::math::students_t;

void TTest::run(Experiment *experiment) {

  static ResultType res{};

  // The first group is always the control group
  for (int i{experiment->setup.nd()}, d{0}; i < experiment->setup.ng();
       ++i, ++d %= experiment->setup.nd()) {

    if (params.var_equal) {
      res = two_samples_t_test_equal_sd((*experiment)[d].mean_, (*experiment)[d].stddev_,
                                        (*experiment)[d].nobs_, (*experiment)[i].mean_,
                                        (*experiment)[i].stddev_, (*experiment)[i].nobs_,
                                        params.alpha, params.alternative);
    } else {
      res = two_samples_t_test_unequal_sd((*experiment)[d].mean_, (*experiment)[d].stddev_,
                                          (*experiment)[d].nobs_, (*experiment)[i].mean_,
                                          (*experiment)[i].stddev_, (*experiment)[i].nobs_,
                                          params.alpha,
                                          params.alternative);
    }

    (*experiment)[i].stats_ = res.tstat;
    (*experiment)[i].pvalue_ = res.pvalue;
    (*experiment)[i].sig_ = res.sig;
  }
}

TTest::ResultType TTest::t_test(const arma::Row<float> &dt1,
                                const arma::Row<float> &dt2, float alpha,
                                TestStrategy::TestAlternative alternative) {
  return t_test(arma::mean(dt1), arma::stddev(dt1), dt1.size(), arma::mean(dt2),
                arma::stddev(dt2), dt2.size(), alpha, alternative, true);
}

TTest::ResultType TTest::t_test(float Sm1, float Sd1, float Sn1, float Sm2,
                                float Sd2, float Sn2, float alpha,
                                TestStrategy::TestAlternative alternative,
                                bool equal_var = false) {

  if (Sm1 == 0.) {
    return single_sample_t_test(Sm1, Sm2, Sd2, Sn2, alpha, alternative);
  }

  if (equal_var) {
    return two_samples_t_test_equal_sd(Sm1, Sd1, Sn1, Sm2, Sd2, Sn2, alpha,
                                       alternative);
  } else {
    return two_samples_t_test_unequal_sd(Sm1, Sd1, Sn1, Sm2, Sd2, Sn2, alpha,
                                         alternative);
  }
}

std::pair<float, bool>
TTest::compute_pvalue(float t_stat, float df, float alpha, TestStrategy::TestAlternative alternative) {

  using boost::math::students_t;
  students_t dist(df);

  double p{0};
  bool sig{false};

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Mean != M
    p = 2. * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) { // Alternative "NOT REJECTED"
      sig = true;
    } else { // Alternative "REJECTED"
      sig = false;
    }
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Mean  > M
    p = cdf(dist, t_stat);
    if (p > alpha) { // Alternative "NOT REJECTED"
      sig = true;
    } else { // Alternative "REJECTED"
      sig = false;
    }
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Mean  < M
    p = cdf(complement(dist, t_stat));
    if (p > alpha) { // Alternative "NOT REJECTED"
      sig = true;
    } else { // Alternative "REJECTED"
      sig = false;
    }
  }

  return std::make_pair(p, sig);
}

///
/// A Students t test applied to a single set of data. We are testing the null
/// hypothesis that the true mean of the sample is M, and that any variation is
/// down to chance.  We can also test the alternative hypothesis that any
/// difference is not down to chance
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      M      True Mean.
/// @param      Sm     Sample Mean.
/// @param      Sd     Sample Standard Deviation.
/// @param      Sn     Sample Size.
/// @param      alpha  Significance Level.
/// @return     TTest::ResultType
///
TTest::ResultType
TTest::single_sample_t_test(float M, float Sm, float Sd, unsigned Sn,
                            float alpha,
                            TestStrategy::TestAlternative alternative) {

  bool sig = false;

  // Difference in means:
  float diff = Sm - M;

  // Degrees of freedom:
  float df = Sn - 1;

  // t-statistic:
  float t_stat = diff * sqrt(float(Sn)) / Sd;

  //
  // Finally define our distribution, and get the probability:
  //
  students_t dist(df);
  float p = 0;

  //
  // Finally print out results of alternative hypothesis:
  //

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Mean != M
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Mean  > M
    p = cdf(dist, t_stat);
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Mean  < M
    p = cdf(complement(dist, t_stat));
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  return {.tstat = t_stat, .df = df, .pvalue = p, .sig = sig};
}

///
/// A Students t test applied to two sets of data. We are testing the null
/// hypothesis that the two samples have the same mean and that any difference
/// if due to chance.
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      Sm1    Sample Mean 1.
/// @param      Sd1    Sample Standard Deviation 1.
/// @param      Sn1    Sample Size 1.
/// @param      Sm2    Sample Mean 2.
/// @param      Sd2    Sample Standard Deviation 2.
/// @param      Sn2    Sample Size 2.
/// @param      alpha  Significance Level.
/// @return     TTest::ResultType
///
TTest::ResultType TTest::two_samples_t_test_equal_sd(
    float Sm1, float Sd1, unsigned Sn1, float Sm2, float Sd2, unsigned Sn2,
    float alpha, TestStrategy::TestAlternative alternative) {

  bool sig = false;

  // Degrees of freedom:
  float df = Sn1 + Sn2 - 2;

  // Pooled variance and hence standard deviation:
  float sp = sqrt(((Sn1 - 1) * Sd1 * Sd1 + (Sn2 - 1) * Sd2 * Sd2) / df);

  // NOTE: I had to do this, I don't want my simulations fail.
  // While this is not perfect, it allows me to handle an edge case
  // SAM should not throw and should continue working.
  if (!(isgreater(sp, 0) or isless(sp, 0))) {
    // Samples are almost equal and elements are constant
    sp += std::numeric_limits<float>::epsilon();
  }

  // t-statistic:
  float t_stat = (Sm1 - Sm2) / (sp * sqrt(1.0 / Sn1 + 1.0 / Sn2));

  //
  // Define our distribution, and get the probability:
  //
  students_t dist(df);
  float p = 0;

  //
  // Finally print out results of alternative hypothesis:
  //

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Sample 1 Mean != Sample 2 Mean
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Sample 1 Mean <  Sample 2 Mean
    p = cdf(dist, t_stat);
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {

    // Sample 1 Mean >  Sample 2 Mean
    p = cdf(complement(dist, t_stat));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else
      sig = false; // Alternative "REJECTED"
  }

  return {.tstat = t_stat, .df = df, .pvalue = p, .sig = sig};
}

///
/// A Students t test applied to two sets of data with _unequal_ variance. We
/// are testing the null hypothesis that the two samples have the same mean and
/// that any difference is due to chance.
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      Sm1    Sample Mean 1.
/// @param      Sd1    Sample Standard Deviation 1.
/// @param      Sn1    Sample Size 1.
/// @param      Sm2    Sample Mean 2.
/// @param      Sd2    Sample Standard Deviation 2.
/// @param      Sn2    Sample Size 2.
/// @param      alpha  Significance Level.
/// @return     TTest::ResultType
///
TTest::ResultType TTest::two_samples_t_test_unequal_sd(
    float Sm1, float Sd1, unsigned Sn1, float Sm2, float Sd2, unsigned Sn2,
    float alpha, TestStrategy::TestAlternative alternative) {

  bool sig = false;

  // Degrees of freedom:
  float df = Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2;
  df *= df;
  float t1 = Sd1 * Sd1 / Sn1;
  t1 *= t1;
  t1 /= (Sn1 - 1);
  float t2 = Sd2 * Sd2 / Sn2;
  t2 *= t2;
  t2 /= (Sn2 - 1);
  df /= (t1 + t2);

  // t-statistic:
  float t_stat = (Sm1 - Sm2) / sqrt(Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2);

  //
  // Define our distribution, and get the probability:
  //
  students_t dist(df);
  float p = 0;

  //
  // Finally print out results of alternative hypothesis:
  //

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Sample 1 Mean != Sample 2 Mean
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Sample 1 Mean <  Sample 2 Mean
    p = cdf(dist, t_stat);
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Sample 1 Mean >  Sample 2 Mean
    p = cdf(complement(dist, t_stat));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else
      sig = false; // Alternative "REJECTED"
  }

  return {.tstat = t_stat, .df = df, .pvalue = p, .sig = sig};
}


// ========================================================================== //
// Copyright (c) 2014-2019 The University of Texas at Austin.                 //
// All rights reserved.                                                       //
//                                                                            //
// Licensed under the Apache License, Version 2.0 (the "License");            //
// you may not use this file except in compliance with the License.           //
// A copy of the License is included with this software in the file LICENSE.  //
// If your copy does not contain the License, you may obtain a copy of the    //
// License at:                                                                //
//                                                                            //
//     https://www.apache.org/licenses/LICENSE-2.0                            //
//                                                                            //
// Unless required by applicable law or agreed to in writing, software        //
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT  //
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.           //
// See the License for the specific language governing permissions and        //
// limitations under the License.                                             //
//                                                                            //
// ========================================================================== //

#pragma once

#include <iostream>
#include "dtypes.h"

#include <boost/math/quaternion.hpp>
#include <cmath>

typedef boost::math::quaternion<double> quaternion;

namespace gxy
{

class Q
{
public:
	Q() { zero(); }
	Q(quaternion q) : q(q) {}
	Q(vec3f axis) { q = quaternion(0.0, axis.x, axis.y, axis.z); }
	Q(vec3f u, float r) { q =  quaternion(cos(r/2), sin(r/2)*u.x, sin(r/2)*u.y, sin(r/2)*u.z); }

	~Q() {}

	Q operator*(const Q& q) const
	{
		return Q(this->q * q.q);
	}

	void print(std::ostream& o)
	{
	}

	vec3f rotate(vec3f v)
	{
		quaternion qr = this->q * quaternion(0.0, v.x, v.y, v.z) * conj(this->q);
		return vec3f( qr.R_component_2(), qr.R_component_3(),qr.R_component_4());
	}

	void zero() { q = quaternion(1.0, 0.0, 0.0, 0.0); }

	quaternion get() { return q; }

private:
	quaternion q;
};

class Trackball
{
public:
	Trackball(float s = 1) : size(s) {}
	~Trackball() {}

	void spin(float p1x, float p1y, float p2x, float p2y)
	{
    vec3f axis;      /* Axis of rotation */
    float phi;       /* how much to rotate about axis */
    vec3f p1, p2, d;
    float t;

    if (p1x == p2x && p1y == p2y)
			return;

    /*
     * First, figure out z-coordinates for projection of P1 and P2 to
     * deformed sphere
     */
    p1 = vec3f(p1x, p1y, project_to_sphere(size,p1x,p1y));
    p2 = vec3f(p2x, p2y, project_to_sphere(size,p2x,p2y));

    /*
     *  Now, we want the cross product of P1 and P2
     */
    cross(p2,p1,axis);
    normalize(axis);

    /*
     *  Figure out how much to rotate around that axis.
     */
    sub(p1,p2,d);
    t = len(d) / (2.0*size);

    /*
     * Avoid problems with out-of-control values...
     */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;
    phi = 2.0 * asin(t);

		current = current * Q(axis, phi);

		return;
	}

	vec3f rotate_vector(vec3f v)
	{
		quaternion qr = current.get() * quaternion(0.0, v.x, v.y, v.z) * conj(current.get());
		return vec3f( qr.R_component_2(), qr.R_component_3(),qr.R_component_4());
	}

	Q get() { return current; }

	void reset() { current.zero(); }

private:
	Q current;
	float size;

	float project_to_sphere(float r, float x, float y)
	{
    float d, t, z;

    d = sqrt(x*x + y*y);
    if (d < r * 0.70710678118654752440)
        z = sqrt(r*r - d*d);
    else
		{
        t = r / 1.41421356237309504880;
        z = t*t / d;
    }
    return z;
	}
};

}


/**
 * \Description:
 * this node is used to estimate the absoulte location of the robot using the optimization library  "RobOptim".
 * It subscribe to:
 * - /beacon_distances
 * It publishes to:
 * - /robot_pose
 *
 * SERRANO&ALI_ECN_M1_2017
 */

// Cpp
#include <stdexcept>
#include <sstream>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <math.h>

// Boost
#include <boost/shared_ptr.hpp>

// RobOptim
#include <roboptim/core/linear-function.hh>
#include <roboptim/core/differentiable-function.hh>
#include <roboptim/core/twice-differentiable-function.hh>
#include <roboptim/core/io.hh>
#include <roboptim/core/solver.hh>
#include <roboptim/core/solver-factory.hh>

//ROS
#include "ros/ros.h"
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/Float64MultiArray.h>

// blue beacon coordinates
#define X1 0
#define Y1 0
// red beacon coordinates
#define X2 0
#define Y2 190
// green beacon coordinates
#define X3 350
#define Y3 0
// upper and lower bounds of map
#define LB 0
#define UB 400

// global variables
float blue_distance, red_distance, green_distance;
bool are_dist_available = false;
ros::Publisher pose_pub;

using namespace roboptim;
using namespace std;

// function to minimize
struct F : public TwiceDifferentiableFunction
{
    F () : TwiceDifferentiableFunction (2, 1, "(x[0]-X1)^2 + (x[1]-Y1)^2 + (x[0]-X2)^2 + (x[1]-Y2)^2 + (x[0]-X3)^2 + (x[1]-Y3)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X1, 2) + pow(x[1]-Y1, 2) + pow(x[0]-X2, 2) + pow(x[1]-Y2, 2) + pow(x[0]-X3, 2) + pow(x[1]-Y3, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X1) + 2*(x[0]-X2) + 2*(x[0]-X3),
                2*(x[1]-Y1) + 2*(x[1]-Y2) + 2*(x[1]-Y3);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 6.0, 0.0,
                0.0, 6.0;
    }
};

// constraint 1
struct G0 : public TwiceDifferentiableFunction
{
    G0 () : TwiceDifferentiableFunction (2, 1, "(x[0]-X1)^2 + (x[1]-Y1)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X1, 2) + pow(x[1]-Y1, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X1),
                2*(x[1]-Y1);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 2.0, 0.0,
                0.0, 2.0;
    }
};

// constraint 2
struct G1 : public TwiceDifferentiableFunction
{
    G1 () : TwiceDifferentiableFunction (2, 1, "(x[0]-X2)^2 + (x[1]-Y2)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X2, 2) + pow(x[1]-Y2, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X2),
                2*(x[1]-Y2);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 2.0, 0.0,
                0.0, 2.0;
    }
};

// constraint 3
struct G2 : public TwiceDifferentiableFunction
{
    G2 () : TwiceDifferentiableFunction (2, 1, "(x[0]-X3)^2 + (x[1]-Y3)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X3, 2) + pow(x[1]-Y3, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X3),
                2*(x[1]-Y3);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 2.0, 0.0,
                0.0, 2.0;
    }
};

// Callback functions
void getDistanceCallback(std_msgs::Float64MultiArray distances)
{
    are_dist_available = true;
    blue_distance = distances.data[0];
    red_distance = distances.data[1];
    green_distance = distances.data[2];
}

int main (int argc, char** argv)
{

    //ROS Initialization
    ros::init(argc, argv, "triangulation_optimization");

    // Define your node handles
    ros::NodeHandle nh, nh_loc("~");

    // Declare your subscribers
    ros::Subscriber dist_sub = nh.subscribe<std_msgs::Float64MultiArray>("beacon_distances", 1, getDistanceCallback);

    // Declare your publishers
    pose_pub = nh.advertise<geometry_msgs::Pose2D>("robot_pose", 1);

    // node initilization
    // ...

    // rate
    ros::Rate rate(30); // Hz
    while (ros::ok()){
        ros::spinOnce();

        // node code
        if (!are_dist_available) {
            ROS_INFO("Waiting for distances");
            continue;
        }

        // ------------ RobOptim ---------------
        typedef Solver<EigenMatrixDense> solver_t;

        // Create cost function.
        boost::shared_ptr<F> f (new F ());

        // Create problem.
        solver_t::problem_t pb (f);

        // Set bounds for all optimization parameters.
        // 1. < x_i < 5. (x_i in [1.;5.])
        for (Function::size_type i = 0; i < pb.function ().inputSize (); ++i)
            pb.argumentBounds ()[i] = Function::makeInterval (LB, UB);

        // Set the starting point.
        Function::vector_t start (pb.function ().inputSize ());
        start << 150.,100.;
        pb.startingPoint() = start;

        // Create constraints.
        boost::shared_ptr<G0> g0 (new G0 ());
        boost::shared_ptr<G1> g1 (new G1 ());
        boost::shared_ptr<G2> g2 (new G2 ());

        F::intervals_t bounds;
        solver_t::problem_t::scaling_t scaling;

        // Add constraints
        bounds.push_back(Function::makeLowerInterval( pow(blue_distance, 2) ));  // blue beacon distance
        scaling.push_back (1.);
        pb.addConstraint (g0, bounds, scaling);

        bounds.clear ();
        scaling.clear ();

        bounds.push_back(Function::makeLowerInterval( pow(red_distance, 2) ));   // red beacon distance
        scaling.push_back (1.);
        pb.addConstraint (g1, bounds, scaling);

        bounds.clear ();
        scaling.clear ();

        bounds.push_back(Function::makeLowerInterval( pow(green_distance, 2) )); // green beacon distance
        scaling.push_back (1.);
        pb.addConstraint (g2, bounds, scaling);

        bounds.clear ();
        scaling.clear ();

        // Initialize solver.

        // Here we are relying on the CFSQP solver.
        // You may change this string to load the solver you wish to use:
        //  - Ipopt: "ipopt", "ipopt-sparse", "ipopt-td"
        //  - Eigen: "eigen-levenberg-marquardt"
        //  etc.
        // The plugin is built for a given solver type, so choose it adequately.
        SolverFactory<solver_t> factory ("ipopt", pb);
        solver_t& solver = factory ();

        // Compute the minimum and retrieve the result.
        solver_t::result_t res = solver.minimum ();

        // Display solver information.
        // std::cout << solver << std::endl;

        // Check if the minimization has succeeded.

        // Process the result
        switch (res.which ())
        {
        case solver_t::SOLVER_VALUE:
        {
            // Get the result.
            Result& result = boost::get<Result> (res);

            // Display the result.
            std::cout << "A solution has been found: " << std::endl << result << std::endl;
            geometry_msgs::Pose2D pose_msg;
            pose_msg.x = result.x[0];
            pose_msg.y = result.x[1];
            pose_msg.theta = 0;
            pose_pub.publish(pose_msg);
            break;
        }

        case solver_t::SOLVER_VALUE_WARNINGS:
        {
            // Get the result.
            ResultWithWarnings& result = boost::get<ResultWithWarnings> (res);

            // Display the result.
            std::cout << "A solution w/warning has been found: " << std::endl
                      << result << std::endl;

            break;
        }

        case solver_t::SOLVER_NO_SOLUTION:
        case solver_t::SOLVER_ERROR:
        {
            std::cout << "A solution should have been found. Failing..."
                      << std::endl
                      << boost::get<SolverError> (res).what ()
                      << std::endl;

            break;
        }
        }

        are_dist_available = false;

        rate.sleep();
    }
}


/*
 * DIPlib 3.0
 * This file contains the thin plate spline functionality
 *
 * (c)2019, Cris Luengo.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "diplib/library/numeric.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#if __GNUC__ >= 7 || __clang_major__ >= 12
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#if __GNUC__ >= 9
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <Eigen/QR>
#include <Eigen/Cholesky>

namespace dip {

namespace {

dfloat RadialBasis( dfloat r ) {
   return r > 0 ? r * r * std::log( r ) : 0;
}

} // namespace

ThinPlateSpline::ThinPlateSpline(
      FloatCoordinateArray coordinate,    // use std::move if you can!
      FloatCoordinateArray const& value,  // correspondence points
      dfloat lambda
) {
   // NOTE: `source` and `destination` are already checked for sizes
   c_ = std::move( coordinate );
   dip::uint nPoints = c_.size();
   dip::uint nDims = c_[ 0 ].size();

   // Create matrices L and b
   DIP_ASSERT( value.size() == nPoints );
   dip::uint N = nPoints + nDims + 1;
   Eigen::MatrixXd L( N, N );
   L.fill( 0 );
   Eigen::MatrixXd b( N, nDims );
   b.fill( 0 );
   dfloat alpha = 0;
   for( dip::uint ii = 0; ii < nPoints; ++ii ) {
      for( dip::uint jj = 0; jj < ii; ++jj ) {
         L( ii, jj ) = L( jj, ii ); // Previously computed, L is symmetric
      }
      for( dip::uint jj = ii + 1; jj < nPoints; ++jj ) {
         dfloat d = Distance( c_[ ii ], c_[ jj ] );
         L( ii, jj ) = RadialBasis( d );
         alpha += d;
      }
      L( ii, nPoints ) = 1;
      for( dip::uint jj = 0; jj < nDims; ++jj ) {
         L( ii, nPoints + 1 + jj ) = c_[ ii ][ jj ];
         b( ii, jj ) = value[ ii ][ jj ] - c_[ ii ][ jj ];
      }
   }
   if( lambda > 0.0 ) {
      alpha /= static_cast< dfloat >( nPoints * ( nPoints - 1 ) / 2 );
      alpha *= alpha * lambda;
      for( dip::uint ii = 0; ii < nPoints; ++ii ) {
         L( ii, ii ) = alpha;
      }
   }
   for( dip::uint jj = 0; jj < nDims + 1; ++jj ) {
      L.row( nPoints + jj ) = L.col( nPoints + jj );
   }

   // Solve equation Lx=b for x
   // Using Eigen::Ref to get in-place decomposition, it re-uses L to store the decomposition.
   Eigen::HouseholderQR <Eigen::Ref< Eigen::MatrixXd >> decomposition( L );
   //Eigen::ColPivHouseholderQR <Eigen::Ref< Eigen::MatrixXd >> decomposition( L );
   // TODO: Eigen::HouseholderQR is faster but less accurate than Eigen::ColPivHouseholderQR. Which one to pick?
   x_.resize( N * nDims );
   Eigen::Map< Eigen::MatrixXd >( x_.data(), N, nDims ) = decomposition.solve( b );
}

// Evaluates the thin plate spline function at point `pt`.
FloatArray ThinPlateSpline::Evaluate( FloatArray const& pt ) {
   dip::uint nPoints = c_.size();
   dip::uint nDims = c_[ 0 ].size();
   dip::uint N = nPoints + nDims + 1;
   Eigen::Map <Eigen::MatrixXd> x( x_.data(), N, nDims );
   // Note: w( ii, jj ) = x( ii, jj ), and a( ii, jj ) = x( nPoints + ii, jj )
   FloatArray res = pt;
   for( dip::uint ii = 0; ii < nPoints; ++ii ) {
      dfloat scale = RadialBasis( Distance( pt, c_[ ii ] ));
      for( dip::uint jj = 0; jj < nDims; ++jj ) {
         res[ jj ] += x( ii, jj ) * scale;
      }
   }
   for( dip::uint jj = 0; jj < nDims; ++jj ) {
      res[ jj ] += x( nPoints, jj );
   }
   for( dip::uint ii = 0; ii < nDims; ++ii ) {
      for( dip::uint jj = 0; jj < nDims; ++jj ) {
         res[ jj ] += x( nPoints + 1 + ii, jj ) * pt[ ii ];
      }
   }
   return res;
}

} // namespace dip

#if defined(__GNUG__) || defined(__clang__)
#pragma GCC diagnostic pop
#endif


#pragma once

#include <Eigen/Dense>
#include <geometry.hpp>

// reference: https://www.kalmanfilter.net/multiSummary.html fuck http://ros-developer.com/2019/04/10/kalman-filter-explained-with-python-code-from-scratch/
// Everything is fitted for 2d tracking

class Kalman{
public:
    static const int    nx = 4,
                        nu = 2,
                        nz = 2, NUM_VARS = 1;

    //  Actual state
    Eigen::Matrix<double, nx, 1> Xn;
    // Output state
    Eigen::Matrix<double, nz, 1> Zn;
    // State transition matrix
    Eigen::Matrix<double, nx, nx> F;
    // Control matrix
    Eigen::Matrix<double, nx, nu> G;
    // Covariance matrix
    Eigen::Matrix<double, nx, nx> P;
    // Process covariance matrix
    Eigen::Matrix<double, nx, nx> Q;
    // Uncertainty matrix
    Eigen::Matrix<double, nz, nz> R;
    // Observation matrix
    Eigen::Matrix<double, nz, nx> H;
    // Kalman gain matrix
    Eigen::Matrix<double, nx, nz> K;
    // Identity matrix
    Eigen::Matrix<double, nx, nx> I;

    float dt;

    // Variable indexes
    static const int iX  = 0; // X position
    static const int iY  = 1; // Y position
    static const int idX = 2; // X velocity
    static const int idY = 3; // Y velocity

    Kalman(){
        // Prediction matrices
        F.setIdentity();
        G.setZero();
        Q.setIdentity();
        P.setIdentity();

        H <<    1, 0, 0, 0,
                0, 1, 0, 0;

        R <<    1, 0,
                0, 1;
    }

    Kalman(double iniX, double inidX, double iniY, double inidY){
        Xn(iX)  = iniX;
        Xn(idX) = inidX;
        Xn(iY)  = iniY;
        Xn(idY) = inidY;

        // Prediction matrices
        F = F.setIdentity();
        G = G.setZero();
        Q = Q.setIdentity();

        R = R.setIdentity();
        R *= 5;

        P = P.setIdentity();

        H = H.setZero();
        H(iX, iX) = 1;
        H(iY, iY) = 1;

        I = I.Identity();
    }

    void init(double iniX, double iniY, double dt){
        Xn(iX)  = iniX;
        Xn(idX) = 0;
        Xn(iY)  = iniY;
        Xn(idY) = 0;
        this->dt = dt;

        F(iX, idX) = dt;
        F(iY, idY) = dt;
        G(iX, iX) = 0.5 * dt * dt;
        G(iY, iY) = 0.5 * dt * dt;
        G(idX, iX) = dt;
        G(idY, iY) = dt;

        Q = G * 1 * G.transpose();
    }

    // // Prediction method for object tracking
    // void predict(){
    //     // Predict:   Xn+1 = F * Xn + G * Un
    //     Xn = F * Xn;

    // }

    // Prediction method for object tracking
    void predict(){
        // Predict:   Xn+1 = F * Xn + G * Un
        Xn = F * Xn;

        P  = F * P * F.transpose() + Q;
    }

    void update(int x, int y){
        // Zn
        Zn <<   x,
                y;

        // K
        K = P * H.transpose() * (H * P * H.transpose() + R).inverse();

        // Xn
        Xn = Xn + K * (Zn - H * Xn);

        // P
        Eigen::Matrix<double, nx, nx> I;
        I = I.Identity();
        // P = P * (I.Identity() - K * H);
        // P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();
        P = P * (I - K * H);
    }

    std::pair<double, double> getPosition(){
        return {Xn(iX), Xn(iY)};
    }
};


#ifndef enhancer_hpp
#define enhancer_hpp

#include <cmath>
#include <Eigen/Core>

namespace enhancer
{
#if defined(ENHANCER_WITH_LIFT_GAMMA_GAIN)
    constexpr int NUM_PARAMETERS = 12;
#else
    constexpr int NUM_PARAMETERS = 5;
#endif

    ///////////////////////////////////////////////////////////
    // Interface
    ///////////////////////////////////////////////////////////

    inline Eigen::Vector3d enhance(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters);

    ///////////////////////////////////////////////////////////
    // Implementation
    ///////////////////////////////////////////////////////////

    namespace internal
    {
        inline Eigen::Vector3d convertRgbToLinearRgb(const Eigen::Vector3d& rgb)
        {
            return rgb.array().pow(2.2).matrix();
        }

        inline Eigen::Vector3d convertLinearRgbToRgb(const Eigen::Vector3d& linear_rgb)
        {
            return linear_rgb.array().pow(1.0 / 2.2).matrix();
        }

        // Y'UV (BT.709) to linear RGB
        // Values are from https://en.wikipedia.org/wiki/YUV
        inline Eigen::Vector3d yuv2rgb(const Eigen::Vector3d& yuv)
        {
            constexpr double m[9] = { +1.00000, +1.00000, +1.00000,   // 1st column
                                      +0.00000, -0.21482, +2.12798,   // 2nd column
                                      +1.28033, -0.38059, +0.00000 }; // 3rd column
            return Eigen::Map<const Eigen::Matrix3d>(m) * yuv;
        }

        // Linear RGB to Y'UV (BT.709)
        // Values are from https://en.wikipedia.org/wiki/YUV
        inline Eigen::Vector3d rgb2yuv(const Eigen::Vector3d& rgb)
        {
            constexpr double m[9] = { +0.21260, -0.09991, +0.61500,   // 1st column
                                      +0.71520, -0.33609, -0.55861,   // 2nd column
                                      +0.07220, +0.43600, -0.05639 }; // 3rd column
            return Eigen::Map<const Eigen::Matrix3d>(m) * rgb;
        }

        inline double rgb2h(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            double h;
            if (M == m)      h = 0.0;
            else if (m == b) h = 60.0 * (g - r) / (M - m) + 60.0;
            else if (m == r) h = 60.0 * (b - g) / (M - m) + 180.0;
            else if (m == g) h = 60.0 * (r - b) / (M - m) + 300.0;
            else             abort();
            h /= 360.0;
            if (h < 0.0) {
                ++ h;
            } else if (h > 1.0) {
                -- h;
            }
            return h;
        }

        inline double rgb2s4hsv(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            if (M < 1e-14) return 0.0;
            return (M - m) / M;
        }

        inline double rgb2s4hsl(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            if (M - m < 1e-14) return 0.0;
            return (M - m) / (1.0 - std::abs(M + m - 1.0));
        }

        inline Eigen::Vector3d hsl2rgb(const Eigen::Vector3d& hsl)
        {
            auto hue2rgb = [](const double f1, const double f2, double hue)
            {
                if (hue < 0.0) hue += 1.0;
                if (hue > 1.0) hue -= 1.0;

                double res;
                if ((6.0 * hue) < 1.0)
                    res = f1 + (f2 - f1) * 6.0 * hue;
                else if ((2.0 * hue) < 1.0)
                    res = f2;
                else if ((3.0 * hue) < 2.0)
                    res = f1 + (f2 - f1) * ((2.0 / 3.0) - hue) * 6.0;
                else
                    res = f1;
                return res;
            };

            if (hsl.y() == 0.0)
            {
                return Eigen::Vector3d(hsl.z(), hsl.z(), hsl.z());
            }

            const double f2 = (hsl.z() < 0.5) ? hsl.z() * (1.0 + hsl.y()) : (hsl.z() + hsl.y()) - (hsl.y() * hsl.z());
            const double f1 = 2.0 * hsl.z() - f2;

            Eigen::Vector3d rgb;
            rgb(0) = hue2rgb(f1, f2, hsl.x() + (1.0 / 3.0));
            rgb(1) = hue2rgb(f1, f2, hsl.x());
            rgb(2) = hue2rgb(f1, f2, hsl.x() - (1.0 / 3.0));

            return rgb;
        }

        inline Eigen::Vector3d rgb2hsv(const Eigen::Vector3d& rgb)
        {
            const double& r = rgb(0);
            const double& g = rgb(1);
            const double& b = rgb(2);

            const double M = std::max({r, g, b});

            const double h = rgb2h(rgb);
            const double s = rgb2s4hsv(rgb);
            const double v = M;

            return Eigen::Vector3d(h, s, v);
        }

        inline double rgb2l(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            return 0.5 * (M + m);
        }

        inline Eigen::Vector3d hsv2rgb(const Eigen::Vector3d& hsv)
        {
            const double& h = hsv(0);
            const double& s = hsv(1);
            const double& v = hsv(2);

            if (s < 1e-14)
            {
                return Eigen::Vector3d(v, v, v);
            }

            const double h6 = h * 6.0;
            const int    i  = static_cast<int>(floor(h6)) % 6;
            const double f  = h6 - static_cast<double>(i);
            const double p  = v * (1 - s);
            const double q  = v * (1 - (s * f));
            const double t  = v * (1 - (s * (1 - f)));
            double r, g, b;
            switch(i)
            {
                case 0: r = v; g = t; b = p; break;
                case 1: r = q; g = v; b = p; break;
                case 2: r = p; g = v; b = t; break;
                case 3: r = p; g = q; b = v; break;
                case 4: r = t; g = p; b = v; break;
                case 5: r = v; g = p; b = q; break;
            }

            return Eigen::Vector3d(r, g, b);
        }

        inline Eigen::Vector3d rgb2hsl(const Eigen::Vector3d& rgb)
        {
            const double h = rgb2h(rgb);
            const double s = rgb2s4hsl(rgb);
            const double l = rgb2l(rgb);

            return Eigen::Vector3d(h, s, l);
        }

        inline float clamp(const float value) { return std::max(0.0, std::min(static_cast<double>(value), 1.0)); }
        inline Eigen::Vector3d clamp(const Eigen::Vector3d& v) { return Eigen::Vector3d(clamp(v.x()), clamp(v.y()), clamp(v.z())); }

        inline Eigen::Vector3d changeColorBalance(const Eigen::Vector3d& inputRgb, const Eigen::Vector3d& shift)
        {
            constexpr double a     = 0.250;
            constexpr double b     = 0.333;
            constexpr double scale = 0.700;

            const double          lightness = rgb2l(inputRgb);
            const Eigen::Vector3d midtones  = (clamp((lightness - b) / a + 0.5) * clamp((lightness + b - 1.0) / (- a) + 0.5) * scale) * shift;
            const Eigen::Vector3d newColor  = clamp(inputRgb + midtones);
            const Eigen::Vector3d newHsl    = rgb2hsl(newColor);

            return hsl2rgb(Eigen::Vector3d(newHsl(0), newHsl(1), lightness));
        }

        inline Eigen::Vector3d applyLiftGammaGainEffect(const Eigen::Vector3d& linear_rgb,
                                                        const Eigen::Vector3d& lift,
                                                        const Eigen::Vector3d& gamma,
                                                        const Eigen::Vector3d& gain)
        {
            const Eigen::Array3d lift_applied_linear_rgb  = ((linear_rgb.array() - Eigen::Array3d::Ones()) * (Eigen::Array3d::Constant(2.0) - lift.array()) + Eigen::Array3d::Ones()).max(0.0);
            const Eigen::Array3d gain_applied_linear_rgb  = lift_applied_linear_rgb * gain.array();
            const Eigen::Array3d gamma_applied_linear_rgb = gain_applied_linear_rgb.pow(gamma.array().inverse());

            return gamma_applied_linear_rgb.matrix();
        }

        inline Eigen::Vector3d applyTemperatureTintEffect(const Eigen::Vector3d& linear_rgb, const double temperature, const double tint)
        {
            constexpr double scale = 0.10;

            return clamp(yuv2rgb(rgb2yuv(linear_rgb) + temperature * scale * Eigen::Vector3d(0.0, -1.0, 1.0) + tint * scale * Eigen::Vector3d(0.0, 1.0, 1.0)));
        }

        inline Eigen::Vector3d applyBrightnessEffect(const Eigen::Vector3d& linear_rgb, const double brightness)
        {
            constexpr double scale = 1.5;

            return linear_rgb.array().pow(1.0 / (1.0 + scale * brightness)).matrix();
        }

        inline Eigen::Vector3d applySaturationEffect(const Eigen::Vector3d& linear_rgb, const double saturation)
        {
            const Eigen::Vector3d hsv = rgb2hsv(clamp(linear_rgb));
            const double s = clamp(hsv(1) * (saturation + 1.0));

            return hsv2rgb(Eigen::Vector3d(hsv(0), s, hsv(2)));
        }

        inline Eigen::Vector3d applyContrastEffect(const Eigen::Vector3d& linear_rgb, const double contrast)
        {
            constexpr double pi_4 = 3.14159265358979 * 0.25;

            const double contrast_coef = std::tan((contrast + 1.0) * pi_4);

            return convertRgbToLinearRgb((contrast_coef * (convertLinearRgbToRgb(linear_rgb) - Eigen::Vector3d::Constant(0.5)) + Eigen::Vector3d::Constant(0.5)).array().max(0.0));
        }

        inline Eigen::Vector3d enhance(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters)
        {
            assert(parameters.size() == NUM_PARAMETERS);

            const double brightness  = clamp(parameters[0]) - 0.5;
            const double contrast    = clamp(parameters[1]) - 0.5;
            const double saturation  = clamp(parameters[2]) - 0.5;

#if defined(ENHANCER_WITH_LIFT_GAMMA_GAIN)
            const Eigen::Vector3d lift  = Eigen::Vector3d::Constant(0.5) + clamp(parameters.segment<3>(3)); // [0.5, 1.5]^3
            const Eigen::Vector3d gamma = Eigen::Vector3d::Constant(0.5) + clamp(parameters.segment<3>(6)); // [0.5, 1.5]^3
            const Eigen::Vector3d gain  = Eigen::Vector3d::Constant(0.5) + clamp(parameters.segment<3>(9)); // [0.5, 1.5]^3
#else
            const double temperature = clamp(parameters[3]) - 0.5;
            const double tint        = clamp(parameters[4]) - 0.5;
#endif

            Eigen::Vector3d linear_rgb = convertRgbToLinearRgb(input_rgb);

#if defined(ENHANCER_WITH_LIFT_GAMMA_GAIN)
            // Lift/Gamma/Gain
            linear_rgb = applyLiftGammaGainEffect(linear_rgb, lift, gamma, gain);
#else
            // Approximate temperature/tint effect
            linear_rgb = applyTemperatureTintEffect(linear_rgb, temperature, tint);
#endif

            // Brightness
            linear_rgb = applyBrightnessEffect(linear_rgb, brightness);

            // Contrast
            linear_rgb = applyContrastEffect(linear_rgb, contrast);

            // Saturation
            linear_rgb = applySaturationEffect(linear_rgb, saturation);

            return clamp(convertLinearRgbToRgb(linear_rgb));
        }

        inline Eigen::Vector3d enhance_v1(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters)
        {
            assert(parameters.size() == NUM_PARAMETERS);

            const double          brightness  = parameters[0] - 0.5;
            const double          contrast    = parameters[1] - 0.5;
            const double          saturation  = parameters[2] - 0.5;
            const Eigen::Vector3d balance     = parameters.segment<3>(3) - Eigen::Vector3d::Constant(0.5);

            // color balance
            Eigen::Vector3d rgb = changeColorBalance(input_rgb, balance);

            // brightness
            rgb *= 1.0 + brightness;

            // contrast
            constexpr double pi_4 = 3.14159265358979 * 0.25;
            const double contrast_coef = std::tan((contrast + 1.0) * pi_4);
            rgb = contrast_coef * (rgb - Eigen::Vector3d::Constant(0.5)) + Eigen::Vector3d::Constant(0.5);

            // clamp
            rgb = clamp(rgb);

            // saturation
            Eigen::Vector3d hsv = rgb2hsv(rgb);
            double s = hsv.y();
            s *= saturation + 1.0;
            hsv(1) = clamp(s);
            const Eigen::Vector3d output_rgb = hsv2rgb(hsv);

            return output_rgb;
        }
    }

    inline Eigen::Vector3d enhance(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters)
    {
        return internal::enhance(input_rgb, parameters);
    }
}

#endif /* enhancer_hpp */


/* Small utility functions useful for point-based registration */
#include <Util.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <exception>

#include <Eigen/Dense>
#include <Exceptions.hpp>

bool isApproxEqual(double a, double b, double eps) {
    // Check whether a and b are equal, to within tolerance eps.
    auto diff = std::abs(a - b);
    return diff < eps;
}

bool isApproxEqual(double a, double b) {
    // Epsilon not specified, default to 0.001;
    return isApproxEqual(a, b, 0.001);
}

Eigen::Vector3d find_pointset_average(const Eigen::MatrixXd& pointset) {
    auto average = pointset.rowwise().mean();
    return average;
}

Eigen::MatrixXd residuals_from_point(const Eigen::MatrixXd& pointset, const Eigen::Vector3d& point) {
    return pointset.colwise() - point;
}

Eigen::VectorXd distances_between_pointsets(const Eigen::MatrixXd& pointset, const Eigen::MatrixXd& pointset_dash) {
    return (pointset - pointset_dash).colwise().norm();
}

double root_mean_square(const Eigen::VectorXd& v) {
    return sqrt((v.cwiseProduct(v)).mean());
}

Eigen::Matrix4d compose_final_transform(const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation) {
    // Compose 3x3 rotation and 3d vector translation to get a joint rotation+translation.
    // Final transform is a 4x4 rigid transform matrix, with rotation part in the top-left 3x3, and translation in the top-right 3x1.
    Eigen::Matrix4d final_transform;
    final_transform.block(0,0,3,3) << rotation;
    final_transform.block(0,3,3,1) << translation;
    final_transform.block(3,0,1,3) << 0, 0, 0;
    final_transform.block(3,3,1,1) << 1;

    return final_transform;
}

Eigen::MatrixXd apply_transform(const Eigen::MatrixXd& pointset, const Eigen::Matrix4d& transform) {
    // Need to add a one on the end of each vector, for the translation part of the transform.
    Eigen::MatrixXd pointset_augmented(4,pointset.cols());
    pointset_augmented.block(0,0,3,pointset.cols()) << pointset;
    pointset_augmented.block(3,0,1,pointset.cols()) << Eigen::MatrixXd::Constant(1, pointset.cols(), 1);

    auto proposed_pointset = transform * pointset_augmented;

    // Now remove the unnecessary bottom row of the transformed pointset.
    auto proposed_pointset_reduced = proposed_pointset.block(0,0,3,pointset.cols());

    return proposed_pointset_reduced;
}

Eigen::MatrixXd load_pointcloud_from_file(std::string filename) {
    int max_points = 1E6;
    int line_counter = 0;
    Eigen::MatrixXd points(3,max_points);

    std::ifstream infile;
    infile.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    try {
        infile.open(filename);

        double x, y, z;

        while(infile >> x >> y >> z) {
            points.col(line_counter) << x, y, z;
            line_counter++;
        }

    } catch(std::ifstream::failure e) {
        if(!infile.eof()) {
            // Any reason other than EOF is a failure. TODO: give a specific error message for non-existent file.
            std::cerr << "Could not read file " << filename << std::endl;
            throw(PointMatchingEx);
        } else{
            infile.close();

            auto pointcloud = points.block(0,0,3,line_counter);
            return pointcloud;
        }
    }

}

Eigen::Matrix4d load_transform_from_file(std::string filename) {
    int line_counter = 0;
    Eigen::Matrix4d transform;

    std::ifstream infile;
    infile.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    try {
        infile.open(filename);

        double a, b, c, d;

        while(infile >> a >> b >> c >> d) {
            transform.row(line_counter) << a, b, c, d;
            line_counter++;
        }

    } catch(std::ifstream::failure e) {
        if(!infile.eof()) {
            // Any reason other than EOF is a failure. TODO: give a specific error message for non-existent file.
            std::cerr << "Could not read file " << filename << std::endl;
            throw(PointMatchingEx);
        } else if(infile.eof() && line_counter == 4) {
            infile.close();
            return transform;
        } else {
            std::cerr << "Could not read file " << filename << std::endl;
            throw(PointMatchingEx);
        }
    }

}

void write_matrix_to_file(const Eigen::MatrixXd& matrix, std::string filename) {
    std::ofstream outfile(filename);
    if(outfile.is_open()) {
        outfile << matrix;
    }
}


//
// Created by jack on 9/13/20.
//

#include "feature_extracter.h"

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

#include "glog/logging.h"

namespace nautilus::input_processing {

std::vector<Eigen::Vector2f> GetNeighborhood(
    const std::vector<Eigen::Vector2f>& points, size_t point_index,
    size_t neighbors_per_side, double max_neighbor_distance) {
  std::vector<Eigen::Vector2f> neighbors;
  // Left side neighbors.
  for (size_t neighbor_idx =
           std::max(static_cast<size_t>(0), point_index - neighbors_per_side);
       neighbor_idx < point_index; neighbor_idx++) {
    if ((points[point_index] - points[neighbor_idx]).norm() <=
        max_neighbor_distance) {
      neighbors.push_back(points[neighbor_idx]);
    }
  }
  // Right side neighbors.
  for (size_t neighbor_idx = point_index + 1;
       neighbor_idx < std::min(points.size(), point_index + neighbors_per_side);
       neighbor_idx++) {
    neighbors.push_back(points[neighbor_idx]);
  }
  return neighbors;
}

Eigen::Vector2f ComputeMean(const std::vector<Eigen::Vector2f>& neighborhood) {
  // Compute the mean and return the mean vector.
  Eigen::Vector2f mean_vector(0, 0);
  for (const auto& p : neighborhood) {
    mean_vector += p;
  }
  return (1.0 / neighborhood.size()) * mean_vector;
}

std::vector<std::pair<double, Eigen::Vector2f>> ComputeSmoothnessScores(
    const std::vector<Eigen::Vector2f>& points,
    std::vector<float>* unsorted_scores, int neighbors_per_side,
    double max_neighbor_distance, int min_neighbor_num) {
  // For each point used the smoothness formula.
  // This formula is computing a scatter matrix for every point and its
  // neighborhood. Then the smoothness score is the smallest eigenvalue /
  // largest eigenvalue.
  std::vector<std::pair<double, Eigen::Vector2f>> smoothness_scores;
  for (size_t i = 0; i < points.size(); i++) {
    const auto& point = points[i];
    // Get the points around point, and then include point.
    std::vector<Eigen::Vector2f> neighborhood =
        GetNeighborhood(points, i, neighbors_per_side, max_neighbor_distance);
    if (neighborhood.size() < static_cast<size_t>(min_neighbor_num)) {
      // Skip this iteration if not enough neighbors.
      continue;
    }
    neighborhood.push_back(point);
    // Compute the neighborhood of point and all the points around it.
    const Eigen::Vector2f mean_vector = ComputeMean(neighborhood);
    // Now get the scatter matrix using the formula summation from 1 to n of all
    // points (Xi - m)(Xi - m)^T
    Eigen::Matrix2f scatter_matrix;
    scatter_matrix << 0, 0, 0, 0;
    for (const auto& p : neighborhood) {
      Eigen::Matrix2f temp =
          (p - mean_vector) * ((p - mean_vector).transpose());
      scatter_matrix += temp;
    }
    Eigen::EigenSolver<Eigen::Matrix2f> eigen_solver;
    eigen_solver.compute(scatter_matrix);
    // Smoothness score is smaller eigen value / larger eigen value so it is [0,
    // 1].
    auto eigen_values = eigen_solver.eigenvalues();
    CHECK_EQ(eigen_values.rows(), 2);
    double eigen_value_1 = eigen_values(0, 0).real();
    double eigen_value_2 = eigen_values(1, 0).real();
    double smoothness_score = std::min(eigen_value_1, eigen_value_2) /
                              std::max(eigen_value_1, eigen_value_2);
    if (smoothness_score < 0 || smoothness_score > 1) {
      std::cout << smoothness_score << std::endl;
      std::cout << "Eigen Values: " << eigen_value_1 << " " << eigen_value_2
                << std::endl;
    }
    smoothness_scores.emplace_back(smoothness_score, point);
    unsorted_scores->push_back(smoothness_score);
  }
  return smoothness_scores;
}

FeatureExtractor::FeatureExtractor(const std::vector<Eigen::Vector2f>& points,
                                   double threshold, double distance_threshold,
                                   int neighbor_num, int max_edge_num,
                                   int max_planar_num, int min_neighbor_num)
    : threshold_(threshold),
      distance_threshold_(distance_threshold),
      neighbors_per_side_(neighbor_num),
      max_edge_number_(max_edge_num),
      max_planar_number_(max_planar_num),
      min_neighbor_num_(min_neighbor_num) {
  // Compute the smoothness scores of every point once.
  smoothness_points_ =
      ComputeSmoothnessScores(points, &unordered_scores_, neighbors_per_side_,
                              max_neighbor_distance_, min_neighbor_num_);
  // Now sort the points based on their smoothness.
  std::sort(smoothness_points_.begin(), smoothness_points_.end(),
            [](const std::pair<double, Eigen::Vector2f>& point_a,
               const std::pair<double, Eigen::Vector2f>& point_b) {
              return point_a.first < point_b.first;
            });
}

bool validFeaturePoint(std::pair<double, Eigen::Vector2f> point,
                       std::vector<Eigen::Vector2f> points, double threshold,
                       double distance_threshold, size_t max_size,
                       bool is_edge = false) {
  // Planar points must be less than the threshold, and edge points must be more
  // than the threshold.
  if (!is_edge && point.first > threshold) {
    return false;
  }
  if (is_edge && point.first < threshold) {
    return false;
  }
  // We can accept points.
  if (points.size() >= max_size) {
    return false;
  }
  // Not close to any of the other points.
  for (const auto& p : points) {
    if ((p - point.second).norm() < distance_threshold) {
      return false;
    }
  }
  return true;
}

std::vector<Eigen::Vector2f> FeatureExtractor::GetPlanarPoints() {
  std::vector<Eigen::Vector2f> planar_points;
  // Super simple O(n^2) algorithm to find the planar points.
  for (size_t i = 0; i < smoothness_points_.size(); i++) {
    if (validFeaturePoint(smoothness_points_[i], planar_points, threshold_,
                          distance_threshold_, max_planar_number_)) {
      planar_points.push_back(smoothness_points_[i].second);
    }
  }
  return planar_points;
}

std::vector<Eigen::Vector2f> FeatureExtractor::GetEdgePoints() {
  std::vector<Eigen::Vector2f> edge_points;
  for (int i = smoothness_points_.size() - 1; i >= 0; i--) {
    if (validFeaturePoint(smoothness_points_[i], edge_points, threshold_,
                          distance_threshold_, max_edge_number_, true)) {
      edge_points.push_back(smoothness_points_[i].second);
    }
  }
  return edge_points;
}

std::vector<float> FeatureExtractor::GetSmoothnessScores() {
  return unordered_scores_;
}
}  // namespace nautilus::input_processing


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/auto_differentiation.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <chrono>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d(
    (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
    (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
  );
}

// Solve ICP with linear algebra (SVD solution)
void pose_estimation_3d3d(vector<Eigen::Vector3d> pts1,
                          vector<Eigen::Vector3d> pts2,
                          Eigen::Matrix3d &R, Eigen::Vector3d &t) {
  Eigen::Vector3d c1(0.0, 0.0, 0.0), c2(0.0, 0.0, 0.0);
  for (auto& p : pts1)
    c1 += p;
  c1 /= pts1.size();
  for (auto& p : pts2)
    c2 += p;
  c2 /= pts2.size();

  for (auto& p : pts1)
    p -= c1;
  for (auto& p : pts2)
    p -= c2;

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < pts1.size(); ++i)
  {
    W += pts1[i] * pts2[i].transpose();
  }
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  R = svd.matrixU() * svd.matrixV().transpose();
  if (R.determinant() < 0)
    R = -R;

  t = c1 - R * c2;
}

class VertexPose: public g2o::BaseVertex<6, Sophus::SE3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void setToOriginImpl() override {
      _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl(const double* update) override {
      Eigen::Matrix<double, 6, 1> u;
      u << update[0], update[1], update[2], update[3], update[4], update[5];
      _estimate = Sophus::SE3d::exp(u) * _estimate;
    }

  virtual bool read(istream &in) override {}
  virtual bool write(ostream &out) const override {}
};

class EdgeICP: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeICP(const Eigen::Vector3d& X2): _X2(X2) {}

    virtual void computeError() override {
      const VertexPose* v = static_cast<VertexPose*>(_vertices[0]);
      Sophus::SE3d Rt = v->estimate();
      _error = measurement() - Rt * _X2;
    }

    virtual void linearizeOplus() override {
      // fill _jacobianOPlusXi;
      const VertexPose* v = static_cast<VertexPose*>(_vertices[0]);
      Sophus::SE3d Rt = v->estimate();
      Eigen::Vector3d X2_transf = Rt * _X2;
      _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
      _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(X2_transf);
    }

    // Auto-diff is not working well. No idea why
    // template <class T>
    // bool operator() (const T* params, T* errors) const {
    //   const Eigen::Map<const Sophus::SE3<T>> Rt_sophus(params);
    //   Eigen::Matrix<T, 3, 1> X2(T(_X2.x()), T(_X2.y()), T(_X2.z()));
    //   Eigen::Matrix<T, 3, 1> X2_transf = Rt_sophus * X2;
    //   errors[0] = T(_measurement[0]) - X2_transf.x();
    //   errors[1] = T(_measurement[1]) - X2_transf.y();
    //   errors[2] = T(_measurement[2]) - X2_transf.z();
    //   return true;
    // }
    // G2O_MAKE_AUTO_AD_FUNCTIONS;

    virtual bool read(std::istream&) override {}
    virtual bool write(std::ostream&) const override {}

  private:
    Eigen::Vector3d _X2;
};

// Solve ICP with non-linear optimization
void bundleAdjustment(
  const vector<Eigen::Vector3d> &pts1,
  const vector<Eigen::Vector3d> &pts2,
  Eigen::Matrix3d &R, Eigen::Vector3d &t) {

    Sophus::SE3d pose_init(R, t);

    typedef g2o::BlockSolverX BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
      g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    VertexPose *vertex_pose = new VertexPose();
    vertex_pose->setId(0);
    vertex_pose->setEstimate(pose_init);
    optimizer.addVertex(vertex_pose);

    for (int i = 0; i < pts1.size(); ++i)
    {
      EdgeICP *edge = new EdgeICP(pts2[i]);
      edge->setVertex(0, vertex_pose);
      edge->setMeasurement(pts1[i]);
      edge->setInformation(Eigen::Matrix3d::Identity());
      optimizer.addEdge(edge);
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(50);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization ICP costs time: " << time_used.count() << " seconds." << endl;

    Sophus::SE3d optim_pose = vertex_pose->estimate();
    R = optim_pose.so3().unit_quaternion().toRotationMatrix();
    t = optim_pose.translation();
  }

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  string f4 = "../2_depth.png"; //argv[3];

  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);

  Mat depth1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat depth2 = imread(f4, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  std::vector<Eigen::Vector3d> pts1, pts2;

  for (DMatch m:matches) {
    ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
    if (d1 == 0 || d2 == 0)   // bad depth
      continue;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    float dd1 = float(d1) / 5000.0;
    float dd2 = float(d2) / 5000.0;
    pts1.push_back(Eigen::Vector3d(p1.x * dd1, p1.y * dd1, dd1));
    pts2.push_back(Eigen::Vector3d(p2.x * dd2, p2.y * dd2, dd2));
  }

  cout << "3d-3d pairs: " << pts1.size() << endl;
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Zero();
  pose_estimation_3d3d(pts1, pts2, R, t);
  cout << "ICP via SVD results: " << endl;
  cout << "R = " << R << endl;
  cout << "t = " << t.transpose() << endl;
  // cout << "R_inv = " << R.t() << endl;
  // cout << "t_inv = " << -R.t() * t << endl;

  // verify p1 = R * p2 + t
  double total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (SVD): " << total_error / pts1.size() << "\n";

  cout << "calling bundle adjustment" << endl;
  R = Eigen::Matrix3d::Identity();
  t = Eigen::Vector3d::Zero();
  bundleAdjustment(pts1, pts2, R, t);

  // verify p1 = R * p2 + t
  total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (BA): " << total_error / pts1.size() << "\n";
}