#include <CGAL/config.h>
#define CGAL_EIGEN3_ENABLED
#if defined(BOOST_GCC) && (__GNUC__ <= 4) && (__GNUC_MINOR__ < 4)
#include <iostream>
int main()
{
  std::cerr << "NOTICE: This test requires G++ >= 4.4, and will not be compiled." << std::endl;
}
#else
#include <CGAL/Epick_d.h>
#include <eigen3/Eigen/Core>
#include <CGAL/Delaunay_triangulation.h>
#include <CGAL/IO/Triangulation_off_ostream.h>
#include <CGAL/point_generators_d.h>
#include <CGAL/Timer.h>
#include <CGAL/algorithm.h>
#include <CGAL/Origin.h>

#include <vector>
#include <random>
#include <string>
#include <fstream>
#include <cstdlib>
#include <algorithm>
#include <cmath>
#include <boost/algorithm/string.hpp>
#include <chrono>

// uncomment this if want generate random points
// #define RANDOM_PTS

const double lower_bound = 0.0;
const double upper_bound = 1.0;
const double step_size = 0.01;
const double small_step_size = 0.001;
//TODO: Change the path_prefix before running the function!
const std::string path_prefix = "/Users/angelynaye/Desktop/Research/result/space-partition-adv";

/** Helper Functions Section  **/

//function to read in data from csv file
std::vector<std::vector<double> > read_data(std::string file_name, int dim,
std::string delimeter = " ")
{
	std::ifstream file(file_name);

	std::vector<std::vector<double> > data_list;

	std::string line = "";
	// Iterate through each line split the content using delimeter
  // convert it to double
	while (getline(file, line))
	{
		std::vector<std::string> vec;
		boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        std::vector<double> dd;
        dd.reserve(dim);
        for(std::vector<std::string>::iterator it = vec.begin();
        it != vec.end(); ++it) {
          dd.push_back(std::stod(*it));
        }

		data_list.push_back(dd);

	}
	// Close the File
	file.close();

	return data_list;
}

// return true if within [0,1]
template<int D>
bool check_boundary(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a) {
    for(int i = 0; i < D; i++) {
      if(a[i] < lower_bound || a[i] > upper_bound) {
        return false;
      }
    }
    return true;
}

// compute Euclidean distance of dimension D points
template<int D>
double distance(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d b) {
    double distance = 0;
    for(int i = 0; i < D; i++) {
      distance += pow(a[i] - b[i], 2);
    }
    return distance;
  }

// compute distance of all points in vector to another point v
template<int D>
std::vector<double> neighbors_distance(
  std::set<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Vertex_handle> neighs,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d st_proj,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d cur_n) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;
    typedef typename DT::Vertex_handle Vertex_handle;
    typedef std::set<Vertex_handle> Vertex_set;
    std::vector<double> nei_dists;
    for(typename Vertex_set::iterator neighs_it = neighs.begin();
    neighs_it != neighs.end(); neighs_it++) {
      Point n = (*neighs_it)->point();
      // remove the n on the voronoi edge from the neighbors list
      if(n == cur_n) continue;
      nei_dists.push_back(distance<D>(n, st_proj));
    }
    return nei_dists;
  }

// return dot product of two points
template<int D>
double compute_dot_product(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d b) {
    double product = 0;
    for(int i = 0; i < D; i++) {
      product += a[i] * b[i];
    }
    return product;
  }

// assign the bounding value for edge
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d assign_vals(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;
    double new_a[D];

    for(int i = 0; i < D; i++) {
      if(a[i] < lower_bound ) {
        new_a[i] = lower_bound;
      } else if (a[i] > upper_bound) {
        new_a[i] = upper_bound;
      }
    }
    Point result(&new_a[0], &new_a[D]);
    return result;
}

// return addition of point
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d point_addition(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d b) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    // std::vector<double> sum;
    double sum[D];
    // sum.reserve(D);
    for(int i = 0; i < D; i++) {
      sum[i] = a[i] + b[i];
      // sum.push_back(a[i] + b[i]);
    }
    Point result(&sum[0], &sum[D]);
    // Point result(&sum.at(0), &sum.at(sum.size() - 1));
    return result;
  }

// return multiplication of a point and a constant double
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d point_mul(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d a,
  double b) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    // std::vector<double> mul;
    // mul.reserve(D);
    double mul[D];
    for(int i = 0; i < D; i++) {
      mul[i] = a[i]*b;
      // mul.push_back(a[i]*b);
    }
    Point result(&mul[0], &mul[D]);
    // Point result(&mul.at(0), &mul.at(mul.size() - 1));
    return result;
  }

// return whether right point is larger than left point in a given direction
template<int D>
bool is_larger(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d right,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d left,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d direction) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    Point diff = point_addition<D>(right, point_mul<D>(left,  -1.0));
    // since right = left + direction * constant
    // constant is the same for all direction, we only need to check first poistion's c
    double diff_0 = diff[0];
    double direction_0 = direction[0];
    double c = diff_0 / direction_0;
    return c > 0;
  }

// return the projection of a point on half space
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d projection(
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d v,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d w,
  double c) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    // v - v*unit_w*w + c*unit_w
    double w_norm = sqrt(compute_dot_product<D>(w,w));
    Point unit_w = point_mul<D>(w, 1/w_norm);
    double proj_normal_length = compute_dot_product<D>(v, unit_w);
    Point proj_normal = point_mul<D>(w, proj_normal_length);
    Point proj_v = point_addition<D>(v, proj_normal);
    Point offset = point_mul<D>(unit_w, c);
    return point_addition<D>(proj_v, offset);
  }

// binary search get start and end of the voronoi edge
template<int D>
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d binary_search(
  std::set<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Vertex_handle> neighs,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d cur_n,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d start,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d direction,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d v,
  typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d w,
  double c,  double cur_dist, double min_nei_dist, bool reverse) {
    typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
    typedef CGAL::Delaunay_triangulation<K> DT;
    typedef typename DT::Point_d Point;

    Point original_dir = direction;
    Point left = start;
    Point right = start;
    Point right_proj;
    double mul = 2.0;
    std::vector<double> nei_dists;

    // reverse = true if we want find end point in case 1/ start/end point in case 2:
    // first point closer to other neighs than v: distance(v, proj_pt) >= min_nei_dist
    if(reverse) {
      cur_dist *= -1.0;
      min_nei_dist *= -1.0;
    }

    // find the range of the point

    // find start in case 1: first point closer to v than to other neighs
    // i.e. distance(v, proj_pt) <= min_nei_dist if reverse -> ">="
    while(check_boundary<D>(right) && cur_dist > min_nei_dist) {
      left = right;
      direction = point_mul<D>(direction, mul);
      right = point_addition<D>(right, direction);
      right_proj = projection<D>(right, w, c);
      cur_dist = reverse? -1.0 * distance<D>(right_proj, v) : distance<D>(right_proj, v);
      nei_dists = neighbors_distance<D>(neighs, right_proj, cur_n);
      min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
      min_nei_dist = reverse? -1.0 * min_nei_dist : min_nei_dist;
    }

    // get `right` inside the boundary if it's out
    // may because the step size is too large that we skip the voronoi edge
    if(!check_boundary<D>(right)) {
      if(cur_dist > min_nei_dist) {
        // case 1: cur_dist > min_nei_dist: redo it with small step size,
        right = start;
        direction = original_dir;
        while(check_boundary<D>(right) && cur_dist > min_nei_dist) {
          left = right;
          right = point_addition<D>(right, direction);
          right_proj = projection<D>(right, w, c);
          cur_dist = reverse? -1.0 * distance<D>(right_proj, v) : distance<D>(right_proj, v);
          nei_dists = neighbors_distance<D>(neighs, right_proj, cur_n);
          min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
          min_nei_dist = reverse? -1.0 * min_nei_dist : min_nei_dist;
        }
      } else {
        // case 2: cur_dist <= min_nei_dist: get all the points within bound
        right = assign_vals<D>(right);
      }
    }

    Point mid;
    Point mid_proj;
    // (right - left)/ direction > 0
    while(is_larger<D>(right, left, original_dir)) {
      Point diff = point_mul<D>(point_addition<D>(right, point_mul<D>(left, -1.0)), 1/2.0);
      // get mid point
      mid = point_addition<D>(left, diff);
      mid_proj = projection<D>(mid, w, c);
      cur_dist = reverse? -1.0 * distance<D>(mid_proj, v) : distance<D>(mid_proj, v);
      nei_dists = neighbors_distance<D>(neighs, mid_proj, cur_n);
      min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
      min_nei_dist = reverse? -1.0 * min_nei_dist : min_nei_dist;

      // if midpoint out of bound or midpoint is closer to v than all the other neighbors
      if(!check_boundary<D>(mid) || cur_dist <= min_nei_dist) {
        right = mid;
      } else {
        left = point_addition<D>(mid, original_dir);
      }
    }

    return right;
  }

template<int D>
std::vector<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d>
find_voronoi_edge(
std::set<typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Vertex_handle> neighs,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d cur_n,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d lp,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d v,
typename CGAL::Delaunay_triangulation<CGAL::Epick_d<CGAL::Dimension_tag<D>>>::Point_d w, double c) {

  typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
  typedef CGAL::Delaunay_triangulation<K> DT;
  typedef typename DT::Point_d Point;
  typedef typename DT::Vertex_handle Vertex_handle;
  typedef std::set<Vertex_handle> Vertex_set;

  std::vector<double> dummy = std::vector<double>(D + 1, 0.5);
  int end = dummy.size() - 1;

  Point start(&dummy.at(0), &dummy.at(end));

  double sum = 0;
  // projection of point onto plane
  Point st_proj = projection<D>(start, w, c);
  // calculate v's neighbors' distance
  std::vector<double> nei_dists = neighbors_distance<D>(neighs, st_proj, cur_n);

  double cur_dist = distance<D>(st_proj, v);
  double min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());

  Point start_vor;
  Point end_vor;

  // generate random direction to walk [current random seed based on time]
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::uniform_int_distribution<int> dist(-1 , 1);
  double direct = dist(generator); // -1 or 1
  direct *= step_size;

  // case 1: st_proj isn’t included in the voronoi edge
  if(cur_dist > min_nei_dist) {
    Point point_direct = point_mul<D>(lp, direct);
    Point st2 = point_addition<D>(start, point_direct);
    Point st2_proj = projection<D>(st2, w, c);
    double temp_dist = distance<D>(st2_proj, v);
    //oof, we are moving the wrong direction:
    if(temp_dist > cur_dist) {
      point_direct = point_mul<D>(point_direct, -1.0); // flip the direction
    }

    start_vor =
    binary_search<D>(neighs, cur_n, start, point_direct, v, w, c, cur_dist, min_nei_dist, false);

    Point start_vor_proj = projection<D>(start, w, c);
    double dist = distance<D>(start_vor_proj, v);
    nei_dists = neighbors_distance<D>(neighs, start_vor_proj, cur_n);
    double temp_min_nei_dist = *std::min_element(nei_dists.begin(), nei_dists.end());
    end_vor =
    binary_search<D>(neighs, cur_n, start_vor, point_direct, v, w, c, dist, temp_min_nei_dist, true);

  } else {
    Point point_direct = point_mul<D>(w, direct);
    Point another_point_direct = point_mul<D>(point_direct, -1.0); // flip the direction
    start_vor =
    binary_search<D>(neighs, cur_n, start, point_direct, v, w, c, cur_dist, min_nei_dist, true);
    end_vor =
    binary_search<D>(neighs, cur_n, start, another_point_direct, v, w, c, cur_dist, min_nei_dist, true);
  }

  std::vector<Point> result;
  result.reserve(2);
  result.push_back(start_vor);
  result.push_back(end_vor);

  return result;
}

// build LSH
template<int D>
std::vector<std::vector<std::vector<double>>> compute_LSH(std::string file_name,
std::size_t N, std::size_t num_proj, double bucket_size)
{
  typedef CGAL::Epick_d<CGAL::Dimension_tag<D>> K;
  typedef CGAL::Delaunay_triangulation<K> DT;

  typedef typename DT::Vertex Vertex;
  typedef typename DT::Vertex_handle Vertex_handle;
  typedef typename DT::Full_cell Full_cell;
  typedef typename DT::Full_cell_handle Full_cell_handle;
  typedef typename DT::Facet Facet;
  typedef typename DT::Point_d Point;
  typedef typename DT::Geom_traits::RT RT;
  typedef typename DT::Finite_full_cell_const_iterator Finite_full_cell_const_iterator;
  typedef typename DT::Finite_vertex_iterator Finite_vertex_iterator;
  typedef typename DT::Vertex_iterator Vertex_iterator;
  typedef typename DT::Face Face;
  typedef std::set<Vertex_handle> Vertex_set;
  typedef std::vector<Face> Faces;
  typedef CGAL::Random_points_in_cube_d<Point> Random_points_iterator;

  CGAL::Timer cost;  // timer
  std::vector<Point> points;

  // Generate points
  #ifdef RANDOM_PTS
  CGAL::Random rng;
  Random_points_iterator rand_it(D, 1.0, rng); // generate point within the cube with length 1
  std::copy_n(rand_it, N, std::back_inserter(points));
  #endif

  std::string full_file_name = path_prefix + file_name;

  // CSVReader reader(full_file_name);
  std::cout <<"          Reading file: " << file_name << std::endl;

  // Get the data from CSV File
  std::vector<std::vector<double> > data_vec = read_data(full_file_name, D);

  // construct points
  for(int i = 0; i < data_vec.size(); i++) {
    std::vector<double> cur = data_vec.at(i);
    double temp[cur.size()];
    std::copy(cur.begin(), cur.end(), temp);
    Point p(&temp[0], &temp[cur.size()]);
    points.push_back(p);
  }

  #ifdef READ_PTS
  for(int i = 0; i < D; i++) {
    std::cout << "Points 0[ " << i << "]"<< points.at(0)[i] << std::endl;
  }
  #endif

  cost.reset();
  cost.start();

  N = data_vec.size();
  std::cout << "Delaunay triangulation of " << N <<
    " points in dim " << D << ":" << std::endl;

  // create delaunay triangulation with dimension D
  DT dt(D);

  dt.insert(points.begin(), points.end());

  // assert the delaunay triangle is valid
  CGAL_assertion(dt.is_valid());

  // generate random line passing through (0,0), stored it in vector<Point> w
  std::default_random_engine generator;
  double cur[D] = {};
  // randomly generate pts from standard normal distribution
  std::normal_distribution<double> distribution(0.0, 1.0);
  std::vector<Point> proj_lines;

  double sum_sqr;
  for(int i= 0; i< num_proj; ++i) {
    sum_sqr = 0.0;
    for(int j = 0; j < D; j++) {
        cur[j]= distribution(generator);
        sum_sqr += pow(cur[j], 2);
    }

    // check not divide by zero, add a really small constant
    // divide by norm + e^-10
    double norm = sqrt(sum_sqr);
    if(norm == 0) norm += 1e-9;
    for(int j = 0; j < D; j++) {
        cur[j] /= norm;
    }

    Point p(&cur[0], &cur[D]);
    proj_lines.push_back(p);
    std::cout << "\n" << std::endl;
  }

  int max_bucket = 2 * std::ceil(sqrt(D) / bucket_size);
  int bound = max_bucket / 2;
  int num_edges = 0;

  // generate all the neighs for each vertices
  // calculate the #edges for all
  Vertex_iterator fvit = dt.vertices_begin();
  for (;fvit != dt.vertices_end(); fvit++) {
    if(dt.is_infinite(fvit)) continue;
    Vertex_handle curr = fvit;
    // circulate through incident full cells to get all neighbors of current vertex
    std::vector<Full_cell_handle> neigh_full_cellss;
    dt.tds().incident_full_cells(curr, back_inserter(neigh_full_cellss));
    Vertex_handle vhh;
    for(typename std::vector<Full_cell_handle>::iterator it = neigh_full_cellss.begin();
    it != neigh_full_cellss.end(); ++it ) {
        for( int i = 0; i <= dt.current_dimension(); ++i )
        {
            vhh = (*it)->vertex(i);
            if( dt.is_infinite(vhh) || vhh ==  curr)
                continue;
            num_edges++;
        }
    }
  }

  std::vector<std::vector<std::vector<double>>> buks(num_proj,
  std::vector<std::vector<double>>(max_bucket, std::vector<double>(num_edges)));

  // double buks[num_proj][max_bucket][num_edges];

  // iterate through all vertices
  fvit = dt.vertices_begin();
  for (;fvit != dt.vertices_end(); fvit++) {
    if(dt.is_infinite(fvit)) continue;
    Vertex_handle cur = fvit;
    Point v = cur->point();
    // circulate through incident full cells to get all neighbors of current vertex
    std::vector<Full_cell_handle> neigh_full_cells;
    dt.tds().incident_full_cells(cur, back_inserter(neigh_full_cells));
    Vertex_set neighs;
    Vertex_handle vh;
    for(typename std::vector<Full_cell_handle>::iterator it = neigh_full_cells.begin();
    it != neigh_full_cells.end(); ++it ) {
        for( int i = 0; i <= dt.current_dimension(); ++i )
        {
            vh = (*it)->vertex(i);
            if( dt.is_infinite(vh) || vh ==  cur)
                continue;
            neighs.insert(vh);
        }
    }

    int idx = 0;
    Point n;
    // iterate through neighbors
    for(typename Vertex_set::iterator neighs_it = neighs.begin();
    neighs_it != neighs.end(); neighs_it++) {
      n = (*neighs_it)->point();

      // find the half space
      Point w = point_addition<D>(v, point_mul<D>(n, -1.0)); // v - n

      Point half_point = point_mul<D>(point_addition<D>(n, v), 1 /2.0); // (n + v) / 2

      double product = compute_dot_product<D>(w, half_point);

      // iterate through project line
      for(std::size_t i = 0; i != proj_lines.size(); i++) {
        Point lp = proj_lines[i];
        std::vector<Point> edge = find_voronoi_edge<D>(neighs, n, lp, v, w, product);

        Point start = edge.at(0);
        Point end = edge.at(1);
        // project the start and end point to the line
        double a = compute_dot_product<D>(w, start);
        double b = compute_dot_product<D>(w, end);
        int s_idx = std::max(ceil(std::min(a,b)), lower_bound);
        int e_idx = std::min(upper_bound, ceil(std::max(a,b)));

        std::cout<< "s_idx " << "line: "<<i<< " neigh: "<<idx << " is " << s_idx << std::endl;
        std::cout<< "e_idx " << "line: "<<i<< " neigh: "<<idx << " is " << e_idx << std::endl;

        for(int s = s_idx; s != e_idx + 1; s++) {
          // buks[i][s][idx] += 1;
          buks.at(i).at(s).at(idx) += 1;
        }
      }
      // std::cout << "I am here ------------" << std::endl;
      idx++;
    }
  }

  double timing = cost.time();

  std::cout<< "Total computation time is: " << timing << std::endl;
  return buks;

}

int main(int argc, char **argv)
{
    srand(static_cast<unsigned int>(time(NULL)));

    std::vector<std::vector<std::vector<double>>> lsh_buk =
    compute_LSH<2>("data.csv", 10, 5, 0.1);

    return 0;
}
#endif

// FIXME: low dimension, 3 , put 5 - 10 points, know exactly the edges
// fix the projection line [eliminate all randomness]
// get the neighbors, check correct
// get the voronoi edges, check
// test the projection
//


#ifndef PARS_VIEWER_TRANSFORM_HPP
#define PARS_VIEWER_TRANSFORM_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace pars
{
class transform
{
public:
  explicit transform  ();
  transform           (const transform&  that) = default;
  transform           (      transform&& temp) = default;
  virtual ~transform  ()                       = default;
  transform& operator=(const transform&  that) = default;
  transform& operator=(      transform&& temp) = default;

  Eigen::Vector3f    translation       () const;
  Eigen::Quaternionf rotation          () const;
  Eigen::Vector3f    rotation_euler    () const;
  Eigen::Vector3f    scale             () const;

  Eigen::Vector3f    right             () const;
  Eigen::Vector3f    up                () const;
  Eigen::Vector3f    forward           () const;

  void               set_translation   (const Eigen::Vector3f   & translation);
  void               set_rotation      (const Eigen::Quaternionf& rotation   );
  void               set_rotation_euler(const Eigen::Vector3f   & rotation   );
  void               set_scale         (const Eigen::Vector3f   & scale      );

  void               translate         (const Eigen::Vector3f   & value      );
  void               rotate            (const Eigen::Quaternionf& value      , const bool postmultiply = false);
  void               rotate_euler      (const Eigen::Vector3f   & value      , const bool postmultiply = false);
  void               scale             (const Eigen::Vector3f   & value      );
  void               look_at           (const Eigen::Vector3f   & forward    , const Eigen::Vector3f& up = Eigen::Vector3f(0.0f, 1.0f, 0.0f));
  void               reset             ();

protected:
  Eigen::Vector3f    translation_;
  Eigen::Quaternionf rotation_   ;
  Eigen::Vector3f    scale_      ;
};
}

#endif


/*
 * Copyright (c) 2013-2019 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_MAFFINE2_HPP
#define ODE_MAFFINE2_HPP

// ODE using Affine and Mean Value Form (fast)

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/psa.hpp>
#include <kv/affine.hpp>
#include <kv/ode.hpp>
#include <kv/ode-autodif.hpp>
#include <kv/ode-param.hpp>
#include <kv/ode-callback.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
ode_onlytype1(F f, ub::vector< interval<T> >& init, const interval<T>& start, const interval<T>& end, int order) {
	int n = init.size();
	int i, j;

	ub::vector< psa< interval<T> > > x, y;
	psa< interval<T> > torg;
	psa< interval<T> > t;

	ub::vector< interval<T> > result;

	interval<T> deltat;

	bool save_mode, save_uh, save_rh;

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< interval<T> >::mode();
	save_uh = psa< interval<T> >::use_history();
	save_rh = psa< interval<T> >::record_history();
	psa< interval<T> >::mode() = 1;
	psa< interval<T> >::use_history() = false;
	psa< interval<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< interval<T> >::record_history() = true;
	psa< interval<T> >::history().clear();
	#endif
	for (j=0; j<order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< interval<T> >::use_history() = true;
		if (j == order - 1) psa< interval<T> >::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) y(i) = integrate(y(i));
		x = init + y;
	}

	deltat = end - start;

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), deltat);
	}

	init = result;

	psa< interval<T> >::mode() = save_mode;
	psa< interval<T> >::use_history() = save_uh;
	psa< interval<T> >::record_history() = save_rh;
}

template <class T, class F>
void
ode_onlytype1(F f, ub::vector< autodif< interval<T> > >& init, const interval<T>& start, const interval<T>& end, int order) {
	int n = init.size();
	int i, j, k;

	ub::vector< psa< autodif< interval<T> > > > x, y;
	psa< autodif< interval<T> > > torg;
	psa< autodif< interval<T> > > t;

	ub::vector< autodif< interval<T> > > result;

	interval<T> deltat;

	bool save_mode, save_uh, save_rh;

	x = init;

	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< autodif< interval<T> > >::mode();
	save_uh = psa< autodif< interval<T> > >::use_history();
	save_rh = psa< autodif< interval<T> > >::record_history();
	psa< autodif< interval<T> > >::mode() = 1;
	psa< autodif< interval<T> > >::use_history() = false;
	psa< autodif< interval<T> > >::record_history() = false;
	#if ODE_FAST == 1
	psa< autodif< interval<T> > >::record_history() = true;
	psa< autodif< interval<T> > >::history().clear();
	#endif
	for (j=0; j<order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< autodif< interval<T> > >::use_history() = true;
		if (j == order - 1) psa< autodif< interval<T> > >::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) y(i) = integrate(y(i));
		x = init + y;
	}

	deltat = end - start;

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), (autodif< interval<T> >)deltat);
	}

	init = result;

	psa< autodif< interval<T> > >::mode() = save_mode;
	psa< autodif< interval<T> > >::use_history() = save_uh;
	psa< autodif< interval<T> > >::record_history() = save_rh;
}

template <class T, class F>
int
ode_maffine2(F f, ub::vector< affine<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>(), ub::vector< psa< interval<T> > >* result_psa = NULL)
{
	int n = init.size();
	int i, j;

	ub::vector< interval<T> > c;
	ub::vector< interval<T> > fc;
	ub::vector< interval<T> > I, Idummy, I2;
	ub::vector< autodif< interval<T> > > Iad;

	ub::vector< interval<T> > result_i;
	ub::matrix< interval<T> > result_d;

	ub::vector< affine<T> > result;

	int maxnum_save;

	interval<T> deltat_n;
	ub::vector< psa< interval<T> > > psa_result;

	int r;

	interval<T> end2 = end;

	I.resize(n);
	c.resize(n);
	for (i=0; i<n; i++) {
		I(i) = to_interval(init(i));
		c(i) = mid(I(i));
	}

	Idummy = I;
	r = ode(f, Idummy, start, end2, p, &psa_result);
	if (r == 0) return 0;

	if (result_psa != NULL) {
		*result_psa = psa_result;
	}

	deltat_n = pow(end2 - start, p.order);

	I2.resize(n);
	for (i=0; i<n; i++) {
		I2(i) = psa_result(i).v(p.order) * deltat_n;
	}

	Iad = autodif< interval<T> >::init(I);
	// NOTICE: below must be autodif version
	ode_onlytype1(f, Iad, start, end2, p.order-1);

	fc = c;
	ode_onlytype1(f, fc, start, end2, p.order-1);

	autodif< interval<T> >::split(Iad, result_i, result_d);

	if (p.ep_reduce == 0) {
		maxnum_save = affine<T>::maxnum();
	}

	result = I2 + fc + prod(result_d, init - c);

	if (p.ep_reduce == 0) {
		epsilon_reduce2(result, maxnum_save);
	} else {
		epsilon_reduce(result, p.ep_reduce, p.ep_reduce_limit);
	}

	init = result;
	if (r == 1) end = end2;

	return r;
}

template <class T, class F>
int
odelong_maffine2(
	F f,
	ub::vector< affine<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	ub::vector< affine<T> > x, x1;
	interval<T> t, t1;
	int ret_ode;
	int ret_val = 0;
	bool ret_callback;

	ub::vector< psa< interval<T> > > result_tmp;

	x = init;
	t = start;
	p.set_autostep(true);

	while (1) {
		x1 = x;
		t1 = end;

		ret_ode = ode_maffine2(f, x1, t, t1, p, &result_tmp);
		if (ret_ode == 0) {
			if (ret_val == 1) {
				init = x1;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << to_interval(x1) << "\n";
		}

		ret_callback = callback(t, t1, to_interval(x), to_interval(x1), result_tmp);

		if (ret_callback == false) {
			init = x1;
			end = t1;
			return 3;
		}

		if (ret_ode == 2) {
			init = x1;
			return 2;
		}

		t = t1;
		x = x1;
	}
}

template <class T, class F>
int
odelong_maffine2(
	F f,
	ub::vector< interval<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i;
	ub::vector< affine<T> > x;
	int maxnum_save;
	int r;

	maxnum_save = affine<T>::maxnum();
	affine<T>::maxnum() = 0;
	x = init;

	r = odelong_maffine2(f, x, start, end, p, callback);

	affine<T>::maxnum() = maxnum_save;

	if (r == 0) return 0;

	for (i=0; i<s; i++) init(i) = to_interval(x(i));

	return r;
}

} // namespace kv

#endif // ODE_MAFFINE2_HPP


#include <bits/stdc++.h>
#include <alglib/graph/directed_graph.h>
#include <alglib/graph/dfs.h>

using namespace std;
using namespace alglib::graph;

int main() {
    directed_graph<int> g;

    vector<int> numbers = {2, 3, 5, 6, 7, 8, 10, 12};

    for(int nu : numbers)
        g.add_vertex(nu);

    // create an edge (a, b) if a | b for all pairs
    for(int i = 0; i < numbers.size(); i++)
        for(int j = i+1; j < numbers.size(); j++)
            if(numbers[j] % numbers[i] == 0)
                g.add_edge(numbers[i], numbers[j]);

    vector<int> dfs;
    preorder_dfs(g, back_inserter(dfs));

    for(int no : dfs)
        cout << no << "\t";
    cout << endl;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2003 Ferdinando Ametrano
Copyright (C) 2007, 2008 Klaus Spanderen
Copyright (C) 2007 Neil Firth
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef cl_adjoint_matrices_hpp
#define cl_adjoint_matrices_hpp
#pragma once

#include <boost/test/unit_test.hpp>
#include <ql/math/matrix.hpp>

class AdjointMatricesTest{
public:
    static bool testInverse();
    static bool testDeterminant();
    static bool testSqrt();
    static bool testPolynom();
    static bool testEigenvectors();
    //static bool testHighamSqrt();
    //static bool testSVD();
    //static bool testQRDecomposition();
    //static bool testQRSolve();
    static bool testOrthogonalProjection();
    static boost::unit_test_framework::test_suite* suite();
};

#endif


#pragma once

#include <boost/mp11.hpp>

namespace xzr
{
namespace utility
{
namespace impl
{
template <typename T,
          T Begin,
          T Steps,
          bool Increase,
          T Delta = T(1),
          typename = boost::mp11::make_integer_sequence<T, Steps>>
struct generate_range;

template <typename T, T B, T S, T D, T... Ns>
struct generate_range<T, B, S, true, D, boost::mp11::integer_sequence<T, Ns...>>
{
    using type = boost::mp11::integer_sequence<T, B + D * Ns...>;
};

template <typename T, T B, T S, T D, T... Ns>
struct generate_range<T, B, S, false, D, boost::mp11::integer_sequence<T, Ns...>>
{
    using type = boost::mp11::integer_sequence<T, B - D * Ns...>;
};
} // namespace impl

template <typename T, T N, T M>
using make_integer_range = typename impl::generate_range<T, N, (N <= M) ? (M - N) : (N - M), (N <= M)>::type;

// see https://github.com/taocpp/sequences
template <std::size_t N, std::size_t M>
using make_index_range = make_integer_range<std::size_t, N, M>;
} // namespace utility
} // namespace xzr


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

#ifndef GaussianProcess_hpp
#define GaussianProcess_hpp

#include <stdio.h>
#include <iostream>
#include <vector>
#include <memory>
#include <complex>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

#include "KernelFunction.hpp"
#include "MathUtils.hpp"

namespace loc{

    class GaussianProcessParameterSet{
    public:
        std::vector<double> sigmaFs{1,2,3,5};
        std::vector<double> lengthes{1,2,3,4,5,7,9};
        std::vector<double> lengthFloors{0.01};
        std::vector<double> sigmaNs{1};
    };

    class GaussianProcessParameters{
    public:
        GaussianKernel::Parameters gaussianKernelParameters;
        double sigmaN;
    };

    class GaussianProcess{

    private:
        // variables to be serialized
        ////std::shared_ptr<KernelFunction> mKernel;
        GaussianKernel mGaussianKernel;
        Eigen::MatrixXd X_;
        Eigen::MatrixXd Weights_;
        double sigmaN_ = 1.0;

        // variables not to be serialized
        Eigen::MatrixXd Y_;
        Eigen::MatrixXd K_;
        Eigen::MatrixXd Ky_;
        Eigen::MatrixXd invKy_;
        Eigen::MatrixXd Actives_;
        GaussianProcessParameterSet mParameterSet;

    public:
        // A function for serealization
        template<class Archive>
        void serialize(Archive& ar);

        virtual GaussianProcess& sigmaN(double sigmaN);
        virtual double sigmaN() const;
        /*
        GaussianProcess& kernel(std::shared_ptr<KernelFunction> kernel){
            mKernel = kernel;
            return *this;
        }
        */
        virtual GaussianProcess& gaussianProcessParameterSet(const GaussianProcessParameterSet&);
        virtual GaussianProcess& gaussianKernel(GaussianKernel gaussianKernel);
        virtual GaussianKernel gaussianKernel() const;

        virtual Eigen::MatrixXd X() const;
        virtual Eigen::MatrixXd Y() const;
        virtual GaussianProcess& fit(const Eigen::MatrixXd & X, const Eigen::MatrixXd& Y);
        virtual GaussianProcess& fit(const Eigen::MatrixXd & X, const Eigen::MatrixXd& Y, const Eigen::MatrixXd& Actives);
        virtual GaussianProcess& actives(const Eigen::MatrixXd& Actives);

        virtual Eigen::MatrixXd computeKernelMatrix(const Eigen::MatrixXd& X);
        virtual Eigen::VectorXd computeKstar(double x[]) const;

        virtual Eigen::VectorXd predict(double x[]) const;
        virtual Eigen::VectorXd predict(const Eigen::VectorXd& kstar) const;

        virtual double predict(double x[], int index);
        virtual std::vector<double> predict(double x[], const std::vector<int>& indices) const;
        virtual std::vector<double> predict(const Eigen::VectorXd& kstar, const std::vector<int>& indices) const;
        virtual Eigen::VectorXd predictVarianceF(double x[]) const;
        virtual Eigen::VectorXd predictVarianceF(const Eigen::VectorXd& kstar) const;

        virtual double computeLogLikelihood(double x[], const Eigen::VectorXd& y) const;
        virtual double marginalLogLikelihood();
        virtual double predictiveLogLikelihood();
        virtual double leaveOneOutMSE();

        virtual std::vector<GaussianProcessParameters> createParameterMatrix(const GaussianProcessParameterSet&) const;
        virtual void fitCV(const Eigen::MatrixXd & X, const Eigen::MatrixXd& Y, const Eigen::MatrixXd& Actives);
    };
}

#endif /* GaussianProcess_hpp */


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include "zabr.hpp"
#include "utilities.hpp"

#include <boost/assign/list_of.hpp>

#include <ql/termstructures/volatility/sabrsmilesection.hpp>
#include <ql/experimental/volatility/zabrsmilesection.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

void ZabrTest::testConsistency() {

    Real tol = 1E-4;

    Real alpha = 0.08;
    Real beta = 0.70;
    Real nu = 0.20;
    Real rho = -0.30;
    Real tau = 5.0;
    Real forward = 0.03;

    SabrSmileSection sabr(tau, forward,
                          boost::assign::list_of(alpha)(beta)(nu)(rho));

    ZabrSmileSection<ZabrShortMaturityLognormal> zabr0(
        tau, forward, boost::assign::list_of(alpha)(beta)(nu)(rho)(1.0));

    ZabrSmileSection<ZabrShortMaturityNormal> zabr1(
        tau, forward, boost::assign::list_of(alpha)(beta)(nu)(rho)(1.0));

    ZabrSmileSection<ZabrLocalVolatility> zabr2(
        tau, forward, boost::assign::list_of(alpha)(beta)(nu)(rho)(1.0));

    // for full finite prices reduce the number of intermediate points here
    // below the recommended value to speed up the test
    ZabrSmileSection<ZabrFullFd> zabr3(
        tau, forward, boost::assign::list_of(alpha)(beta)(nu)(rho)(1.0),
        std::vector<Real>(), 2);

    Real k = 0.0001;
    while (k <= 0.70) {
        Real c0 = sabr.optionPrice(k);
        Real z0 = zabr0.optionPrice(k);
        Real z1 = zabr1.optionPrice(k);
        Real z2 = zabr2.optionPrice(k);
        Real z3 = zabr3.optionPrice(k);
        if (std::fabs(z0 - c0) > tol)
            BOOST_ERROR("Zabr short maturity lognormal expansion price "
                          "("
                          << z0 << ") deviates from Sabr Hagan 2002 price "
                                   "by " << (z0 - c0));
        if (std::fabs(z1 - c0) > tol)
            BOOST_ERROR("Zabr short maturity normal expansion price "
                          "("
                          << z1 << ") deviates from Sabr Hagan 2002 price "
                                   "by " << (z1 - c0));
        if (std::fabs(z2 - c0) > tol)
            BOOST_ERROR("Zabr local volatility price "
                          "("
                          << z2 << ") deviates from Sabr Hagan 2002 price "
                                   "by " << (z2 - c0));
        if (std::fabs(z3 - c0) > tol)
            BOOST_ERROR("Zabr full finite difference price "
                          "("
                          << z3 << ") deviates from Sabr Hagan 2002 price "
                                   "by " << (z3 - c0));
        k += 0.0001;
    }
}

test_suite *ZabrTest::suite() {
    test_suite *suite = BOOST_TEST_SUITE("NoArbSabrModel tests");
    suite->add(QUANTLIB_TEST_CASE(&ZabrTest::testConsistency));
    return suite;
}


// 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 <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "utils.hpp"

using namespace boost::numeric::ublas;

static const double TOL(1.0e-5); ///< Used for comparing two real numbers.

BOOST_UBLAS_TEST_DEF ( test_double_scaled_norm_2 ) {
    vector<double> v(2);
    v[0] = 0; v[1] = 1.0e155;

    const double expected = 1.0e155;

    BOOST_UBLAS_DEBUG_TRACE( "norm is " << norm_2(v) );
    BOOST_UBLAS_TEST_CHECK(std::abs(norm_2(v) - expected) < TOL);
}

BOOST_UBLAS_TEST_DEF ( test_float_scaled_norm_2 ) {
    vector<float> v(2);
    v[0] = 0; v[1] = 1.0e20;

    const float expected = 1.0e20;

    BOOST_UBLAS_DEBUG_TRACE( "norm is " << norm_2(v) );
    BOOST_UBLAS_TEST_CHECK(std::abs(norm_2(v) - expected) < TOL);
}

int main() {
    BOOST_UBLAS_TEST_BEGIN();

    BOOST_UBLAS_TEST_DO( test_double_scaled_norm_2 );
    BOOST_UBLAS_TEST_DO( test_float_scaled_norm_2 );

    BOOST_UBLAS_TEST_END();
}


#include "utils/data_generator.cpp"
#include "../src/numerical/gauss-newton/gn_solver.h"
#include "../src/logging/easylogging++.h"
#include <armadillo>

using arma::mat;

INITIALIZE_EASYLOGGINGPP

mat WEIGHTS = {100, -20, 50, -0.5};

arma::mat quadratic_model(arma::mat x, arma::mat L)
{
    return x * arma::pow(L, 2);
}

int main(int argc, char *argv[])
{
    el::Configurations conf("./logging-config.conf");
    el::Loggers::reconfigureLogger("default", conf);

    auto data_generator = DataGenerator();
    auto L = data_generator.generate_library();
    auto s = data_generator.generate_signal(WEIGHTS);
    auto s_quadratic = data_generator.generate_signal(WEIGHTS, L, quadratic_model);

    LOG(INFO) << "True: " << WEIGHTS;

    GNSolver gn_solver = GNSolver(L);
    arma::mat result1 = gn_solver.solve(s);
    LOG(INFO) << "GN fit: " << result1;

    GNSolver gn_solver_quadratic = GNSolver(L);
    gn_solver_quadratic.set_model(quadratic_model);
    arma::mat result2 = gn_solver_quadratic.solve(s_quadratic);
    LOG(INFO) << "GN quadratic fit: " << result2;

    return 0;
}


/*************************************************************************
	> File Name: main.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Thu Mar  7 19:39:14 2019
 ************************************************************************/

#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <array>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Eigen>
#include "csv_reader.h"
#include "motion_model.h"
#include "trajectory_optimizer.h"

#define L 1.0
#define DS 0.1
#define CONST_V 3.0  // use a const linear velocity here
typedef std::vector<std::vector<float>> Table;

using namespace cpprobotics;

StateList sample_states(std::vector<float> angle_samples,
                        float a_min, float a_max,
                        int d, float p_max, float p_min, int nh){
  StateList states;
  for(float item:angle_samples){
    float a = a_min + ( a_max - a_min ) * item;
    for(int j=0; j<nh; j++){
      float xf = d * std::cos(a);
      float yf = d * std::sin(a);
      float yawf;
      if(nh == 1) yawf = (p_max - p_min)/2.0 + a;
      else yawf = p_min + (p_max - p_min) * j /(nh-1) + a;
      states.push_back(TrajState(xf, yf, yawf));
    }
  }
  return states;
};

StateList calc_uniform_polar_states(int nxy, int nh, int d,
                                    float a_min, float a_max,
                                    float p_min, float p_max){
  std::vector<float> angle_samples;
  for(int i=0; i<nxy; i++){
    angle_samples.push_back(i*1.0/(nxy-1));
  }
  StateList states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh);
  return states;
};

StateList calc_biased_polar_states(float goal_angle, int ns, int nxy,
                                   int nh, int d,
                                   float a_min, float a_max,
                                   float p_min, float p_max){
  std::vector<float> asi;
  std::vector<float> cnav;
  float cnav_max = std::numeric_limits<float>::min();
  float cnav_sum = 0;
  for(int i=0; i<ns-1; i++){
    float asi_sample = a_min + (a_max - a_min)*i/(ns-1);
    asi.push_back(asi_sample);
    float cnav_sample = M_PI - std::abs(asi_sample - goal_angle);
    cnav.push_back(cnav_sample);
    cnav_sum += cnav_sample;
    if (cnav_max < cnav_sample){
      cnav_max = cnav_sample;
    }
  }

  std::vector<float> csumnav;
  float cum_temp = 0;
  for(int i=0; i<ns-1; i++){
    cnav[i] = (cnav_max - cnav[i]) / (cnav_max * ns - cnav_sum);
    cum_temp += cnav[i];
    csumnav.push_back(cum_temp);
  }

  int li = 0;
  std::vector<float> angle_samples;
  for(int i=0; i<nxy; i++){
    for(int j=li; j<ns-1; j++){
      if (j*1.0/ns >= i*1.0/(nxy -1)){
        angle_samples.push_back(csumnav[j]);
        li = j - 1;
        break;
      }
    }
  }

  StateList states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh);
  return states;
};

StateList calc_lane_states(float l_center, float l_heading, float l_width, float v_width, float d, int nxy){
  float xc = std::cos(l_heading) * d + std::sin(l_heading) * l_center;
  float yc = std::sin(l_heading) * d + std::cos(l_heading) * l_center;

  StateList states;
  for(int i=0; i<nxy; i++){
    float delta = -0.5 * (l_width - v_width) + (l_width - v_width) * i / (nxy -1);
    float xf = xc - delta * std::sin(l_heading);
    float yf = yc + delta * std::cos(l_heading);
    states.push_back(TrajState(xf, yf, l_heading));
  }
  return states;
}

Parameter search_nearest_one_from_lookuptable(TrajState target, Table csv_file){

    float min_d = std::numeric_limits<float>::max();
    int min_id = -1;

    for(unsigned int i=0; i<csv_file.size(); i++)
    {
      float dx = target.x - csv_file[i][0];
      float dy = target.y - csv_file[i][1];
      float dyaw = target.yaw - csv_file[i][2];
      float d = std::sqrt(dx * dx + dy * dy + dyaw * dyaw);

      if ( d<min_d ){
        min_id = i;
        min_d = d;
      }
    }
    Parameter best_p(std::sqrt(target.x * target.x + target.y * target.y),
        {{0, csv_file[min_id][4], csv_file[min_id][5]}});
    return best_p;
}

std::vector<Traj> generate_path(StateList states, Table csv_file, float k0=0.0){
  std::vector<Traj> traj_list;
  for(TrajState state:states){
    Parameter   p = search_nearest_one_from_lookuptable(state, csv_file);
    p.steering_sequence[0] = k0;

    // default settings for this scenario
    State init_state(0, 0, 0, CONST_V);
    MotionModel m_model(L, DS, init_state);
    float cost_th_ = 0.1;
    std::vector<float> h_step_{0.5, 0.02, 0.02};
    int max_iter = 100;

    TrajectoryOptimizer traj_opti_obj(m_model, p, state);
    Traj traj = traj_opti_obj.optimizer_traj(max_iter, cost_th_, h_step_, true, true);
    traj_list.push_back(traj);
  }
  return traj_list;
};

std::vector<Traj> uniform_terminal_state_sample_test(Table csv_file){
  float k0 = 0.0;
  int nxy = 5;  // number of position sampling
  int nh = 3;  // number of heading sampling
  int d = 20; // distance to target
  float a_min = -45.0/180 * M_PI; // position sampling min angle
  float a_max = +45.0/180 * M_PI; // position sampling max angle
  float p_min = -45.0/180 * M_PI; // heading sampling min angle
  float p_max = +45.0/180 * M_PI; // heading sampling max angle

  StateList states = calc_uniform_polar_states(nxy, nh, d,
                                               a_min, a_max,
                                               p_min, p_max);

  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  return traj_list;
};

std::vector<Traj> biased_terminal_state_sample_test(Table csv_file){
  float k0 = 0.0;
  int nxy = 30;  // number of position sampling
  int nh = 2;  // number of heading sampling
  int d = 20; // distance to target
  float a_min = -45.0/180 * M_PI; // position sampling min angle
  float a_max = +45.0/180 * M_PI; // position sampling max angle
  float p_min = -20.0/180 * M_PI; // heading sampling min angle
  float p_max = +20.0/180 * M_PI; // heading sampling max angle

  int ns = 100;
  float goal_angle = 0.0;
  StateList states = calc_biased_polar_states(goal_angle, ns,
                                              nxy, nh, d,
                                              a_min, a_max,
                                              p_min, p_max);

  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  return traj_list;
};

std::vector<Traj> lane_state_sample_test(Table csv_file){
  float k0 = 0.0;
  float l_center = 10.0;
  float l_heading = 90.0/180.0 * M_PI;
  float l_width = 3.0;
  float v_width = 1.0;
  int d = 10;
  int nxy = 5;

  StateList states = calc_lane_states(l_center, l_heading, l_width,
                                      v_width, d, nxy);

  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  return traj_list;
};

int main(){
  //uniform_terminal_state_sample_test1();
    std::vector<std::vector<float>> lookup_table;

    std::ifstream file("../../lookuptable.csv");
    CSVIterator loop(file);
    loop++;
    for(; loop != CSVIterator(); ++loop)
    {
      std::vector<float> temp;
      for(int i=0; i<6; i++){
        temp.push_back(std::stod((*loop)[i]));
      }
      lookup_table.push_back(temp);
    }
    std::vector<Traj> traj_list1 = uniform_terminal_state_sample_test(lookup_table);
    std::vector<Traj> traj_list2 = biased_terminal_state_sample_test(lookup_table);
    std::vector<Traj> traj_list3 = lane_state_sample_test(lookup_table);

};


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   DependencyTest.hpp
 * Author: Jordan McManus
 */

#ifndef DEPENDENCYTEST_HPP
#define DEPENDENCYTEST_HPP

#include "catch.hpp"
#include "opencv2/opencv.hpp"
#include <Eigen>

TEST_CASE("Test Eigen") {
    Eigen::Vector3d v(1.0, 2.0, 3.0);

    double eps = 1e-9;
    REQUIRE(std::abs(v(0) - 1.0) < eps);
    REQUIRE(std::abs(v(1) - 2.0) < eps);
    REQUIRE(std::abs(v(2) - 3.0) < eps);

}

TEST_CASE("Test opencv") {

    cv::Mat * I = new cv::Mat();

    REQUIRE(I);

}

#endif /* DEPENDENCYTEST_HPP */


//  To use the simple FFT implementation
//  g++ -o demofft -I.. -Wall -O3 FFT.cpp

//  To use the FFTW implementation
//  g++ -o demofft -I.. -DUSE_FFTW -Wall -O3 FFT.cpp -lfftw3 -lfftw3f -lfftw3l

#ifdef USE_FFTW
#include <fftw3.h>
#endif

#include <vector>
#include <complex>
#include <algorithm>
#include <iterator>
#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/FFT>

using namespace std;
using namespace Eigen;

template <typename T> T mag2(T a) { return a * a; }
template <typename T> T mag2(std::complex<T> a) { return norm(a); }

template <typename T> T mag2(const std::vector<T> &vec) {
    T out = 0;
    for (size_t k = 0; k < vec.size(); ++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T> T mag2(const std::vector<std::complex<T>> &vec) {
    T out = 0;
    for (size_t k = 0; k < vec.size(); ++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T>
vector<T> operator-(const vector<T> &a, const vector<T> &b) {
    vector<T> c(a);
    for (size_t k = 0; k < b.size(); ++k)
        c[k] -= b[k];
    return c;
}

template <typename T> void RandomFill(std::vector<T> &vec) {
    for (size_t k = 0; k < vec.size(); ++k)
        vec[k] = T(rand()) / T(RAND_MAX) - .5;
}

template <typename T> void RandomFill(std::vector<std::complex<T>> &vec) {
    for (size_t k = 0; k < vec.size(); ++k)
        vec[k] = std::complex<T>(T(rand()) / T(RAND_MAX) - .5,
                                 T(rand()) / T(RAND_MAX) - .5);
}

template <typename T_time, typename T_freq> void fwd_inv(size_t nfft) {
    typedef typename NumTraits<T_freq>::Real Scalar;
    vector<T_time> timebuf(nfft);
    RandomFill(timebuf);

    vector<T_freq> freqbuf;
    static FFT<Scalar> fft;
    fft.fwd(freqbuf, timebuf);

    vector<T_time> timebuf2;
    fft.inv(timebuf2, freqbuf);

    long double rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
    cout << "roundtrip rmse: " << rmse << endl;
}

template <typename T_scalar> void two_demos(int nfft) {
    cout << "     scalar ";
    fwd_inv<T_scalar, std::complex<T_scalar>>(nfft);
    cout << "    complex ";
    fwd_inv<std::complex<T_scalar>, std::complex<T_scalar>>(nfft);
}

void demo_all_types(int nfft) {
    cout << "nfft=" << nfft << endl;
    cout << "   float" << endl;
    two_demos<float>(nfft);
    cout << "   double" << endl;
    two_demos<double>(nfft);
    cout << "   long double" << endl;
    two_demos<long double>(nfft);
}

int main() {
    demo_all_types(2 * 3 * 4 * 5 * 7);
    demo_all_types(2 * 9 * 16 * 25);
    demo_all_types(1024);
    return 0;
}


// Copyright 2019 Xanadu Quantum Technologies Inc.

// 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.
/**
 * @file
 * Contains functions for computing the Torontonian using the algorithm described in
 * *A faster hafnian formula for complex matrices and its benchmarking
 * on the Titan supercomputer*, [arxiv:1805.12498](https://arxiv.org/abs/1805.12498)
 */
#pragma once
#include <stdafx.h>
#include <numeric>

#ifdef LAPACKE
#define EIGEN_SUPERLU_SUPPORT
#define EIGEN_USE_BLAS
#define EIGEN_USE_LAPACKE

#define LAPACK_COMPLEX_CUSTOM
#define lapack_complex_float std::complex<float>
#define lapack_complex_double std::complex<double>
#endif

#include <Eigen/Eigenvalues>
#include "fsum.hpp"

namespace libwalrus {
/**
 * Given a string of length `len`, finds the positions in which it has a 1
 * and stores its position i, as 2*i and 2*i+1 in consecutive slots
 * of the array pos.
 *
 * It also returns (twice) the number of ones in array dst
 *
 * @param dst character array representing binary digits.
 * @param len length of the array `dst`.
 * @param pos resulting character array of length `2*len` storing
 * the indices at which `dst` contains the values 1.
 * @return returns twice the number of ones in array `dst`.
 */
void find2T (char *dst, Byte len, Byte *pos, char offset)
{
    Byte j = offset - 1;

    for (Byte i = 0; i < len; i++) {
        if (1 == dst[i]) {
            pos[j] = len - i - 1;
            pos[j + offset] = 2 * len - i - 1;
            j--;
        }
    }
}

/**
 * Partial sum of a character array
 *
 * @param dst character array
 * @param m sum the first m characters
 *
 * @return the partial sum
 */
char sum(char *dst, Byte m) {
    char sum_tot = 0;
    for (int i = 0; i < m; i++) {
        sum_tot += (Byte)dst[i];
    }
    return sum_tot;
}

/**
 * Computes the Torontonian of an input matrix.
 *
 * If the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * This function uses OpenMP (if available) to parallelize the reduction.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
template <typename T>
inline T torontonian(std::vector<T> &mat) {
    int n = std::sqrt(static_cast<double>(mat.size()));
    Byte m = n / 2;
    unsigned long long int x = static_cast<unsigned long long int>(pow(2, m));

    namespace eg = Eigen;
    eg::Matrix<T, eg::Dynamic, eg::Dynamic> A = eg::Map<eg::Matrix<T, eg::Dynamic, eg::Dynamic>, eg::Unaligned>(mat.data(), n, n);

#ifdef _OPENMP
    int nthreads = omp_get_max_threads();
    omp_set_num_threads(nthreads);
#else
    int nthreads = 1;
#endif

    std::vector<unsigned long long int> threadbound_low(nthreads);
    std::vector<unsigned long long int> threadbound_hi(nthreads);

    for (int i = 0; i < nthreads; i++) {

        threadbound_low[i] = i * x / nthreads;
        threadbound_hi[i] = (i + 1) * x / nthreads;
    }

    std::vector<T> localsum(nthreads);

    #pragma omp parallel for shared(localsum)

    for (int ii = 0; ii < nthreads; ii++) {

        T netsum = static_cast<T>(0.0);
        for (unsigned long long int k = threadbound_low[ii]; k < threadbound_hi[ii]; k++) {

            unsigned long long int xx = k;
            char* dst = new char[m];

            dec2bin(dst, xx, m);
            char len = sum(dst, m);

            Byte* short_st = new Byte[2 * len];
            find2T(dst, m, short_st, len);
            delete [] dst;

            eg::Matrix<T, eg::Dynamic, eg::Dynamic> B;
            B.resize(2 * len, 2 * len);

            for (int i = 0; i < 2 * len; i++) {
                for (int j = 0; j < 2 * len; j++) {
                    B(i, j) = -A(short_st[i], short_st[j]);
                }
            }

            delete [] short_st;

            for (int i = 0; i < 2 * len; i++) {
                B(i, i) += static_cast<T>(1);
            }

            T det = std::real(B.determinant());

            if (len % 2 == 0) {
                netsum += static_cast<T>(1.0) / std::sqrt(det);
            }
            else {
                netsum -= static_cast<T>(1.0) / std::sqrt(det);
            }

        }

        localsum[ii] = netsum;

    }

    int n_local = localsum.size();
    T final = 0.0;
    T sign = 1.0;

    if (m % 2 != 0)
        sign = -1.0;

    for (int i = 0; i < n_local; i++) {
        final += localsum[i]    ;
    }

    return sign * final;
}

/**
 * Computes the Torontonian of an input matrix using the
 * [Shewchuck algorithm](https://github.com/achan001/fsum),
 * a significantly more [accurate summation algorithm](https://link.springer.com/article/10.1007%2FPL00009321).
 *
 * Note that the fsum implementation currently only allows for
 * double precision, and precludes use of OpenMP parallelization.
 *
 * Note: if the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
template <typename T>
inline double torontonian_fsum(std::vector<T> &mat) {
    // Here weinput the matrix from python. The variable n is the size of the matrix
    int n = std::sqrt(static_cast<double>(mat.size()));
    Byte m = n / 2;
    unsigned long long int x = static_cast<unsigned long long int>(pow(2, m));

    fsum::sc_partials netsum;

    namespace eg = Eigen;
    eg::Matrix<T, eg::Dynamic, eg::Dynamic> A = eg::Map<eg::Matrix<T, eg::Dynamic, eg::Dynamic>, eg::Unaligned>(mat.data(), n, n);

    for (int k = 0; k < x; k++) {
        unsigned long long int xx = k;
        char* dst = new char[m];

        dec2bin(dst, xx, m);
        char len = sum(dst, m);

        Byte* short_st = new Byte[2 * len];
        find2T(dst, m, short_st, len);
        delete [] dst;

        // eg::Matrix<double,eg::Dynamic,eg::Dynamic> B(2*len, 2*len, 0.);
        eg::Matrix<T, eg::Dynamic, eg::Dynamic> B;
        B.resize(2 * len, 2 * len);

        for (int i = 0; i < 2 * len; i++) {
            for (int j = 0; j < 2 * len; j++) {
                B(i, j) = -A(short_st[i], short_st[j]);
            }
        }

        delete [] short_st;

        for (int i = 0; i < 2 * len; i++) {
            B(i, i) += 1;
        }

        long double det = std::real(B.determinant());

        if (len % 2 == 0) {
            netsum += 1.0 / std::sqrt(det);
        }
        else {
            netsum += -1.0 / std::sqrt(det);
        }
    }

    double sign = 1.0;

    if (m % 2 != 0)
        sign = -1.0;

    return static_cast<double>(netsum) * static_cast<double>(sign);
}

/**
 * Computes the Torontonian of an input matrix.
 *
 * If the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * This is a wrapper around the templated function `libwalrus::torontonian` for Python
 * integration. It accepts and returns complex double numeric types, and
 * returns sensible values for empty and non-even matrices.
 *
 * In addition, this wrapper function automatically casts all matrices
 * to type `complex<long double>`, allowing for greater precision than supported
 * by Python and NumPy.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
std::complex<double> torontonian_quad(std::vector<std::complex<double>> &mat) {
    std::vector<std::complex<long double>> matq(mat.begin(), mat.end());
    std::complex<long double> tor = torontonian(matq);
    return static_cast<std::complex<double>>(tor);
}

/**
 * Computes the Torontonian of an input matrix.
 *
 * If the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * This is a wrapper around the templated function `libwalrus::torontonian` for Python
 * integration. It accepts and returns double numeric types, and
 * returns sensible values for empty and non-even matrices.
 *
 * In addition, this wrapper function automatically casts all matrices
 * to type `long double`, allowing for greater precision than supported
 * by Python and NumPy.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
double torontonian_quad(std::vector<double> &mat) {
    std::vector<long double> matq(mat.begin(), mat.end());
    long double tor = torontonian(matq);
    return static_cast<double>(tor);
}

}


#include <cmath>
#include "conex/debug_macros.h"
#include "conex/divergence.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;

TEST(MuSelection, DivergenceBound) {
  int n = 3;
  MatrixXd gw = Eigen::VectorXd::Random(n, 1).array().abs();

  double gw_norm_squared = gw.squaredNorm();
  double gw_norm_inf = gw.maxCoeff();
  double gw_trace = gw.sum();
  double hub_desired = 1;
  double k = DivergenceUpperBoundInverse(hub_desired, gw_norm_squared,
                                         gw_norm_inf, gw_trace, n);
  double hub =
      DivergenceUpperBound(k, gw_norm_squared, gw_norm_inf, gw_trace, n);

  EXPECT_TRUE(k >= 0);
  EXPECT_TRUE(2 - k * gw_norm_inf >= 0);
  EXPECT_NEAR(hub, hub_desired, 1e-12);
}

}  // namespace conex


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_axis_angle);

BOOST_AUTO_TEST_CASE(simple_test)
{

  typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;

  typedef math_types::real_type        T;
  typedef math_types::vector3_type     V;
  typedef math_types::quaternion_type  Q;

  // Small positive angle
  {
    Q q;
    T const phi = 0.01;
    V const m = unit( V( 1.0, 1.0, 1.0 ) );
    q.Ru( phi, m);

    T theta;
    V n;
    OpenTissue::math::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }
  // See what happens if axis is flipped
  {
    Q q;
    T const phi = 0.01;
    V const m = -unit( V( 1.0, 1.0, 1.0 ) );
    q.Ru( phi, m);

    T theta;
    V n;
    OpenTissue::math::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }
  // Pick larger angle
  {
    Q q;
    T const phi = 3.0;
    V const m = unit( V( 1.0, 1.0, 1.0 ) );
    q.Ru( phi, m);

    T theta;
    V n;
    OpenTissue::math::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }
  // Pick larger negative angle, the positive angle version representation should be returned!
  {
    Q q;
    T const phi = -3.0;
    V const m = unit( V( 1.0, 1.0, 1.0 ) );
    q.Ru( phi, m);

    T theta;
    V n;
    OpenTissue::math::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( -phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( -m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( -m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( -m(2), n(2), 0.01 );
  }
  // Flip the axis
  {
    Q q;
    T const phi = -3.0;
    V const m = -unit( V( 1.0, 1.0, 1.0 ) );
    q.Ru( phi, m);

    T theta;
    V n;
    OpenTissue::math::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( -phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( -m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( -m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( -m(2), n(2), 0.01 );
  }
  // Large positive angle
  {
    Q q;
    T const phi = 6.0;
    V const m = unit( V( 1.0, 1.0, 1.0 ) );
    q.Ru( phi, m);

    T theta;
    V n;
    OpenTissue::math::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef _LDAPLUSPLUS_OPTIMIZATION_MULTINOMIAL_LOGISTIC_REGRESSION
#define _LDAPLUSPLUS_OPTIMIZATION_MULTINOMIAL_LOGISTIC_REGRESSION

#include <cmath>

#include <Eigen/Core>

namespace ldaplusplus {
namespace optimization {

/**
 * MultinomialLogisticRegression is an implementation of the multinomial
 * logistic loss function (without bias unit).
 *
 * It follows the protocol used by GradientDescent. For the specific function
 * implementations see value() and gradient().
 */
template <typename Scalar>
class MultinomialLogisticRegression
{
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;

    public:
        /**
         * @param X  The documents defining the minimization problem (\f$X \in
         *           \mathbb{R}^{D \times N}\f$)
         * @param y  The class indexes for each document (\f$y \in
         *           \mathbb{N}^N\f$)
         * @param Cy A different weight for each class in the optimization
         *           problem
         * @param L  The L2 regularization penalty for the weights
         */
        MultinomialLogisticRegression(const MatrixX &X, const Eigen::VectorXi &y, VectorX Cy, Scalar L);
        /**
         * @param X  The documents defining the minimization problem (\f$X \in
         *           \mathbb{R}^{D \times N}\f$)
         * @param y  The class indexes for each document (\f$y \in
         *           \mathbb{N}^N\f$)
         * @param L  The L2 regularization penalty for the weights
         */
        MultinomialLogisticRegression(const MatrixX &X, const Eigen::VectorXi &y, Scalar L);

        /**
         * The value of the objective function to be minimized.
         *
         * \f$N\f$ is the number of documents (different vectors), \f$X_n \in
         * \mathbb{R}^D\f$ is the nth document, \f$\eta_y \in \mathbb{R}^D\f$
         * is the weights vector for the class \f$y\f$ defining the hyperplane
         * that separates class \f$y\f$ from all the other, finally \f$y_n\f$
         * is the class of the nth document.
         *
         * \f[
         *     J = -\sum_{n=1}^N C_{y_n}\left(\eta_{y_n}^T X_n - \log\left(
         *         \sum_{\hat{y}=1}^Y \exp\left( \eta_{\hat{y}}^T X_n \right)
         *         \right)\right) +
         *         \frac{L}{2} \left\| \eta \right\|_F^2
         * \f]
         *
         * @param eta The weights of the linear model (\f$\eta \in
         *            \mathbb{R}^{D \times Y}\f$)
         */
        Scalar value(const MatrixX &eta) const;

        /**
         * The gradient of the objective function implemented in value().
         *
         * We use \f$I(y) \in \mathbb{R}^Y\f$ as the indicator vector of
         * \f$y\f$ (a vector with all the values 0 except at the yth position).
         *
         * \f[
         *     \nabla_{\eta} J = -\sum_{n=1}^N C_{y_n} \left(
         *         X_n I(y_n)^T -
         *         \frac{\sum_{\hat{y}=1}^Y X_n I(\hat{y})^T \exp(\eta_{\hat{y}}^T X_n)}
         *              {\sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T X_n)}
         *         \right) +
         *         L \eta
         * \f]
         *
         * @param eta  The weights of the linear model (\f$\eta \in
         *             \mathbb{R}^{D \times Y}\f$)
         * @param grad A matrix of dimensions equal to \f$\eta\f$ that will
         *             hold the result
         */
        void gradient(const MatrixX &eta, Eigen::Ref<MatrixX> grad) const;

    private:
        const MatrixX &X_;
        const Eigen::VectorXi &y_;
        Scalar L_;
        VectorX Cy_;
};

}  // namespace optimization
}  // namespace ldaplusplus
#endif // _LDAPLUSPLUS_OPTIMIZATION_MULTINOMIAL_LOGISTIC_REGRESSION


#ifndef CANNON_PHYSICS_SYSTEMS_KINEMATIC_CAR_H
#define CANNON_PHYSICS_SYSTEMS_KINEMATIC_CAR_H

#include <random>

#include <ompl/control/ODESolver.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>

#include <Eigen/Dense>

#include <cannon/physics/rk4_integrator.hpp>
#include <cannon/physics/systems/system.hpp>
#include <cannon/graphics/geometry/plane.hpp>
#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

namespace oc = ompl::control;
namespace ob = ompl::base;

namespace cannon {
  namespace physics {
    namespace systems {

      struct KinCarSystem : System {
        KinCarSystem(double l = 1.0) : l_(l) {}

        virtual void operator()(const VectorXd& s, VectorXd& dsdt, const double /*t*/) override {
          double th = s[2];
          double uv = s[3];
          double uth = s[4];

          dsdt.resize(5);
          dsdt[0] = uv * std::cos(th);
          dsdt[1] = uv * std::sin(th);
          dsdt[2] = (uv / l_) * std::tan(uth);
          dsdt[3] = 0.0;
          dsdt[4] = 0.0;
        }

        virtual void ompl_ode_adaptor(const oc::ODESolver::StateType& q,
            const oc::Control* control, oc::ODESolver::StateType& qdot) override {

          const double uv = control->as<oc::RealVectorControlSpace::ControlType>()->values[0];
          const double uth = control->as<oc::RealVectorControlSpace::ControlType>()->values[1];

          VectorXd s(5);
          s[0] = q[0];
          s[1] = q[1];
          s[2] = q[2];
          s[3] = uv;
          s[4] = uth;
          VectorXd dsdt(5);

          (*this)(s, dsdt, 0.0);

          qdot.resize(q.size(), 0);
          for (unsigned int i = 0; i < q.size(); i++) {
            qdot[i] = dsdt[i];
          }
        }

        virtual std::tuple<MatrixXd, MatrixXd, VectorXd> get_linearization(const VectorXd& x) override {
          MatrixXd A = MatrixXd::Identity(3, 3);
          VectorXd c = x;

          // TODO Don't hardcode timestep at some point
          double theta = x[2];
          MatrixXd B(3, 2);
          B << std::cos(theta) * 0.01, 0,
               std::sin(theta) * 0.01, 0,
               0, 0;

          return std::make_tuple(A, B, c);
        }

        virtual void
        get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                          Ref<MatrixXd> A,
                                          Ref<MatrixXd> B) override {
          // TODO
          throw std::runtime_error("Not implemented yet");
        }

        static void ompl_post_integration(const ob::State* /*state*/, const
            oc::Control* /*control*/, const double /*duration*/, ob::State *result) {

          ob::SO2StateSpace SO2;
          SO2.enforceBounds(result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1));
        }

        // Parameters
        double l_;
      };

      class KinematicCar {
        public:
          KinematicCar() = delete;

          KinematicCar(Vector3d s, Vector3d g) : e_(s_, 4, time_step), start_(s), goal_(g) {
            std::random_device rd;
            gen_ = std::mt19937(rd());

            xy_dis_ = std::uniform_real_distribution<double>(-1.0, 1.0);
            th_dis_ = std::uniform_real_distribution<double>(-M_PI, M_PI);

            state_ = VectorXd::Zero(5);
            reset();
          }

          std::pair<VectorXd, double> step(double uv, double uth) {
            double clipped_uv = std::max(-1.0, std::min(uv, 1.0));
            //double clipped_uth = std::max(-M_PI, std::min(uth, M_PI));

            state_[3] = clipped_uv;
            state_[4] = uth;

            double goal_r = -std::pow((state_.head(2) - goal_.head(2)).norm(), 2.0);
            double control_r = -std::pow((std::abs(clipped_uv) + std::abs(uth)), 2.0);
            double reward = goal_r + 0.001*control_r;

            e_.set_state(state_);
            state_ = e_.step();

            return std::make_pair(state_.head(3), reward);
          }

          VectorXd reset() {
            //state_.head(3) = start_ + Vector3d::Random() * 0.1;
            state_[0] = xy_dis_(gen_);
            state_[1] = xy_dis_(gen_);
            state_[2] = th_dis_(gen_);

            state_[3] = 0.0;
            state_[4] = 0.0;

            return state_.head(3);
          }

          VectorXd reset(const VectorXd& s) {
            state_[0] = s[0];
            state_[1] = s[1];
            state_[2] = s[2];

            state_[3] = 0.0;
            state_[4] = 0.0;

            return state_.head(3);

          }

          // In seconds
          const double time_step = 0.01;

          KinCarSystem s_;

        private:
          RK4Integrator e_;

          VectorXd state_;

          Vector3d start_;
          Vector3d goal_;

          std::mt19937 gen_;
          std::uniform_real_distribution<double> xy_dis_;
          std::uniform_real_distribution<double> th_dis_;
      };

    } // namespace physics
  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_KINEMATIC_CAR_H */


/** @file
*****************************************************************************

Implementation of a secret-key lattice-based additively homomorphic
vector encryption scheme.

See lwe.hpp

*****************************************************************************
* @author     Samir Menon, Brennan Shacklett, and David J. Wu
* @copyright  MIT license (see LICENSE file)
*****************************************************************************/

#include <cstdlib>
#include <iostream>
#include <cassert>
#include <random>
#include <cstdint>
#include <fstream>
#include <NTL/ZZ.h>

#include "lwe.hpp"
#include <libsnark/common/libsnark_serialization.hpp>

using namespace std;
namespace LWE {

static NTL::ZZ_p random(const NTL::ZZ &mod) {
    // Choose a random value from a space that is 128-bits
    // longer than the target space, and then round down.

    long num_bytes = NTL::NumBytes(mod) + 16;
    unsigned char bytes[num_bytes];
    static ifstream urandom("/dev/urandom", ios::binary);
    urandom.read(reinterpret_cast<char *>(bytes), num_bytes);

    NTL::ZZ randZZ = NTL::ZZFromBytes(bytes, num_bytes);

    return NTL::to_ZZ_p(randZZ % mod);
}

// Sample a discrete Gaussian variable using the Box-Muller
// transform.
static int32_t sample_discrete_gaussian(double stddev) {
    static const double PI = 4.0*atan(1.0);

    double r1 = ((double) rand()) / RAND_MAX;
    double r2 = ((double) rand()) / RAND_MAX;
    double theta = 2*PI*r1;

    return (int32_t) floor(stddev * sqrt(-2.0*log(r2)) * cos(theta) + 0.5);
}

ciphertext& ciphertext::operator=(const ciphertext& other) {
    NTL::ZZ_p::init(LWE::q);

    this->ctxt = other.ctxt;

    return *this;
}

ciphertext ciphertext::operator+(const ciphertext &other) const {
    ciphertext sum = *this;
    sum += other;

    return sum;
}

ciphertext& ciphertext::operator+=(const ciphertext &other) {
    NTL::ZZ_p::init(LWE::q);

    this->ctxt += other.ctxt;
    return *this;
}

ciphertext ciphertext::operator*(uint64_t val) const {
    return operator*(NTL::ZZ_p(val));
}

ciphertext ciphertext::operator*(const NTL::ZZ_p &val) const {
    ciphertext prod = *this;
    prod *= val;

    return prod;
}

ciphertext& ciphertext::operator*=(uint64_t val) {
    return operator*=(NTL::ZZ_p(val));
}

ciphertext& ciphertext::operator*=(const NTL::ZZ_p &val) {
    NTL::ZZ_p::init(LWE::q);

    this->ctxt *= val;
    return *this;
}

ciphertext operator*(uint64_t val, const ciphertext& ct) {
    return operator*(NTL::ZZ_p(val), ct);
}

ciphertext operator*(const NTL::ZZ_p &val, const ciphertext& ct) {
    ciphertext prod = ct;
    prod *= val;

    return prod;
}

secret_key keygen() {
    NTL::ZZ_p::init(LWE::q);
    secret_key sk;

    // Sampled uniformly random matrix A
    matrix A_hat(NTL::INIT_SIZE, n, n);
    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= n; j++) {
            A_hat(i, j) = random(q);
        }
    }

    // Sample secret keys from error distribution
    matrix S_hat(NTL::INIT_SIZE, n, pt_dim);
    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= pt_dim; j++) {
            S_hat(i, j) = sample_discrete_gaussian(stddev);
        }
    }

    // Sample errors from error distribution
    matrix E_hat(NTL::INIT_SIZE, pt_dim, n);
    for (size_t i = 1; i <= pt_dim; i++) {
        for (size_t j = 1; j <= n; j++) {
            E_hat(i, j) = sample_discrete_gaussian(stddev);
        }
    }

    // Construct A = [ A_hat ; S_hat^T * A_hat + p * E_hat ]
    matrix A_bottom = NTL::transpose(S_hat)*A_hat + p_int*E_hat;

    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= n; j++) {
            sk.A(i, j) = A_hat(i, j);
        }
    }

    for (size_t i = 1; i <= pt_dim; i++) {
        for (size_t j = 1; j <= n; j++) {
            sk.A(i + n, j) = A_bottom(i, j);
        }
    }

    // Construct S = [ -S_hat ; I ]
    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= pt_dim; j++) {
            sk.S(i, j) = -S_hat(i, j);
        }
    }

    matrix ident = NTL::ident_mat_ZZ_p(pt_dim);
    for (size_t i = 1; i <= pt_dim; i++) {
        for (size_t j = 1; j <= pt_dim; j++) {
            sk.S(i + n, j) = ident(i, j);
        }
    }

    return sk;
}

ciphertext encrypt(const secret_key &sk, const plaintext &pt) {
    NTL::ZZ_p::init(LWE::q);

    // Sample an LWE error vector for the randomness (n x 1)
    vector r(NTL::INIT_SIZE, n);
    for (size_t i = 1; i <= n; i++) {
        r(i) = sample_discrete_gaussian(stddev);
    }

    vector v_padded(NTL::INIT_SIZE, n + pt_dim);
    for (size_t i = 1; i <= n; i++) {
        v_padded(i) = 0;
    }

    for (size_t i = 1; i <= pt_dim; i++) {
        v_padded(i + n) = pt(i);
    }

    ciphertext ctxt;
    ctxt.ctxt = sk.A*r + v_padded;

    // Add error to each component of ciphertext
    for (size_t i = 1; i <= n + pt_dim; i++) {
        ctxt.ctxt(i) += sample_discrete_gaussian(stddev) * LWE::p_int;
    }

    return ctxt;
}

plaintext decrypt(const secret_key &sk, const ciphertext& ct) {
    NTL::ZZ_p::init(LWE::q);
    vector modqvec = NTL::transpose(sk.S)*ct.ctxt;

    NTL::ZZ_p::init(LWE::p);
    plaintext pt(NTL::INIT_SIZE, pt_dim);
    for (size_t i = 1; i <= pt_dim; i++) {
        NTL::ZZ modq = NTL::rep(modqvec(i));
        if (modq > q/2) {
            modq -= q;
        } else if (modq < -q/2) {
            modq += q;
        }
        pt(i) = ((modq % p_int) + p_int) % p_int;
    }

    return pt;
}

}


#include <iostream>
#include <cassert>

#include "libff/common/profiling.hpp"

#include <NTL/mat_ZZ_p.h>
#include <gmp.h>

#include "bgroup/bgroup.tcc"
#include "ssfe/ssfe_sm.tcc"

using namespace ssfe;

const size_t S = 128;
const size_t K = 2;

const size_t Repeat = 10;

int main() {
    Prms prms;
    Msk msk;
    Vct x;
    Vct y;
    Key sk(S);
    Cph cph(S);
    bool r;
    size_t i, j;

    bgroup_init();
    libff::start_profiling();

    for(i=0;i<Repeat;i++) {
        x.SetLength(S);
        y.SetLength(S);
        for(j=0;j<S;j++) {
            x[j]=random_ZZ_p();
            y[j]=random_ZZ_p();
            x[j+1]=x[j];
            y[j+1]=-y[j];
            j++;
        }

        setup(prms,S,K);
        keygen(msk,prms);

        libff::enter_block("Extract");
        extract(sk,y,msk,prms);
        libff::leave_block("Extract");

        libff::enter_block("Encrypt");
        encrypt(cph,x,msk,prms);
        libff::leave_block("Encrypt");

        libff::enter_block("Decrypt");
        r=decrypt(cph,sk,prms);
        libff::leave_block("Decrypt");

        //Orthogonality should be true
        assert(r != 0);

        y[0]=random_ZZ_p();
        extract(sk,y,msk,prms);
        r = decrypt(cph,sk,prms);

        //Orthogonality should be false
        assert(r == 0);
    }

    std::cout << "OK!\n";

    return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Easy calculation. Newton's Law of Gravity Tutorial,
 *          http://easycalculation.com/physics/classical-physics/learn-newtons-law.php, last
 *          accessed: 12th February, 2012.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "tudat/astro/gravitation/centralGravityModel.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_gravitational_force )

//! Test if gravitational force is computed correctly.
BOOST_AUTO_TEST_CASE( testGravitationalForce )
{
    // Case 1: Compute gravitational force exerted on boy, due to Earth (Easy calculation, 2012).
    {
        // Set gravitational parameter of Earth [m^3 s^-2].
        double gravitationalParameterOfEarth = 6.6726e-11 * 5.98e24;

        // Set position vector of Earth [m].
        Eigen::Vector3d positionOfEarth = Eigen::Vector3d::Zero( );

        // Set mass of boy [kg]
        double massOfBoy = 70.0;

        // Set position vector of boy [m].
        Eigen::Vector3d positionOfBoy( 6.38e6, 0.0, 0.0 );

        // Compute gravitational force acting on boy [N].
        Eigen::Vector3d gravitationalForceExertedOnBoy
                = gravitation::computeGravitationalForce(
                    massOfBoy, positionOfBoy,
                    gravitationalParameterOfEarth, positionOfEarth );

        // Check if computed gravitational force matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( 685.54, gravitationalForceExertedOnBoy.norm( ), 1.0e-3 );
    }

    // Case 2: Compute gravitational force exerted on arbitrary body1, due to arbitrary body2
    //         (Easy calculation, 2012).
    {
        // Set universal gravitational constant [m^3 kg^-1 s^-2].
        double universalGravitationalConstant = 6.6726e-11;

        // Set mass of body1 [kg].
        double massOfBody1 = 1.0e4;

        // Set position vector of body1 [m].
        Eigen::Vector3d positionOfBody1( 0.0, 5.0, 0.0 );

        // Set mass of body2 [kg].
        double massOfBody2 = 2.0e4;

        // Set position vector of body2 [m].
        Eigen::Vector3d positionOfBody2( 0.0, -5.0, 0.0 );

        // Compute gravitational force acting on body1 [N].
        Eigen::Vector3d gravitationalForceExertedOnBody1
                = gravitation::computeGravitationalForce(
                    universalGravitationalConstant, massOfBody1,
                    positionOfBody1, massOfBody2, positionOfBody2 );

        // Check if computed gravitational force matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( 13345200.0e-11, gravitationalForceExertedOnBody1.norm( ),
                                    std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <algorithm>
#include <fstream>
#include <iterator>
#include <map>
#include <set>
#include <sstream>
#include <string>
#include <utility>

#include "utils/profiling.h"

#include <armadillo>

using namespace util;
using namespace arma;

bool checkMat(int row, int col, mat const &mat) {
    if(mat.n_rows == row && mat.n_cols == col) return true;
    return false;
}
bool checkVec(int dim, vec const &vec) {
    if(vec.size() == dim) return true;
    return false;
}

void exit_with_error(std::string error_message) {
    std::cout << error_message << std::endl;
    exit(1);
}

int main() {

    auto timer = Timer{};
    //timer.here_then_reset("");

    int r_dim = 3; // Reduced rank r

    // TF matrix for the test.
    // n_dim by m_dim .
    int n_dim = 50;
    int m_dim = 50;

    sp_mat inM(n_dim,m_dim);
    for(int i = 0; i < n_dim; i++) {
        for(int j = 0; j < m_dim; j++) {
            inM(i,j) = n_dim*i + j + 1;
        }
    }

    mat inU;
    vec ins;
    mat inV;

    svds(inU,ins,inV,inM,r_dim);

    timer.here_then_reset("Truncated SVD is done for original data matrix.\n");

    if(checkMat(n_dim,r_dim,inU) == false) exit_with_error("U matrix is incorrect.");
    if(checkVec(r_dim,ins) == false) exit_with_error("s diagonal matrix is incorrect.");
    if(checkMat(m_dim,r_dim,inV) == false) exit_with_error("V matrix is incorrect.");

    // incremental n_dim by c_dim matrix
    int c_dim = 1;
    mat inC(n_dim,c_dim);
    for(int i = 0; i < n_dim; i++) {
        for(int j = 0; j < c_dim; j++) {
            inC(i,j) = i*i+j*(j+1)+1;
        }
    }

    if(checkMat(n_dim,c_dim,inC) == false) exit_with_error("C matrix is incorrect.");

    mat inL = trans(inU)*inC;
    if(checkMat(r_dim,c_dim,inL) == false) exit_with_error("L matrix is incorrect.");

    mat inH;
    inH = inC - inU*inL;
    if(checkMat(n_dim,c_dim,inH) == false) exit_with_error("H matrix is incorrect.");

    mat inJ;
    mat inK;

    qr(inJ,inK,inH);

    timer.here_then_reset("QR decomposition for H matrix.\n");

    if(checkMat(n_dim,n_dim,inJ) == false) exit_with_error("J matrix is incorrect.");
    if(checkMat(n_dim,c_dim,inK) == false) exit_with_error("K matrix is incorrect.");

    mat inQ(r_dim+n_dim,r_dim+c_dim,fill::zeros);

    for(int r=0;r<r_dim;r++)
        inQ(r,r) = ins(r);

    for(int i=0;i<r_dim;i++) {
        for(int j=0;j<c_dim;j++) {
            inQ(i,r_dim+j) = inL(i,j);
        }
    }

    for(int i=0;i<n_dim;i++) {
        for(int j=0;j<c_dim;j++) {
            inQ(r_dim+i,r_dim+j)=inK(i,j);
        }
    }

    if(checkMat(r_dim+n_dim,r_dim+c_dim,inQ) == false) exit_with_error("Q matrix is incorrect.");

    mat inUp;
    vec insp;
    mat inVp;

    svd(inUp,insp,inVp,inQ);
    timer.here_then_reset("SVD is done for extended Q matrix.\n");

    if(checkMat(r_dim+n_dim,r_dim+n_dim,inUp) == false) exit_with_error("Up matrix is incorrect.");
    if(checkVec(r_dim+c_dim,insp) == false) exit_with_error("sp diagonal matrix is incorrect.");
    // Assuming r_dim+c_dim <= r_dim+n_dim
    if(checkMat(r_dim+c_dim,r_dim+c_dim,inVp) == false) exit_with_error("Vp matrix is incorrect.");

    mat inUpp;
    vec inspp;
    mat inVpp;

    mat mapU;
    mapU = join_rows(inU,inJ);
    if(checkMat(n_dim,r_dim+n_dim,mapU) == false) exit_with_error("mapU matrix is incorrect.");

    mat mapV(m_dim+c_dim,r_dim+c_dim,fill::zeros);
    for(int i=0;i<m_dim;i++) {
        for(int j=0;j<r_dim;j++) {
            mapV(i,j) = inV(i,j);
        }
    }

    for(int i=0;i<c_dim;i++) {
        mapV(m_dim+i,r_dim+i) = 1;
    }
    if(checkMat(m_dim+c_dim,r_dim+c_dim,mapV) == false) exit_with_error("mapV matrix is incorrect.");

    inUpp = mapU * inUp;
    inspp = insp;
    inVpp = mapV * inVp;

    mat inspp_diag(r_dim+n_dim,r_dim+c_dim,fill::zeros);

    for(int i=0;i<r_dim+c_dim;i++) {
        inspp_diag(i,i) = insp(i);
    }

    if(checkMat(n_dim,n_dim+r_dim,inUpp) == false) exit_with_error("Upp matrix is incorrect.");
    if(checkMat(n_dim+r_dim,r_dim+c_dim,inspp_diag) == false) exit_with_error("inspp_diag matrix is incorrect.");
    if(checkMat(m_dim+c_dim,r_dim+c_dim,inVpp) == false) exit_with_error("Vpp matrix is incorrect.");

    mat resMat = inUpp*inspp_diag*trans(inVpp);
    timer.here_then_reset("Updated SVD is done.\n");

    //resMat.print("resMat = ");
    //End of updating SVD (slow way)

    //Begin of brute SVD

    timer.here_then_reset("Test for brute SVD begins. First I make the matrix.\n");

    mat inMb;

    mat inMd(inM.n_rows, inM.n_cols);
    for(int i=0;i<inMd.n_rows;i++) {
        for(int j=0;j<inMd.n_cols;j++) {
            inMd(i,j) = inM(i,j);
        }
    }

    inMb = join_rows(inMd,inC);

    sp_mat inMbb(inMb.n_rows,inMb.n_cols);
    for(int i=0;i< inMbb.n_rows;i++) {
        for(int j=0;j<inMbb.n_cols;j++) {
            inMbb(i,j) = inMb(i,j);
        }
    }

    timer.here_then_reset("Now begins brute SVD calculation.\n");

    mat inUb;
    vec insb;
    mat inVb;

    svds(inUb,insb,inVb,inMbb,r_dim+1); // For the fair comparison, I added 1 to r_dim.

    timer.here_then_reset("Brute SVD is done.\n");

    //End of brute SVD
    return 0;
}


#include <iostream>
#include <vector>
#include <random>
#include <boost/concept_check.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

using namespace std;
using namespace g2o;

class g2o_vertex: public BaseVertex<3, Eigen::Matrix<double, 1, 3>> {
	void oplusImpl(const double* v) {
		_estimate += Eigen::Matrix<double, 1, 3>(v);
	}
	void setToOriginImpl() {
		_estimate.setZero();
	}

	bool read(istream& is) {}
	bool write(ostream& os) const {}
};

class g2o_edge: public BaseUnaryEdge<1, double, g2o_vertex> {
public:
	explicit g2o_edge(double x): _x(x) {}
	void computeError() {
		g2o_vertex* v = static_cast<g2o_vertex*>(_vertices[0]);
		const Eigen::Matrix<double, 1, 3> est = v->estimate();
		_error(0, 0) = _measurement - exp(est(0,0)*_x*_x + est(0,1)*_x + est(0,2));
		//cout << "_error = " << _error(0,0) << endl;
	}

	bool read(istream& is) {}
	bool write(ostream& os) const {}
private:
	double _x;
};

//y = exp(3*x^2 + 2*x + 1)
int main(int argc, char **argv)
{
	typedef BlockSolver<BlockSolverTraits<3, 1>> block_solver;
	block_solver::LinearSolverType* linear_solver = new LinearSolverDense<block_solver::PoseMatrixType>;
	block_solver* blk_slv = new block_solver(linear_solver);

	OptimizationAlgorithmLevenberg* algorithm = new OptimizationAlgorithmLevenberg(blk_slv);
	SparseOptimizer optimizer;
	optimizer.setAlgorithm(algorithm);
	optimizer.setVerbose(true);

	//加顶点
	g2o_vertex* vertex = new g2o_vertex;
	vertex->setEstimate(Eigen::Matrix<double, 1, 3>(0,0,0));
	vertex->setId(0);
	optimizer.addVertex(vertex);

	//生成观测值
	vector<double> _x,_y;
	double x_temp;
	default_random_engine generator;
	normal_distribution<double> distribution(0.0,0.5);
	for(int i=0;i<100;i++) {
		//100 * 0.005 = 0.5,此值不能太大,过大时exp(3*x^2 + 2*x + 1)就溢出了
		x_temp = i*0.005;
		_x.push_back(x_temp);
		_y.push_back(exp(3*x_temp*x_temp + 2*x_temp + 1) + distribution(generator));
	}
	//加边
	for(int i=0;i<100;i++) {
		g2o_edge* edge = new g2o_edge(_x[i]);
		edge->setId(i);
		edge->setVertex(0, vertex);
		edge->setInformation(Eigen::Matrix<double,1,1>(1/0.25));
		edge->setMeasurement(_y[i]);
		optimizer.addEdge(edge);
	}
	//开始进行优化估计
	optimizer.initializeOptimization();
	optimizer.optimize(100);

	cout << "optimized variable: " << vertex->estimate().transpose() << endl;

	return 0;
}


#include "Day15-DuelingGenerators.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include <string>
#include <vector>
#include <cassert>
__END_LIBRARIES_DISABLE_WARNINGS

namespace
{

const unsigned GENERATOR_A_MULTIPLIER = 16807;
const unsigned GENERATOR_B_MULTIPLIER = 48271;

const unsigned GENERATOR_A_MULTIPLE_CRITERION = 4;
const unsigned GENERATOR_B_MULTIPLE_CRITERION = 8;

const unsigned GENERATOR_MODULUS = 2'147'483'647;

const unsigned NO_CRITERIA_NUM_ROUNDS = 40'000'000;
const unsigned CRITERIA_NUM_ROUNDS = 5'000'000;

const unsigned JUDGE_NUM_BINARY_DIGITS_TO_MATCH = 16;

}

namespace AdventOfCode
{
namespace Year2017
{
namespace Day15
{

unsigned generatorStartFromLine(const std::string& line)
{
    std::vector<std::string> tokens;
    boost::split(tokens, line, boost::is_any_of(" "));

    if (tokens.size() != 5)
    {
        throw std::runtime_error("Input line needs to have exactly 5 tokens.");
    }

    return boost::lexical_cast<unsigned>(tokens[4]);
}

constexpr uint64_t generateNextValue(uint64_t prevValue, unsigned multiplier, unsigned multipleCriterion) noexcept
{
    uint64_t currValue{};
    while (true)
    {
        currValue = (prevValue * multiplier) % GENERATOR_MODULUS;

        if (currValue % multipleCriterion == 0)
        {
            break;
        }

        prevValue = currValue;
    }

    return currValue;

}

unsigned judgeFinalCount(unsigned generatorAStart, unsigned generatorBStart, unsigned numRounds, bool isUsingCriteria)
{
    const double judgeModulusDouble = (pow(2, JUDGE_NUM_BINARY_DIGITS_TO_MATCH));
    assert(judgeModulusDouble <= std::numeric_limits<unsigned>::max());

    const unsigned judgeModulus = static_cast<unsigned>(judgeModulusDouble);

    uint64_t generatorAPrevValue = generatorAStart;
    uint64_t generatorBPrevValue = generatorBStart;

    unsigned judgeScore = 0;

    for (unsigned i = 0; i < numRounds; ++i)
    {
        const uint64_t generatorACurrValue = generateNextValue(generatorAPrevValue, GENERATOR_A_MULTIPLIER, isUsingCriteria ? GENERATOR_A_MULTIPLE_CRITERION : 1);
        const uint64_t generatorBCurrValue = generateNextValue(generatorBPrevValue, GENERATOR_B_MULTIPLIER, isUsingCriteria ? GENERATOR_B_MULTIPLE_CRITERION : 1);

        if (generatorACurrValue % judgeModulus == generatorBCurrValue % judgeModulus)
        {
            ++judgeScore;
        }

        generatorAPrevValue = generatorACurrValue;
        generatorBPrevValue = generatorBCurrValue;
    }

    return judgeScore;
}

unsigned judgeFinalCountNoCriteria(unsigned generatorAStart, unsigned generatorBStart)
{
    return judgeFinalCount(generatorAStart, generatorBStart, NO_CRITERIA_NUM_ROUNDS, false);
}

unsigned judgeFinalCountWithCriteria(unsigned generatorAStart, unsigned generatorBStart)
{
    return judgeFinalCount(generatorAStart, generatorBStart, CRITERIA_NUM_ROUNDS, true);
}

}
}
}