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

/*
This class is created for using the extended hull-white model with
term structure of volatility.

Created by Jihoon Lee, 20150127

*/

/*! \file hullwhitevolatility.hpp
	\brief HullWhite functions
*/

#ifndef quantlib_hullwhitevolatility_hpp
#define quantlib_hullwhitevolatility_hpp

#include <ql/time/date.hpp>
#include <ql/math/matrix.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/models/parameter.hpp>
#include <ql/termstructures/interpolatedcurve.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <boost/function.hpp>

#include <ql/types.hpp>

namespace QuantLib {

	class HullWhiteVolatility {
	 public:
		 HullWhiteVolatility();

		 HullWhiteVolatility(
			 Date referenceDate,
			 DayCounter dc,
			 std::vector<Volatility>& vols,
			 std::vector<Date>& volDates);

		 boost::function<Real (Time)> vol() const;

		 Real sigma() const { return sigma_(0.0); }

	 private:

		 Date referencedate_;
		 DayCounter daycounter_;

		 std::vector<Date> volstructure_;
		 std::vector<Time> volperiods_;

		 Parameter sigma_;
	};

}

#endif


/**
 *  @file    SparseSolver.hpp
 *  @brief   Solves a finite difference problem.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef SPARSESOLVER_H
#define SPARSESOLVER_H

#include <vector>
#include <Eigen/SparseCore>
#include "spdlog/spdlog.h"
#include "Problem.hpp"

namespace numerical {

namespace fdm {

/*
 * This class is used to solve the diffusion problem in 1D with uniform and
 * constant diffusion coefficient \f$\alpha\f$ and constant Dirichlet boundary
 * conditions.
 */
template <typename T>
class SparseSolver {
typedef Eigen::SparseMatrix<T> SpMat;
typedef Eigen::Triplet<T> Trip;
typedef Eigen::VectorXd Vec;
private:
  SpMat m_A;
  Vec m_b;
  Vec m_u;
  Problem<T>* m_problem;
public:
  SparseSolver(Problem<T>* problem)
    : m_problem(problem)
     {
      // define variables
      T dx, dt;
  }
  ~SparseSolver(){
      // delete m_A;
      // delete m_b;
  }

  /**
  * Assembles sparse coefficient matrix A.
  *
  * \f[
  *    A_{i,i-1}=-F\Theta,~A{i, i}=1+2F\Theta,~A_{i, i+1}=-F\Theta
  * \f].
  *
  */
  void assemble_a(){
    /*
    // The loops are vectorized for efficiency -- see bench/performances
    SpMat A(nx + 1, nx + 1);
    std::vector<Trip> trp;
    // diagonal terms
    // Eigen::VectorXd val = Eigen::VectorXd::Zero(nx + 1);  // initialized to zero
    Vec diagonal = Eigen::VectorXd::Constant(nx + 1, 1.0);
    diagonal[0] = 0.0;
    diagonal[nx] = 0.0;
    // segment(pos, n) the n coeffs in the range [pos : pos + n - 1]
    diagonal.segment(1, diagonal.size()-2) += Dl * m_a.segment(2, m_a.size()-2);
    diagonal.segment(1, diagonal.size()-2) += Dl * 2.0 * m_a.segment(1, m_a.size()-2);
    diagonal.segment(1, diagonal.size()-2) += Dl * m_a.segment(0, m_a.size()-2);
    // lower terms
    Vec lower = Eigen::VectorXd::Zero(nx);
    lower.segment(0, lower.size()-1) += -Dl * m_a.segment(1, m_a.size()-2);
    lower.segment(0, lower.size()-1) += -Dl * m_a.segment(0, m_a.size()-2);
    // upper terms
    Vec upper = Eigen::VectorXd::Zero(nx);
    upper.segment(1, upper.size()-1) += -Dl * m_a.segment(2, m_a.size()-2);
    upper.segment(1, upper.size()-1) += -Dl * m_a.segment(1, m_a.size()-2);

    // boundary conditions
    diagonal[0] = 1.0;
    upper[0] = 0.0;
    diagonal[nx] = 1.0;
    lower[nx-1] = 0.0;

    // std::cout << diagonal << "\n";
    for(int i=1; i<m_x.size() - 1; i++){
        trp.push_back(Trip(i,i,diagonal[i]));
    }
    // std::cout << lower << "\n";
    for(int i=1; i<m_x.size() - 1; i++){
         trp.push_back(Trip(i,i-1,lower[i-1]));
    }
    // std::cout << upper << "\n";
    for(int i=1; i<m_x.size() - 1; i++){
        trp.push_back(Trip(i,i+1,upper[i]));
    }
    // create sparse matrix
    A.setFromTriplets(trp.begin(), trp.end());
    m_A = A;
    */
  }

  /**
  * Assembles RHS vector b.
  *
  * \f[
  *    b_i = u_i^n + F\left(1-\Theta\right)u_{i+1}^n-2u_i^n+u_{i-1}^n +
  *        \Delta t \Theta f_i^{n+1} + \Delta t \left(1-\Theta\right)f_i^n
  * \f]
  *
  * using vectorization we get:
  *
  * \f[
  *    b[1:n_x-1] = u_n[1:n_x-1] + \left(1-\Theta\right)F
  *        \left(u_n[2:n_x]-2u_n[1:n_x-1]+u_n[0:n_x-2]\right) +
  *        \Theta\Delta t f[1:n_x-1](n+1) +
  *        \left(1-\Theta\right)\Delta t f[1:n_x-1](n)
  * \f]
  *
  */
  void assemble_b(T t){

  }

  /*
  * Solve the time dependent problem.
  */
  virtual Eigen::VectorXf solve(){
      //  Set initial condition
      //for(int i=0; i<u_n.size(); i++){
          // u_n[i] = m_params.init(m_x[i], 0., 0.);
      //}
      // std::cout << u_n << "\n";

      spdlog::info("{}", m_problem->left(0, 1.0, 1.0, 1.0));
      // Time loop

      Eigen::VectorXf solution = Eigen::VectorXf::Unit(4,1);
      return solution;
  }

};

/*
 * For a sparse matrix, return a vector of triplets, such that we can
 * reconstruct the matrix using setFromTriplet function.
 * @param matrix A sparse matrix.
 * @return A triplet with the row, column and value of the non-zero entries.
 */
template <typename Derived>
std::vector<Eigen::Triplet<typename Derived::Scalar>> SparseMatrixToTriplets(
    const Derived& matrix) {
  using Scalar = typename Derived::Scalar;
  std::vector<Eigen::Triplet<Scalar>> triplets;
  triplets.reserve(matrix.nonZeros());
  for (int i = 0; i < matrix.outerSize(); i++) {
    for (typename Derived::InnerIterator it(matrix, i); it; ++it) {
      triplets.push_back(
          Eigen::Triplet<Scalar>(it.row(), it.col(), it.value()));
    }
  }
  return triplets;
}

}  // namespace fdm

}  // namespace numerical

#endif  // SPARSESOLVER_H


#include <Eigen/Dense>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>

const float EPS = 1e-2;
// Convert int data[] to Eigen::MatrixXd
void grid_to_matrix(const nav_msgs::OccupancyGrid msg, Eigen::MatrixXd &map){
  for(int i=0; i<msg.data.size(); ++i){
    map(i/msg.info.width,i%msg.info.height) = msg.data[i];
  }
}
// Get indices of grid from position
// Return false if out of range
bool pose_to_idx(nav_msgs::MapMetaData info, double target_x, double target_y, int& idx_x, int& idx_y){
  geometry_msgs::Pose origin = info.origin;
  float resolution = info.resolution;
  int map_width = info.width, map_height = info.height;
  idx_x = floor((target_x - origin.position.x + EPS)/resolution);
  idx_y = floor((target_y - origin.position.y + EPS)/resolution);
  if(idx_x<0 or idx_x>=map_height or idx_y<0 or idx_y>=map_height) return false;
  return true;
}
// Convert indices of grid to position with type PoseStamped
geometry_msgs::PoseStamped indice_to_pose(nav_msgs::MapMetaData info, int x, int y){
  geometry_msgs::Pose origin = info.origin;
  float resolution = info.resolution;
  geometry_msgs::PoseStamped res;
  res.pose.position.x = origin.position.x+resolution*y;
  res.pose.position.y = origin.position.y+resolution*x; // Have to change xy order
  res.pose.orientation.w = 1.0;
  return res;
}
// Convert indices of grid to position with type Point
geometry_msgs::Point indice_to_point(nav_msgs::MapMetaData info, int x, int y){
  geometry_msgs::Pose origin = info.origin;
  float resolution = info.resolution;
  geometry_msgs::Point res;
  res.x = origin.position.x+resolution*y;
  res.y = origin.position.y+resolution*x; // Have to change xy order
  return res;
}
// Convert frame of pose with given transform
void convertFrame(tf::Transform mat, geometry_msgs::PoseStamped &ps){
  tf::Vector3 pos(ps.pose.position.x, ps.pose.position.y, 0), res = mat*pos;
  ps.pose.position.x = res.getX(); ps.pose.position.y = res.getY();
}
// Convert frame of pose with given transform
void convertFrame(tf::Transform mat, geometry_msgs::Pose &ps){
  tf::Vector3 pos(ps.position.x, ps.position.y, 0), res = mat*pos;
  ps.position.x = res.getX(); ps.position.y = res.getY();
}
// Overloading for input point
void convertFrame(tf::Transform mat, geometry_msgs::Point &p){
  tf::Vector3 pos(p.x, p.y, 0), res = mat*pos;
  p.x = res.getX(); p.y = res.getY();
}


#ifndef RADARMODELROS_H
#define RADARMODELROS_H

//ROS MSGS
#include "nav_msgs/OccupancyGrid.h"
#include "std_msgs/Header.h"
#include "nav_msgs/MapMetaData.h"

//GRIDMAP
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_core/iterators/GridMapIterator.hpp>
#include <grid_map_cv/grid_map_cv.hpp>
#include <grid_map_core/GridMap.hpp>

#include <Eigen/Eigen> // AFTER GRIDMAP!
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/Splines>

// Math
#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <math.h>
#include <vector>

// other
#include <iomanip>
#include <string>

// OpenCV
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace std;
using namespace grid_map;
using namespace std::placeholders;
using Eigen::MatrixXf;

// constants ..................................................................
// We mostly use UPM frog 3D.
const double TAG_LOSSES = -4.8;

// 20 * log10 ( c / (4*pi) )
const double LOSS_CONSTANT = 147.55;
// 4*pi/c
const double PHASE_CONSTANT = 4.192e-8;
const double C = 299792458.0;

// This comes from the manufacturer. Azimut
// gain list entries start at -180 degrees to 180 in steps of 15.
const double ANTENNA_LOSSES_LIST[25] = {
    -22.6, -25.2, -25,   -20.2, -17.6, -15.6, -14,  -11.2, -7.8,
    -5.2,  -2.4,  -0.6,  0,     -0.6,  -2.4,  -5.2, -8.8,  -12.2,
    -16.4, -19.2, -20.8, -24.4, -28.2, -24,   -22.6};
const double ANTENNA_ANGLES_LIST[25] = {
    -180.0, -165.0, -150.0, -135.0, -120.0, -105.0, -90.0, -75.0, -60.0,
    -45.0,  -30.0,  -15.0,  0.0,    15.0,   30.0,   45.0,  60.0,  75.0,
    90.0,   105.0,  120.0,  135.0,  150.0,  165.0,  180.0};

// M6e RFID reader Specs
// Minimum required power to identify a tag. Also depends on other factors, but
// this value is a guide
const double SENSITIVITY = -115; // dB

// Max transmitted power may be limited by the Region regulations
// see Power Requirements  in M6e Hardware Guide
const double MIN_TX_POWER = -25;  // dB
const double MAX_TX_POWER = 0;    // dB
const double STEP_TX_POWER = 0.5; // dB

// These freqs ARE limited depending on the region regulations
// see Regional Frequency Quantization in M6e Hardware Guide
const double STEP_FREQ = 25e3;   // Hertzs
const double MIN_FREQ_A = 865e6; // Hertzs
const double MAX_FREQ_A = 869e6; // Hertzs
const double MIN_FREQ_B = 902e6; // Hertzs
const double MAX_FREQ_B = 928e6; // Hertzs

// most likely we will use EU or NA regions....
const double MIN_FREQ_EU = 865.6e6; // Hertzs
const double MAX_FREQ_EU = 867.6e6; // Hertzs
const double STEP_FREQ_EU = 100e3;  // Hertzs

const double MIN_FREQ_NA = 902e6;  // Hertzs
const double MAX_FREQ_NA = 928e6;  // Hertzs
const double STEP_FREQ_NA = 250e3; // Hertzs

//  ..................................................................

/**
 * @brief The spline is used to interpolate antenna gain values,
 * as we only have the graph
 */
class SplineFunction {

public:
  SplineFunction();

  /**
   * @brief Construct a new Spline Function Interpolation for antenna gain
   *
   * @param x_vec reference azimuth angle (deg) points
   * @param y_vec reference gain (db) points
   */
  SplineFunction(Eigen::VectorXd const &x_vec, Eigen::VectorXd const &y_vec);

  /**
   * @brief Interpolate gain from angle
   *
   * @param x azimuth angle (rads)
   * @return double interpolated gain (dB)
   */
  double interpRad(double x) const;
  float interpRadf(float x) const;

  /**
   * @brief Interpolate gain from angle
   *
   * @param x azimuth angle (degs)
   * @return double interpolated gain (dB)
   */
  double interpDeg(double x) const;

private:
  double scaled_value(double x) const;

  Eigen::RowVectorXd scaled_values(Eigen::VectorXd const &x_vec) const;

  double x_min;
  double x_max;
  double y_min;
  double y_max;

  // Spline of one-dimensional "points."
  Eigen::Spline<double, 1> spline_;
};

//////////////////////////

class RadarModelROS {

public:
  int _Ncol; // number of rows of reference and rfid belief maps (cells)
  int _Nrow; // number of cols of reference and rfid belief maps (cells)
  float _free_space_val; // max value stored in reference map, used as free
                         // space marker
  double _resolution;    // Reference AND belief map resolution (m./cell)

  // we only model here gaussian noise
  double _sigma_power; // noise factor
  double _sigma_phase; // noise factor

  GridMap _rfid_belief_maps; // Prob. beliefs. One layer per tag. Also, one
                             // layer with reference map, mostly for tag layout
                             // representation.
  GridMap _tmp_rfid_c_map;

  std::vector<std::pair<double, double>>  _tags_coords; // tag locations in reference map coords (m.)
  int _numTags;     // rfid tags to consider

  SplineFunction _antenna_gains; // model for antenna power gain depending on
                                 // the angle (dB.)
  // gains cache for hopefully faster access.
  std::vector<float> _antenaGainVector;

  bool _output_prediction = false;
  float gainValue(int x);

  RadarModelROS(const nav_msgs::OccupancyGrid& nav_map, const double sigma_power, const double sigma_phase, const double resolution, bool output_prediction );
  void initRefMap(const nav_msgs::OccupancyGrid& nav_map);

  void loadBelief(const std::string imageURI);

  void saveBelief(const std::string imageURI);

  /**
   * @brief Construct a new Radar Model object
   *        It is based upon the link budget equation. See D. M. Dobkin, “The RF
   * in RFID: Passive UHF RFID in Practice”, Elsevier, 2007
   * @param resolution   reference map and rfid belief map images resolution in
   * m./cell
   * @param sigma_power  noise std in log model
   * @param sigma_phase  noise std in phase
   * @param txtPower     transmitted power (dB)
   * @param freqs        frequencies used in transmission
   * @param tags_coords  tag positions in map coordinates
   * @param imageFileURI reference map file
   *   TODO We will use a fixed antenna model, Gaussian noise model and will
   * assume tags are isotropic. This may need to be revisited ...
   */
  RadarModelROS(const double resolution, const double sigma_power,
             const double sigma_phase,
             const std::vector<std::pair<double, double>> tags_coords,
             const std::string imageFileURI);

  RadarModelROS();

  /**
   * @brief Print scenario map with tags
   *
   * @param fileURI
   */
  void PrintRefMapWithTags(std::string fileURI);

  double received_power_friis_with_obstacles(double antenna_x, double antenna_y,
                                             double antenna_h, double tag_x,
                                             double tag_y, double tag_h,
                                             double freq, double txtPower,
                                             SplineFunction antennaGainsModel);

  double received_power_friis_with_obstacles(double antenna_x, double antenna_y,
                                             double antenna_h, double tag_x,
                                             double tag_y, double tag_h,
                                             double freq, double txtPower);
  /**
   * @brief Plots a power distribution
   *
   * @param fileURI save file location
   * @param f_i frequency to consider in the propagation model
   */
  void PrintRecPower(std::string fileURI, double f_i, double txtPower);

  /**
   * @brief Plots a probability distribution, conditioned to a received power
   *
   * @param fileURI save file location
   * @param rxPw received power
   * @param f_i frequency to consider in the propagation model
   */
  void PrintPowProb(std::string fileURI, double rxPw, double f_i, double txtPower);

  void PrintPhase(std::string fileURI, double f_i);
  void PrintPhaseProb(std::string fileURI, double phi, double f_i);
  void PrintBothProb(std::string fileURI, double rxPw, double phi, double f_i, double txtPower);

  void PrintMap(std::string savePath);
  void initRefMap(const std::string imageURI);
  void getImage(std::string layerName, std::string fileURI);
  void addTagID(std::string tagID, int i);
  /**
   * @brief Get layer name corresponding to given frequency (Hz.)
   *
   * @param freq_i
   * @return std::string
   */
  std::string getLayerName(double freq_i);
  /////////////////////
  /**
   * returns a range vector FROM start TO stop (included) in step increments
   * @param  start min Value in vector
   * @param  stop  max Value in vector
   * @param  step  increment
   * @return       range vector
   */
  std::vector<double> range(double start, double stop, double step);

  /**
   * returns MT_242025NRHK antenna losses in a plane
   * @param  angleRad spheric coordinate within the plane (radians)
   * @return          dB losses
   */
  double antennaPlaneLoss(double angleRad);

  /**
   * my Sign function...
   * @param  x
   * @return   sign of x
   */
  float sign(float x);

  /**
   * Returns Spherical coordinates
   * @param x          cartesian coordinate x (m.)
   * @param y          cartesian coordinate y (m.)
   * @param r          spheric coordinate r (m.)
   * @param phi        relative azimut [0,2pi) (angle between XY projection and
   * X) (radians)
   */
  void getSphericCoords(double x, double y, double &r, double &phi);

  /**
   * Received signal estimated phase difference with pi ambiguity
   * @param  tag_x       Tag x coord (m.) with respect to antenna
   * @param  tag_y       Tag y coord (m.) with respect to antenna
   * @param  freq        Transmission frequency (Hertzs)
   * @return      phase difference (radians)
   */
  double phaseDifference(double tag_x, double tag_y, double freq);

  Eigen::MatrixXf getFriisMat(double x_m, double y_m, double orientation_deg,
                              double freq, double txtPower);
  Eigen::MatrixXf getFriisMatFast(double x_m, double y_m,
                                  double orientation_deg, double freq, double txtPower);

  Eigen::MatrixXf getPhaseMat(double x_m, double y_m, double orientation_deg,
                              double freq);
  Eigen::MatrixXf getProbCond(Eigen::MatrixXf X_mat, double x, double sig);
  /**
   * @param  x             x coord (m.) in map coords of the center
   * @param  y             y coord (m.) in map coords of the center
   * @param  orientation   orientation (rad.) in map coords of the center
   * @param  tag_i         tag we want to read
   */
  double getTotalWeight(double x, double y, double orientation, int tag_i);

  double getTotalWeight(double x, double y, double orientation, double size_x,
                        double size_y, int tag_i);

  double getTotalWeight(grid_map::SubmapIterator iterator, int tag_i);

  double getTotalWeight(grid_map::PolygonIterator iterator, int tag_i);
  double getTotalWeight(int tag_i);

  void addMeasurement(double x, double y, double orientation, double rxPower,
                      double phase, double freq, int i, double txtPower);

  std::string getPowLayerName(double freq_i);

  std::string getPhaseLayerName(double freq_i);

  std::string getTagLayerName(int tag_num);

  /**
   * Get received power from an OMNIDIRECTIONAL tag,
   * given its relative position to antenna.
   * We assume antenna at 0,0,0, facing X coordinate.
   * See http://www.antenna-theory.com/basics/friis.php
   * Sensitivity is -85 dBm / -115 dB
   *
   * @param  tag_r       Tag r coord (m.) with respect to antenna
   * @param  tag_h       Tag h coord (rad.) with respect to antenna
   * @param  freq        Transmission frequency (Hertzs)
   * @param  txtPower    Transmitted power (dB)
   * @param antennaGainsModel   Antena Gain model
   * @return             Received power (dB)
   */
  double received_power_friis_polar(double tag_r, double tag_h, double freq,
                                    double txtPower,
                                    SplineFunction antennaGainsModel);

  void getImageDebug(GridMap *gm, std::string layerName, std::string fileURI);
  Eigen::MatrixXf getPowProbCond(double rxPw, double f_i, double txtPower);
  Eigen::MatrixXf getPhaseProbCond(double ph_i, double f_i);
  Eigen::MatrixXf getProbCond(std::string layer_i, double x, double sig);

  void saveProbMapDebug(std::string savePATH, int tag_num, int step,
                        double robot_x, double robot_y, double robot_head);
  void createTempProbLayer(Eigen::MatrixXf prob_mat, double x_m, double y_m,
                           double orientation_deg);
  cv::Mat rfidBeliefToCVImg(std::string layer_i);
  void getImage(GridMap *gm, std::string layerName, std::string fileURI);
  Position getRelPoint(Position glob_point, double x_m, double y_m,
                       double orientation_rad);
  Eigen::MatrixXf getIntervProb(std::string layer_i, double x, double sigm);
  void fillFriisMat(Eigen::MatrixXf *rxPw_mat, Eigen::MatrixXf *delay_mat,
                    double freq_i, double offset);

  void overlayRobotPose(GridMap *gm, double robot_x, double robot_y, double robot_head,
                        cv::Mat &image);
  void overlayRobotPose(double robot_x, double robot_y, double robot_head,
                        cv::Mat &image);

  void overlayRobotPoseT(double robot_x, double robot_y, double robot_head,
                         cv::Mat &image);
  void rotatePoints(cv::Point *points, int npts, int cxi, int cyi, double ang);
  void clearObstacles(cv::Mat &image);

  Eigen::MatrixXf getProbCondG(std::string layer_i, double x, double sig);
  Eigen::MatrixXf getProbCondLogN(std::string layer_i, double x, double sig);

  void PrintProb(std::string fileURI, Eigen::MatrixXf *prob_mat, double sX,
                 double sY, double res);
  void saveProbMaps(std::string savePath);

  grid_map::Position fromPoint(cv::Point cvp);
  grid_map::Polygon getActiveMapEdges(double robot_x, double robot_y,
                                      double robot_head);

  void PrintProb(std::string fileURI, Eigen::MatrixXf *prob_mat);

  std::pair<int, std::pair<int, int>> findTagFromBeliefMap(int num_tag);

  void normalizeRFIDLayer(std::string layerName);
  void clearObstacleCellsRFIDLayer(std::string layerName);
  void addTagLayer(int tagNum);

  double getNormalizingFactorBayesRFIDActiveArea(double x_m, double y_m,
                                                 double orientation_rad,
                                                 string tagLayerName);
  Eigen::MatrixXf getNegProb(std::string layer_i, double sensitivity,
                             double sigm);

  void debugInfo();
  void debugInfo(GridMap *gm, std::string mapName,std::string baseLayer);

  cv::Point getPoint(double x, double y);
  cv::Point getPoint(  GridMap* gm, double x_m, double y_m);

  void overlayActiveMapEdges(double robot_x, double robot_y, double robot_head,
                             cv::Mat image);
  void overlayMapEdges(cv::Mat image);

  /**
   * Calculate the KL-divergence between posterior and prior distribution over
   * tags position
   *
   * @param x: x-coord (m.) of the center
   * @param y: y-coord (m.) of the center
   * @param orientation: orientation (rad.) in map coords of the center
   * @param size_x: size-x of the active area
   * @param size_y: size-y of the active area
   * @param tag_i: id of the tag emitting the signal
   */
  double getTotalKL(double x, double y, double orientation, double size_x,
                    double size_y, int tag_i);
  /**
   * Calculate the KL-divergence between posterior and prior distribution over
   * tags position
   *
   * @param x: x-coord (m.) of the center
   * @param y: y-coord (m.) of the center
   * @param orientation: orientation (rad.) in map coords of the center
   * @param iterator: iterator over a submap
   * @param tag_i: id of the tag emitting the signal
   */
  double getTotalKL(double x, double y, double orientation,
                    grid_map::SubmapIterator iterator, int tag_i);
  /**
   * Calculate the entropy of the tag position over the map
   *
   * @param x: x-coord (m.) of the center
   * @param y: y-coord (m.) of the center
   * @param orientation: orientation (rad.) in map coords of the center
   * @param size_x: size-x of the active area
   * @param size_y: size-y of the active area
   * @param tag_i: id of the tag emitting the signal
   */
  double getTotalEntropy(double x, double y, double orientation, double size_x,
                         double size_y, int tag_i);
  /**
   * Calculate the entropy of the tag position over the map
   *
   * @param x: x-coord (m.) of the center
   * @param y: y-coord (m.) of the center
   * @param orientation: orientation (rad.) in map coords of the center
   * @param iterator: iterator over a submap
   * @param tag_i: id of the tag emitting the signal
   */
  double getTotalEntropy(double x, double y, double orientation,
                         grid_map::SubmapIterator iterator, int tag_i);
  /**
   * Calculate the entropy of the tag position over the map
   *
   * @param target: the robot pose
   * @param maxX: distance from one focal distance to the further edge
   * @param minX: distance from one focal distance to the clostest edge
   * @param tag_i: id of the tag emitting the signal
   */
  double getTotalEntropyEllipse(double x, double y, double h_rad, double maxX, double minX,
                                int tag_i);
  /**
   * Calculate the entropy of the tag position over the map
   *
   * @param target: the robot pose
   * @param iterator: iterator over an ellipse
   * @param tag_i: id of the tag emitting the signal
   */
  double getTotalEntropyEllipse(grid_map::EllipseIterator iterator,
                                int tag_i);

  void printEllipse(double x, double y, double orient_rad, double maxX, double minX);

  void addTmpMeasurementRFIDCriterion(double x, double y, double orientation,
                                      double rxPower, double phase, double freq,
                                      int i, double len_update);
  Eigen::MatrixXf getPowProbCondRFIDCriterion(double rxPw, double f_i);
  Eigen::MatrixXf getNegProbRFIDCriterion(double sensitivity, double sigm);
  void createTempProbLayerRFIDCriterion(Eigen::MatrixXf prob_mat, double x_m,
                                        double y_m, double orientation_deg,
                                        double len_update);
  grid_map::Polygon getSubMapEdges(double robot_x, double robot_y,
                                   double robot_head, double len);
  // Position getSubMapRelPoint(Position glob_point, double x_m, double  y_m,
  // double orientation_rad, double len);

  template <typename Scalar>
  void meshgrid(const Eigen::Matrix<Scalar, -1, 1> &x,
                const Eigen::Matrix<Scalar, -1, 1> &y,
                Eigen::Matrix<Scalar, -1, -1> &X,
                Eigen::Matrix<Scalar, -1, -1> &Y);

  template <typename Scalar>
  void meshgrid(const Eigen::Matrix<Scalar, 1, -1> &x,
                const Eigen::Matrix<Scalar, 1, -1> &y,
                Eigen::Matrix<Scalar, -1, -1> &X,
                Eigen::Matrix<Scalar, -1, -1> &Y);

  void addLossesTillEdgeLine(grid_map::Index edge_index_start,
                             grid_map::Index edge_index_end,
                             grid_map::Index antenna_index);
  bool useFast = true;
  Eigen::MatrixXf getFriisMatSlow(double x_m, double y_m,
                                  double orientation_deg, double freq, double txtPower);
  Eigen::MatrixXf getPhaseProbCond(double ph_i, double x_m, double y_m,
                                   double orientation_deg, double f_i);
  Eigen::MatrixXf getPowProbCond(double rxPw, double x_m, double y_m,
                                 double orientation_deg, double f_i, double txtPower);

  void PrintRecPower(std::string fileURI, double x_m, double y_m,
                     double orientation_deg, double f_i, double txtPower);
  void PrintPhase(std::string fileURI, double x_m, double y_m,
                  double orientation_deg, double f_i);
  void PrintPowProb(std::string fileURI, double rxPw, double x_m, double y_m,
                    double orientation_deg, double f_i, double txtPower);
  void PrintPhaseProb(std::string fileURI, double phi, double x_m, double y_m,
                      double orientation_deg, double f_i);
  void PrintBothProb(std::string fileURI, double rxPw, double phi, double x_m,
                     double y_m, double orientation_deg, double f_i, double txtPower);
  cv::Mat layerToImage(GridMap *gm, std::string layerName);

  Eigen::MatrixXf getFakeMeasurement(double x_m, double y_m, double orientation_deg,
                                double rxPower, double phase, double freq,
                                double txtPower);
}; // end class

#endif


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/math/box/center.hpp>
#include <fcppt/math/box/comparison.hpp>
#include <fcppt/math/box/object_impl.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	box_center
)
{
FCPPT_PP_POP_WARNING

	typedef fcppt::math::box::object<
		int,
		3
	> box_type;

	box_type const b(
		box_type::vector(10,12,14),
		box_type::dim(24,26,4)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::box::center(
			b
		),
		box_type::vector(22,25,16)
	);
}


/*
 * CubicPolynomialTrajectory.h
 *
 *  Created on: June 21, 2020
 *      Author: Quincy Jones
 *
 * Copyright (c) <2020> <Quincy Jones - quincy@implementedrobotics.com/>
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the Software
 * is furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions 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 NOMAD_CUBICPOLYNOMIALTRAJECTORY_H_
#define NOMAD_CUBICPOLYNOMIALTRAJECTORY_H_

// C System Files

// C++ System Files

// Third Party Includes
#include <Eigen/Dense>

// Project Include Files
namespace Common
{
    class CubicPolynomialTrajectory
    {

    public:
        CubicPolynomialTrajectory(double q_f, double t_f);
        CubicPolynomialTrajectory(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f);
        CubicPolynomialTrajectory(); // Empty Trajectory

        void Generate(double q_f, double t_f);
        void Generate(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f);

        // TODO: Check for valid t between 0<->t_f
        double Position(double t);
        double Velocity(double t);
        double Acceleration(double t);

    protected:
        void ComputeCoeffs();

        Eigen::Vector4d a_; // Coefficients

        double q_0_;
        double v_0_;
        double t_0_;

        double q_f_;
        double v_f_;
        double t_f_;
    };
} // namespace Common

#endif // NOMAD_CUBICPOLYNOMIALTRAJECTORY_H_


/**
 * @file solver.cpp
 * @brief API for ADMM-based formation gain solver
 * @author Parker Lusk <parkerclusk@gmail.com>
 * @date 25 July 2020
 */

#include <iostream>

#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/SparseCholesky>

#include "admm/solver.h"

namespace acl {
namespace aclswarm {
namespace admm {

Solver::Solver(const Params& params)
: params_(params)
{

}

// ----------------------------------------------------------------------------

Eigen::MatrixXd Solver::solve(
                        const Eigen::Matrix<double, 3, Eigen::Dynamic>& pts,
                        const Eigen::MatrixXd& adj)
{

  //
  // Solve 2D gain design subproblem
  //

  const auto A2d = solve2d(pts.topRows(2), adj);

  //
  // Solve 1D gain design subproblem
  //

  const auto A1d = solve1d(pts.bottomRows(1), adj);

  //
  // Combine for 3D gain design problem
  //

  const size_t n = pts.cols();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*n,3*n);
  for (size_t i=0; i<A.rows(); ++i) {
    for (size_t j=0; j<A.cols(); ++j) {

      // which 3x3 A_ij sub-block are we in?
      const size_t blki = i / 3;
      const size_t blkj = j / 3;

      // map index into 2d sub-block
      const size_t i2d = i - blki;
      const size_t j2d = j - blkj;

      // map index into 1d sub-block
      const size_t i1d = blki;
      const size_t j1d = blkj;

      // determine if we are indexing the 3rd row/col in A_ij
      bool row3 = ((i+1) % 3) == 0;
      bool col3 = ((j+1) % 3) == 0;

      if (!row3 && !col3) {
        A(i,j) = A2d(i2d,j2d);
      } else if (row3 && col3) {
        A(i,j) = A1d(i1d,j1d);
      }
    }
  }

  return A;
}

// ----------------------------------------------------------------------------
// Private Methods
// ----------------------------------------------------------------------------

Eigen::MatrixXd Solver::solve1d(
                        const Eigen::Matrix<double, 1, Eigen::Dynamic>& pts,
                        const Eigen::MatrixXd& adj)
{

  //
  // Build orthogonal complement of gain matrix kernel
  //

  const size_t n = adj.rows();
  const size_t d = 1; // ambient dimension of the problem

  // xy stacked
  Eigen::Map<const Eigen::VectorXd> qz(pts.data(), pts.size());

  // one vector
  Eigen::VectorXd ez = Eigen::VectorXd::Ones(n);

  // determine if desired formation is actually 2D (flat planar)
  const double stdev = std::sqrt((qz.array() - qz.mean()).array().square().sum()/(n-1));
  bool xyflat = (stdev < params_.thrPlanar);

  // kernel of gain matrix
  size_t dimKer;
  Eigen::MatrixXd N;
  if (xyflat) {
    dimKer = 1;
    N = Eigen::MatrixXd(pts.size(), dimKer);
    N << qz;
  } else {
    dimKer = 2;
    N = Eigen::MatrixXd(pts.size(), dimKer);
    N << qz, ez;
  }
  const size_t m = n - dimKer; // reduced number due to orth. compl. restriction

  // find the orthogonal complement of the kernel
  // recall: N = [U1 U2][S 0; 0 0][V1h; V2h]. We want U2.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(N, Eigen::ComputeFullU);
  Eigen::MatrixXd Q = svd.matrixU().rightCols(svd.matrixU().cols() - dimKer);

  //
  // Build the gain design optimization problem
  //

  SpMat C, A, b, X;
  parse(d, m, n, adj, Q, C, A, b, X);

  //
  // Solve SDP using ADMM on sparse matrices
  //

  admm(C, A, b, X);

  //
  // Recover gain matrix
  //

  Eigen::MatrixXd Aopt = - Q * X.bottomRightCorner(d*m, d*m) * Q.transpose();
  Aopt = (params_.thrSparseZero < Aopt.array().abs()).select(Aopt, 0.0);

  return Aopt;
}

// ----------------------------------------------------------------------------

Eigen::MatrixXd Solver::solve2d(
                        const Eigen::Matrix<double, 2, Eigen::Dynamic>& pts,
                        const Eigen::MatrixXd& adj)
{

  //
  // Build orthogonal complement of gain matrix kernel
  //

  const size_t n = adj.rows();
  const size_t m = n - 2; // reduced number due to orth. compl. restriction
  const size_t d = 2; // ambient dimension of the problem

  // xy stacked
  Eigen::Map<const Eigen::VectorXd> q(pts.data(), pts.size());

  // 90-degree rotated (-yx stacked)
  Eigen::VectorXd qbar = Eigen::VectorXd::Zero(pts.size());
  Eigen::Map<const Eigen::VectorXd, 0, Eigen::InnerStride<2>> qx(q.data(), q.size()/2);
  Eigen::Map<const Eigen::VectorXd, 0, Eigen::InnerStride<2>> qy(q.data()+1, q.size()/2);
  Eigen::Map<Eigen::VectorXd, 0, Eigen::InnerStride<2>> qbarx(qbar.data(), qbar.size()/2);
  Eigen::Map<Eigen::VectorXd, 0, Eigen::InnerStride<2>> qbary(qbar.data()+1, qbar.size()/2);
  qbarx = -qy;
  qbary =  qx;

  // one vectors
  Eigen::VectorXd ex = Eigen::Vector2d::UnitX().replicate(n, 1);
  Eigen::VectorXd ey = Eigen::Vector2d::UnitY().replicate(n, 1);

  // kernel of gain matrix
  static constexpr size_t dimKer = 4;
  Eigen::Matrix<double, Eigen::Dynamic, dimKer> N = Eigen::MatrixXd(pts.size(), dimKer);
  N << q, qbar, ex, ey;

  // find the orthogonal complement of the kernel
  // recall: N = [U1 U2][S 0; 0 0][V1h; V2h]. We want U2.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(N, Eigen::ComputeFullU);
  Eigen::MatrixXd Q = svd.matrixU().rightCols(svd.matrixU().cols() - dimKer);

  //
  // Build the gain design optimization problem
  //

  SpMat C, A, b, X;
  parse(d, m, n, adj, Q, C, A, b, X);

  //
  // Solve SDP using ADMM on sparse matrices
  //

  admm(C, A, b, X);

  //
  // Recover gain matrix
  //

  Eigen::MatrixXd Aopt = - Q * X.bottomRightCorner(d*m, d*m) * Q.transpose();
  Aopt = (params_.thrSparseZero < Aopt.array().abs()).select(Aopt, 0.0);

  return Aopt;
}

// ----------------------------------------------------------------------------

inline size_t Solver::vecsel(size_t rows, size_t cols, size_t i, size_t j)
{
  return j*rows + i;
}

// ----------------------------------------------------------------------------

inline size_t Solver::blksel(size_t dim, size_t blkidx, size_t subidx)
{
  return dim*blkidx + subidx;
}

// ----------------------------------------------------------------------------

inline void Solver::vectorize(const SpMat& X, SpMat& x)
{
  x.resize(X.size(), 1);
  x.reserve(X.nonZeros());
  x.startVec(0);
  for (size_t j=0; j<X.cols(); ++j) {
    for (SpMat::InnerIterator it(X, j); it; ++it) {
      x.insertBack(j*X.rows() + it.row(), 0) = it.value();
    }
  }
}

// ----------------------------------------------------------------------------

inline void Solver::unvectorize(const SpMat& x, SpMat& X)
{
  X.reserve(x.nonZeros());
  int curj = -1;

  for (SpMat::InnerIterator it(x, 0); it; ++it) {

    // select the correct destination row/col.
    const size_t i = it.row() % X.rows();
    const size_t j = it.row() / X.cols();
    if (j != curj) {
      X.startVec(j);
      curj = j;
    }

    X.insertBack(i, j) = it.value();
  }
}

// ----------------------------------------------------------------------------

void Solver::admm(const SpMat& C, const SpMat& A, const SpMat& b, SpMat& X)
{

  // cached operations
  const SpMat As = A.adjoint(); // dual operator
  Eigen::SimplicialCholesky<SpMat> AAs((A * As).pruned());

  // initialize intermediate variables
  SpMat Xold;
  SpMat S(X.rows(), X.cols());
  SpMat y(b.rows(), 1);

  //
  // ADMM Iterations
  //

  for (size_t i=0; i<params_.maxItr; ++i) {

    // update y
    {
      const SpMat D = C - S - params_.mu * X;
      SpMat Dvec; vectorize(D, Dvec);
      const SpMat e = A * Dvec + params_.mu * b;
      y = AAs.solve(e); // AAs \ e
    }

    // update S
    SpMat W;
    {
      const SpMat d = (As * y).pruned(1, params_.thrSparseZero);
      SpMat dmat(X.rows(), X.cols()); unvectorize(d, dmat);
      const SpMat WW = C - dmat - params_.mu * X;
      W = (WW + SpMat(WW.transpose())) / 2.0;
    }

    // determine index where positive evals start
    Eigen::SelfAdjointEigenSolver<SpMat> es(W);
    size_t k = 0;
    for (size_t i=0; i<W.rows(); ++i) {
      if (es.eigenvalues()(i) > params_.epsEig) {
        k = i;
        break;
      }
    }
    const size_t idxPosStart = W.rows() - k;

    // remove non-positive modes
    const Eigen::MatrixXd V = es.eigenvectors().rightCols(idxPosStart);
    const Eigen::MatrixXd D = es.eigenvalues().tail(idxPosStart).asDiagonal();
    S = (V * D * V.transpose()).sparseView(1, params_.thrSparseZero);

    // update X
    Xold = X;
    X = (S - W) / params_.mu;

    // check stop criteria --- difference in X
    const double diffX = (X - Xold).cwiseAbs().sum();
    if (diffX < params_.thresh) break;

    // check problem specific stop criteria --- trace value of \bar{A}
    const auto Abar = X.bottomRightCorner(X.rows()/2, X.cols()/2);
    const double Etr = Abar.rows(); // expected trace value (d*m)
    double tr = 0;
    for (size_t k=0; k<Abar.rows(); ++k) tr += Abar.coeff(k,k);
    double trPercentErr = (tr - Etr) / Etr;
    if (trPercentErr < params_.threshTr) break;
  }

  //
  // Project soln to ensure graph constraints are satisfied (set S=0)
  //

  const SpMat D = C - params_.mu * X;
  SpMat Dvec; vectorize(D, Dvec);
  const SpMat e = A * Dvec + params_.mu * b;
  y = AAs.solve(e); // AAs \ e

  const SpMat d = (As * y).pruned(1, params_.thrSparseZero);
  SpMat dmat(X.rows(), X.cols()); unvectorize(d, dmat);
  const SpMat WW = C - dmat - params_.mu * X;
  const SpMat W = (WW + SpMat(WW.transpose())) / 2.0;

  X = (- W) / params_.mu;
}

// ----------------------------------------------------------------------------

void Solver::parse(size_t d, size_t m, size_t n,
                      const Eigen::MatrixXd& adj, const Eigen::MatrixXd& Q,
                      SpMat& C, SpMat& A, SpMat& b, SpMat& X)
{
  //
  // Preallocate number of non-zeros
  //

  // block X_11
  const size_t nrA_X11 =
      (d*m-1)*2               // [X_11]_11 can be whatever it wants (t)
                              // but the other diag elements must be == [X_11]_11
    + (d*m)*(d*m-1)/2;        // set upper-triangular elements to zero
  const size_t nrb_X11 = 0;

  // block X_12
  const size_t nrA_X12 =
      d*m                     // each diagonal elem must be 1
    + (d*m)*(d*m-1);          // each off-diagonal elem must be 0
  const size_t nrb_X12 = d*m; // each diagonal elem must be 1

  // structure constraints for each gain matrix block
  const size_t nrA_X22_struct =
  (d == 2) ?
      0.5*m*(m+1)*(2+2)       // structure constraints: A_ij = [a b; -b a]
                              // 0.5*m*(m+1): each blk, including A_ii blks
    - m                       // don't count -b elem on A_ii (below diag)
                              // (2+2) because a-a=0 is 2 and b-b=0 is 2
  : 0; // no structure requirement for 1D subproblem
  const size_t nrb_X22_struct = 0;

  // zero-gain constraints based on given adj mat
  const size_t nr0 = ((adj.array()==0).count() - n)/2; // number of zeros in adj
  const size_t nrA_X22_adjmat =
      nr0*(d*(d*m)*(d*m));    // each 0 in adj creates d constraints on \bar{A}
  const size_t nrb_X22_adjmat = 0;

  // TODO: see MATLAB impl (ADMMGainDesign3D.m). Do we actually need to remove
  // trivial constraints, or was that left over from debugging / designing?

  // trace constraint on \bar{A}
  const size_t nrA_X22_trace =
      d*m;                    // the sum of each [X_22]_ii == d*m*destrace
  const size_t nrb_X22_trace = 1;

  // X must be symmetric: [X]_ij == [X]_ji
  const size_t nrA_X_sym =
      2 * d*m * (2*d*m-1);    //
  const size_t nrb_X_sym = 0;

  // total number of elements from constraints
  const size_t nrA = nrA_X11 + nrA_X12 + nrA_X22_struct + nrA_X22_adjmat + nrA_X22_trace + nrA_X_sym;
  const size_t nrb = nrb_X11 + nrb_X12 + nrb_X22_struct + nrb_X22_adjmat + nrb_X22_trace + nrb_X_sym;

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "nrA_X11: " << nrA_X11 << std::endl;
    std::cout << "nrA_X12: " << nrA_X12 << std::endl;
    std::cout << "nrA_X22_struct: " << nrA_X22_struct << std::endl;
    std::cout << "nr0: " << nr0 << std::endl;
    std::cout << "nrA_X22_adjmat: " << nrA_X22_adjmat << std::endl;
    std::cout << "nrA_X22_trace: " << nrA_X22_trace << std::endl;
    std::cout << "nrA_X_sym: " << nrA_X_sym << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "nrA: " << nrA << std::endl;
    std::cout << "nrb: " << nrb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
  }

  std::vector<Eigen::Triplet<double>> Acoeffs, bcoeffs;
  Acoeffs.reserve(nrA);
  bcoeffs.reserve(nrb);

  size_t itrr = 0; // which row of \mathbf{A} should nz val be in?

  //
  // Build constraints for block X_11
  //

  // diagonal entries of X_11 should be equal to the first diagonal entry
  for (size_t i=1; i<d*m; ++i) {

    // always select first diagonal entry
    Acoeffs.emplace_back(itrr, 0, 1);

    // [1 0 ... -1 ... 0] vec(X) = 0
    //           ^
    //           selects elem corresponding to diagonal, [X_11]_ii
    const size_t itrc = vecsel(2*d*m, 2*d*m, i, i);
    Acoeffs.emplace_back(itrr, itrc, -1);

    // create new row in \mathbf{A} linear constraint matrix
    itrr++;
  }

  // off-diagonal entries should be zero
  for (size_t i=0; i<d*m; ++i) {
    for (size_t j=i+1; j<d*m; ++j) {

      const size_t itrc = vecsel(2*d*m, 2*d*m, i, j);
      Acoeffs.emplace_back(itrr, itrc, 1);

      // create new row in \mathbf{A} linear constraint matrix
      itrr++;
    }
  }

  size_t tmpA = 0;
  size_t tmpb = 0;

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X_11 constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Build constraints for block X_12
  //

  // off-diagonal entries should be zero
  for (size_t i=0; i<d*m; ++i) {
    for (size_t j=0; j<d*m; ++j) {

      const size_t jj = d*m + j; // skip first dm cols to get into X_12
      const size_t itrc = vecsel(2*d*m, 2*d*m, i, jj);

      // diagonal entries should be one
      if (i == j) {
        Acoeffs.emplace_back(itrr, itrc, 1);
        bcoeffs.emplace_back(itrr,    0, 1);
      } else { // all other entries should be zero
        Acoeffs.emplace_back(itrr, itrc, 1);
      }

      // create new row in \mathbf{A} linear constraint matrix
      itrr++;
    }
  }

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X_12 constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Build constraints for block X_22 = \bar{A}
  //

  if (d == 2) {
    // structure constraints A_ij = [a b; -b a]
    for (size_t i=0; i<m; ++i) {
      for (size_t j=i; j<m; ++j) {

        // diagonal entries should be equal
        const size_t ii1 = d*m + blksel(d, i, 0); // skip first dm rows
        const size_t jj1 = d*m + blksel(d, j, 0); // and cols for X_22
        const size_t ii2 = d*m + blksel(d, i, 1);
        const size_t jj2 = d*m + blksel(d, j, 1);
        const size_t itrc1 = vecsel(2*d*m, 2*d*m, ii1, jj1);
        const size_t itrc2 = vecsel(2*d*m, 2*d*m, ii2, jj2);
        Acoeffs.emplace_back(itrr, itrc1,  1);
        Acoeffs.emplace_back(itrr, itrc2, -1);

        // create new row in \mathbf{A} linear constraint matrix
        itrr++;

        // off-diagonal entries should have same value with opposite sign.
        if (i == j) {
          // If operating on a blk on the diag (A_ii), only enfore constraints
          // for the upper triangular portion---sym const enforced later.
          // note that these constraints enforce b = 0.
          const size_t ii = d*m + blksel(d, i, 0); // skip first dm rows
          const size_t jj = d*m + blksel(d, j, 1); // and cols for X_22
          const size_t itrc = vecsel(2*d*m, 2*d*m, ii, jj);
          Acoeffs.emplace_back(itrr, itrc, 1);
        } else {
          const size_t ii1 = d*m + blksel(d, i, 0); // skip first dm rows
          const size_t jj1 = d*m + blksel(d, j, 1); // and cols for X_22
          const size_t ii2 = d*m + blksel(d, i, 1);
          const size_t jj2 = d*m + blksel(d, j, 0);
          const size_t itrc1 = vecsel(2*d*m, 2*d*m, ii1, jj1);
          const size_t itrc2 = vecsel(2*d*m, 2*d*m, ii2, jj2);
          Acoeffs.emplace_back(itrr, itrc1, 1);
          Acoeffs.emplace_back(itrr, itrc2, 1);
        }

        // create new row in \mathbf{A} linear constraint matrix
        itrr++;
      }
    }
  }

  // graph constraints (zero blocks for non-neighbors)
  if (nr0 > 0) {
    for (size_t i=0; i<n; ++i) {
      for (size_t j=i+1; j<n; ++j) {
        if (adj(i,j) == 1) continue;

        // we leverage the structure constraint [a b; -b a] and only
        // create explicit constraints for [A_ij]_11 and [A_ij]_12.

        // two constraint rows are created in \mathbf{A}
        const size_t itrr1 = itrr;
        const size_t itrr2 = itrr + 1;

        // Constraint on [A_ij]_11
        const size_t ii1 = blksel(d, i, 0); // skip first dm rows
        const size_t jj1 = blksel(d, j, 0); // and cols for X_22
        const Eigen::MatrixXd QQ1 = Q.transpose().col(jj1) * Q.row(ii1);

        Eigen::MatrixXd QQ2;
        if (d == 2) {
          // Constraint on [A_ij]_12
          const size_t ii2 = blksel(d, i, 1); // skip first dm rows
          const size_t jj2 = blksel(d, j, 0); // and cols for X_22
          QQ2 = Q.transpose().col(jj2) * Q.row(ii2);
        }

        // Note how the linear transformation using the orthogonal complement Q
        // leaks signal into each element of the gain matrix \bar{A}.
        for (size_t ki=0; ki<d*m; ++ki) {
          for (size_t kj=0; kj<d*m; ++kj) {

            const size_t ii = d*m + ki; // skip first dm rows
            const size_t jj = d*m + kj; // and cols for X_22
            const size_t itrc = vecsel(2*d*m, 2*d*m, ii, jj);

            Acoeffs.emplace_back(itrr1, itrc, QQ1(ki,kj));
            if (d == 2) Acoeffs.emplace_back(itrr2, itrc, QQ2(ki,kj));
          }
        }

        // advance by d rows in \mathbf{A} linear constraint matrix
        itrr += d;
      }
    }
  }

  // trace of \bar{A} matrix must be the specified value
  {
    for (size_t i=0; i<d*m; ++i) {

      const size_t ii = d*m + i; // skip first dm rows/cols for X_22
      const size_t itrc = vecsel(2*d*m, 2*d*m, ii, ii);
      Acoeffs.emplace_back(itrr, itrc, 1);
    }

    // expected trace value
    bcoeffs.emplace_back(itrr, 0, d*m);

    // create new row in \mathbf{A} linear constraint matrix
    itrr++;
  }

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X_22 constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Symmetry constraints for entire X matrix
  //

  // symmetric entries should be equal
  for (size_t i=0; i<2*d*m; ++i) {
    for (size_t j=i+1; j<2*d*m; ++j) {

      const size_t itrc1 = vecsel(2*d*m, 2*d*m, i, j);
      const size_t itrc2 = vecsel(2*d*m, 2*d*m, j, i);
      Acoeffs.emplace_back(itrr, itrc1,  1);
      Acoeffs.emplace_back(itrr, itrc2, -1);

      // create new row in \mathbf{A} linear constraint matrix
      itrr++;
    }
  }

  if (params_.verbose) {
    std::cout << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << "Built rows in \\mathbf{A} for X symmetry constraints" << std::endl;
    std::cout << "nnzA: " << Acoeffs.size()-tmpA << std::endl;
    std::cout << "nnzb: " << bcoeffs.size()-tmpb << std::endl;
    std::cout << "**********************************************" << std::endl;
    std::cout << std::endl;

    tmpA = Acoeffs.size();
    tmpb = bcoeffs.size();
  }

  //
  // Prepare sparse matrices for ADMM
  //

  C.resize(2*d*m, 2*d*m);
  C.reserve(Eigen::VectorXi::Constant(d*m,1)); // reserve 1 nz per column of X_11
  for (size_t i=0; i<d*m; ++i) C.insert(i,i) = 1; // make [I 0; 0 0]

  // initialize decision variable to something fairly close
  X.resize(2*d*m, 2*d*m); // [I I; I I]
  X.reserve(Eigen::VectorXi::Constant(2*d*m,2)); // reserve 2 nz per column
  for (size_t i=0; i<d*m; ++i) {
    X.insert(i,i) = 1;
    X.insert(d*m+i,i) = 1;
  }
  for (size_t i=d*m; i<2*d*m; ++i) {
    X.insert(i,i) = 1;
    X.insert(i-d*m,i) = 1;
  }

  A.resize(itrr, X.size());
  A.setFromTriplets(Acoeffs.begin(), Acoeffs.end());

  b.resize(itrr, 1);
  b.setFromTriplets(bcoeffs.begin(), bcoeffs.end());
}

} // ns admm
} // ns aclswarm
} // ns acl


// Copyright 2004-5 The Trustees of Indiana University.
// Copyright 2002 Brad King and Douglas Gregor

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

#ifndef _ALG_PAGE_RANK_HPP
#define _ALG_PAGE_RANK_HPP

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/overloading.hpp>
#include <boost/graph/page_rank.hpp>
#include <vector>

namespace boost { namespace graph {

//~ struct n_iterations
//~ {
  //~ explicit n_iterations(std::size_t n) : n(n) { }

  //~ template<typename RankMap, typename Graph>
  //~ bool
  //~ operator()(const RankMap&, const Graph&)
  //~ {
    //~ return n-- == 0;
  //~ }

 //~ private:
  //~ std::size_t n;
//~ };

namespace detail {
  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
                      incidence_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type rank_type;

    // Set new rank maps
    BGL_FORALL_VERTICES_T(v, g, Graph) put(to_rank, v, rank_type(1 - damping));

    BGL_FORALL_VERTICES_T(u, g, Graph) {
      rank_type u_rank_out = damping * get(from_rank, u) / out_degree(u, g);
      BGL_FORALL_ADJ_T(u, v, g, Graph){
        rank_type ctx_factor = decision_fct(u, v);
        put(to_rank, v, get(to_rank, v) + u_rank_out * ctx_factor);
      }
    }
  }

  template<typename Graph, typename RankMap, typename RankMap2>
  void page_rank_step(const Graph& g, RankMap from_rank, RankMap2 to_rank,
                      typename property_traits<RankMap>::value_type damping,
                      std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
                      bidirectional_graph_tag)
  {
    typedef typename property_traits<RankMap>::value_type damping_type;
    BGL_FORALL_VERTICES_T(v, g, Graph) {
      typename property_traits<RankMap>::value_type rank(0);
      BGL_FORALL_INEDGES_T(v, e, g, Graph){
        damping_type ctx_factor = decision_fct(v, source(e, g));
        rank += get(from_rank, source(e, g)) / out_degree(source(e, g), g) * ctx_factor;
      }
      put(to_rank, v, (damping_type(1) - damping) + damping * rank);
    }
  }
} // end namespace detail

template<typename Graph, typename RankMap, typename Done, typename RankMap2>
void
page_rank(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
          Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n,
          RankMap2 rank_map2
          BOOST_GRAPH_ENABLE_IF_MODELS_PARM(Graph, vertex_list_graph_tag))
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  rank_type initial_rank = rank_type(rank_type(1) / n);
  BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, initial_rank);

  bool to_map_2 = true;
  while ((to_map_2 && !done(rank_map, g)) ||
         (!to_map_2 && !done(rank_map2, g))) {
    typedef typename graph_traits<Graph>::traversal_category category;

    if (to_map_2) {
      detail::page_rank_step(g, rank_map, rank_map2, damping, decision_fct, category());
    } else {
      detail::page_rank_step(g, rank_map2, rank_map, damping, decision_fct, category());
    }
    to_map_2 = !to_map_2;
  }

  if (!to_map_2) {
    BGL_FORALL_VERTICES_T(v, g, Graph) put(rank_map, v, get(rank_map2, v));
  }
}

template<typename Graph, typename RankMap, typename Done>
void
page_rank(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
          Done done,
          typename property_traits<RankMap>::value_type damping,
          typename graph_traits<Graph>::vertices_size_type n)
{
  typedef typename property_traits<RankMap>::value_type rank_type;

  std::vector<rank_type> ranks2(num_vertices(g));
  page_rank(g, rank_map, decision_fct, done, damping, n,
            make_iterator_property_map(ranks2.begin(), get(vertex_index, g)));
}

template<typename Graph, typename RankMap, typename Done>
inline void
page_rank_ctx(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct,
          Done done,
          typename property_traits<RankMap>::value_type damping = 0.85)
{
  page_rank(g, rank_map, decision_fct, done, damping, num_vertices(g));
}

template<typename Graph, typename RankMap>
inline void
page_rank_ctx(const Graph& g, RankMap rank_map,
          std::function<typename property_traits<RankMap>::value_type (typename graph_traits<Graph>::vertex_descriptor, typename graph_traits<Graph>::vertex_descriptor)> decision_fct)
{
  page_rank_ctx(g, rank_map, decision_fct, n_iterations(20));
}

} } // end namespace boost::graph

#ifdef BOOST_GRAPH_USE_MPI
#  include <boost/graph/distributed/page_rank.hpp>
#endif

#endif // BOOST_GRAPH_PAGE_RANK_HPP


#pragma once

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

#include "../util/assert.hh"
#include "../util/Maybe.hh"

namespace bold
{
  template<typename T>
  class LineSegment2;

  class Math
  {
  public:
    static Maybe<Eigen::Vector3d> intersectRayWithGroundPlane(Eigen::Vector3d const& position,
                                                              Eigen::Vector3d const& direction,
                                                              double const planeZ);

    static Maybe<Eigen::Vector3d> intersectRayWithPlane(Eigen::Vector3d const& position,
                                                        Eigen::Vector3d const& direction,
                                                        Eigen::Vector4d const& plane);

    static Eigen::Vector2d linePointClosestToPoint(LineSegment2<double> const& segment,
                                                   Eigen::Vector2d const& point);

    // TODO what if 'vector' has zero length? should this return 'Maybe<Vector2d>'?
    static Eigen::Vector2d findPerpendicularVector(Eigen::Vector2d const& vector);

    static std::function<double()> createUniformRng(double min, double max, bool randomSeed = true);
    static std::function<double()> createNormalRng(double mean, double stddev, bool randomSeed = true);

    static constexpr double degToRad(double degrees) { return (degrees * M_PI) / 180.0; }
    static constexpr double radToDeg(double radians) { return (radians / M_PI) * 180.0; }

    static double smallestAngleBetween(Eigen::Vector2d v1, Eigen::Vector2d v2);

    static Eigen::Affine3d alignUp(Eigen::Affine3d const& transform);

    template<typename T>
    static constexpr T clamp(T val, T min, T max)
    {
      return val < min
        ? min
        : val > max
          ? max
          : val;
    }

    /**
      * Maps @link input to a value in the range from @link lowerOutput to @link upperOutput.
      * If @link input is outside the range from @link lower to @link upper, then @link lowerOutput or @link upperOutput are returned,
      * otherwise the value is linearly interpolated.
      * Note that @link lower must be less than @link upper, but that there is no restriction on values of @link lowerOutput and @link upperOutput.
      */
    template<typename T>
    static T lerp(double const& input, double const& lower, double const& upper, T const& lowerOutput, T const& upperOutput)
    {
      if (unlikely(upper <= lower))
        throw std::runtime_error("lower must be less than upper");

      double ratio = (input - lower) / (upper - lower);
      ratio = clamp(ratio, 0.0, 1.0);

      return lowerOutput + (upperOutput - lowerOutput) * ratio;
    }

    template<typename T>
    static constexpr T lerp(double const& ratio, T const& lowerOutput, T const& upperOutput)
    {
      return lowerOutput + (upperOutput - lowerOutput) * ratio;
    }

    /** Constrains the angle to range [-PI,PI). */
    static double normaliseRads(double rads)
    {
      rads = fmod(rads + M_PI, 2*M_PI);
      if (rads < 0)
          rads += 2*M_PI;
      return rads - M_PI;
    }

    /** Angle spanned by rotation between two angles
     *
     * The rotation is measured by rotating from @a a1 to @a2 in
     * positive direction (counter clockwise for right hand system).
     */
    static double angleDiffRads(double a1, double a2)
    {
      double rads = a2 - a1;
      if (rads < 0.0)
        rads += 2.0 * M_PI;
      return rads;
    }

    /** Absolute distance between two angles in radians
     *
     * e.g.:
     * |pi - .5 pi| = |.5 pi - pi| = .5 pi
     * | -.1 po - .1pi | = | .1pi - -.1 pi | = .2 pi
     * | -.9 pi - .9 pi | = | .9 pi - -.9pi | = .2 pi
     */
    static double shortestAngleDiffRads(double a1, double a2)
    {
      // The fmod() function computes the floating-point remainder of
      // dividing x by y.  The return value is x - n * y, where n is
      // the quotient of x / y, rounded toward zero to an integer.
      double d = fmod(a2 - a1, 2 * M_PI);

      d += (d > M_PI)
        ? -2 * M_PI
        : d <= -M_PI
          ? 2 * M_PI
          : 0;

      return d;
    }

    /** Returns the angle to a point, as defined in the agent frame,
     * where zero is straight ahead and positive is to the left
     * (counter-clockwise). */
    template<int N>
    static double angleToPoint(Eigen::Matrix<double, N, 1> const& point)
    {
      static_assert(N > 1, "Vector must have at least two dimensions");
      return ::atan2(-point.x(), point.y());
    }

    /** Returns the point at the given angle and distance, as defined
     * in the agent frame, where zero is straight ahead and positive
     * is to the left (counter-clockwise). */
    static inline Eigen::Vector2d pointAtAngle(double angle, double distance)
    {
      return Eigen::Vector2d(cos(angle) * distance, sin(angle) * distance);
    }

    /** Returns mean of angles
     *
     * TODO: Algorithm at:
     * http://www.codeproject.com/Articles/190833/Circular-Values-Math-and-Statistics-with-Cplusplus
     * seems to give more intuitive results
     */
    static double angularMean(std::vector<double> const& angles)
    {
      double x = 0.0;
      double y = 0.0;
      for (auto a : angles)
      {
        x += sin(a);
        y += cos(a);
      }
      return atan2(x / angles.size(), y / angles.size());
    }

  private:
    Math() = delete;
  };
}


// -*- mode: c++; fill-column: 80; indent-tabs-mode: nil; -*-

#include <cassert>
#include <cmath>

#include <iomanip>
#include <iostream>
#include <vector>

#include "expsum/fast_esprit.hpp"
#include <boost/math/special_functions/bessel.hpp>

using size_type = arma::uword;
// using Complex   = std::complex<double>;

//------------------------------------------------------------------------------
// Test functors
//------------------------------------------------------------------------------

struct BesselJ0
{
    double operator()(double x) const
    {
        return boost::math::cyl_bessel_j(0, x);
    }
};

struct rinv
{
    double operator()(double x) const
    {
        return 1.0 / x;
    }
};

template <typename F, typename Vec>
void make_sample(F f, double xmin, double xmax, Vec& result)
{
    auto np = result.n_elem;
    auto h  = (xmax - xmin) / (np - 1);
    for (size_type n = 0; n < np; ++n)
    {
        result(n) = f(xmin + n * h);
    }

    return;
}

template <typename F>
void test_fast_esprit(F fn, double xmin, double xmax, size_type N, size_type L,
                      size_type M, double eps)
{
    using value_type  = decltype(fn(xmin));
    using vector_type = arma::Col<value_type>;
    using esprit_type = expsum::fast_esprit<value_type>;
    using real_type   = typename esprit_type::real_type;

    vector_type exact(N);
    make_sample(fn, xmin, xmax, exact);
    auto delta = (xmax - xmin) / (N - 1);

    // ESPRIT esprit(N, std::min<size_type>(100, n / 2));
    esprit_type esprit(N, L, M);

    esprit.fit(exact, xmin, delta, eps);

    auto nterms = esprit.exponents().n_elem;
    std::cout << "# " << nterms << " terms found\n"
              << "# exponent, weight\n";

    for (size_type i = 0; i < nterms; ++i)
    {
        std::cout << esprit.exponents()(i) << '\t' << esprit.weights()(i)
                  << '\n';
    }

    std::cout << "# x, approx, exact, abserr, relerr\n";

    for (size_type i = 0; i < N; ++i)
    {
        auto x      = xmin + i * delta;
        auto approx = esprit.eval_at(x);
        auto abserr = std::abs(approx - exact(i));
        auto relerr =
            (abserr == real_type()) ? real_type() : abserr / std::abs(exact(i));

        std::cout << x << '\t' << approx << '\t' << exact(i) << '\t' << abserr
                  << '\t' << relerr << '\n';
    }
}

int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    std::cout << "# Approximation of Bessel J0(x): x in [0, 1000] by fast "
                 "ESPRIT method."
              << std::endl;
    size_type N = 1024;  // # of sampling points
    size_type L = N / 2; // window length
    size_type M = 100;   // max # of terms
    double xmin = 0.0;
    double xmax = 1000.0;
    double eps  = 1.0e-10;
    test_fast_esprit(BesselJ0(), xmin, xmax, N, L, M, eps);

    std::cout << "\n\n# Approximation of 1/r: r in [1, 10^{6}] by fast"
                 "ESPRIT method."
              << std::endl;
    N    = (1 << 12);
    L    = N / 2;
    M    = 100;
    xmin = 1.0;
    xmax = 1.0e+6;
    eps  = 1.0e-8;
    test_fast_esprit(rinv(), xmin, xmax, N, L, M, eps);

    // std::cout << "# Exponential sum recovery test" << std::endl;
    // const auto pi = arma::datum::pi;
    // numeric::ExponentialSum<Complex> orig(5);

    // orig.exponents(0) = Complex(0.0,     0.0);
    // orig.exponents(1) = Complex(0.0,  pi / 4);
    // orig.exponents(2) = Complex(0.0, -pi / 4);
    // orig.exponents(3) = Complex(0.0,  pi / 2);
    // orig.exponents(4) = Complex(0.0, -pi / 2);

    // orig.weights(0) = Complex(34.0,  0.0);
    // orig.weights(1) = Complex(300.0, 0.0);
    // orig.weights(2) = Complex(300.0, 0.0);
    // orig.weights(3) = Complex(1.0,   0.0);
    // orig.weights(4) = Complex(1.0,   0.0);

    // N = 1024;
    // L = N / 2;
    // M = 20;
    // eps = 1.0e-10;
    // test_frequency_estimation(orig, N, L, M, eps);

    return 0;
}


/**
 * @file nystroem_method_test.cpp
 * @author Ryan Curtin
 *
 * Test the NystroemMethod class and ensure that the reconstructed kernel matrix
 * errors are comparable with those in the literature.
 */
#include <mlpack/core.hpp>

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

#include <mlpack/methods/nystroem_method/ordered_selection.hpp>
#include <mlpack/methods/nystroem_method/random_selection.hpp>
#include <mlpack/methods/nystroem_method/kmeans_selection.hpp>
#include <mlpack/methods/nystroem_method/nystroem_method.hpp>

using namespace mlpack;
using namespace mlpack::kernel;

BOOST_AUTO_TEST_SUITE(NystroemMethodTest);

/**
 * Make sure that if the rank is the same and we do a full-rank approximation,
 * the result is virtually identical (a little bit of tolerance for floating
 * point error).
 */
BOOST_AUTO_TEST_CASE(FullRankTest)
{
  // Run several trials.
  for (size_t trial = 0; trial < 5; ++trial)
  {
    arma::mat data;
    data.randu(5, trial * 200);

    GaussianKernel gk;
    NystroemMethod<GaussianKernel, OrderedSelection> nm(data, gk, trial * 200);

    arma::mat g;
    nm.Apply(g);

    // Construct exact kernel matrix.
    arma::mat kernel(trial * 200, trial * 200);
    for (size_t i = 0; i < trial * 200; ++i)
      for (size_t j = 0; j < trial * 200; ++j)
        kernel(i, j) = gk.Evaluate(data.col(i), data.col(j));

    // Reconstruct approximation.
    arma::mat approximation = g * g.t();

    // Check closeness.
    for (size_t i = 0; i < trial * 200; ++i)
    {
      for (size_t j = 0; j < trial * 200; ++j)
      {
        if (kernel(i, j) < 1e-5)
          BOOST_REQUIRE_SMALL(approximation(i, j), 1e-4);
        else
          BOOST_REQUIRE_CLOSE(kernel(i, j), approximation(i, j), 1e-5);
      }
    }
  }
}

/**
 * Can we accurately represent a rank-10 matrix?
 */
BOOST_AUTO_TEST_CASE(Rank10Test)
{
  arma::mat data;
  data.randu(500, 500); // Just so it's square.

  // Use SVD and only keep the first ten singular vectors.
  arma::mat U;
  arma::vec s;
  arma::mat V;
  arma::svd(U, s, V, data);

  // Don't set completely to 0; the hope is that K is still positive definite.
  s.subvec(0, 9) += 1.0; // Make sure the first 10 singular vectors are large.
  s.subvec(10, s.n_elem - 1).fill(1e-6);
  arma::mat dataMod = U * arma::diagmat(s) * V.t();

  // Add some noise.
  dataMod += 1e-5 * arma::randu<arma::mat>(dataMod.n_rows, dataMod.n_cols);

  // Calculate the true kernel matrix.
  LinearKernel lk;
  arma::mat kernel = dataMod.t() * dataMod;

  // Now use the linear kernel to get a Nystroem approximation; try this several
  // times.
  double normalizedFroAverage = 0.0;
  for (size_t trial = 0; trial < 20; ++trial)
  {
    LinearKernel lk;
    NystroemMethod<LinearKernel, RandomSelection> nm(dataMod, lk, 10);

    arma::mat g;
    nm.Apply(g);

    arma::mat approximation = g * g.t();

    // Check the normalized Frobenius norm.
    const double normalizedFro = arma::norm(kernel - approximation, "fro") /
        arma::norm(kernel, "fro");

    normalizedFroAverage += normalizedFro;
  }

  normalizedFroAverage /= 20;
  BOOST_REQUIRE_SMALL(normalizedFroAverage, 1e-3);
}

/**
 * Can we reproduce the results in Zhang, Tsang, and Kwok (2008)?
 * They provide the following test points (approximately) in their experiments
 * in Section 4.1, for the german dataset:
 *
 *  rank = 0.02n; approximation error: ~27
 *  rank = 0.04n; approximation error: ~15
 *  rank = 0.06n; approximation error: ~10
 *  rank = 0.08n; approximation error: ~7
 *  rank = 0.10n; approximation error: ~3
 */
BOOST_AUTO_TEST_CASE(GermanTest)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("german.csv", dataset, true);

  // These are our tolerance bounds.
  double results[5] = { 32.0, 20.0, 15.0, 12.0, 9.0 };

  // The bandwidth of the kernel is selected to be the half the average
  // distance between each point and the mean of the dataset.  This isn't
  // _exactly_ what the paper says, but I've modified what it said because our
  // formulation of what the Gaussian kernel is is different.
  GaussianKernel gk(16.461);

  // Calculate the true kernel matrix.
  arma::mat kernel(dataset.n_cols, dataset.n_cols);
  for (size_t i = 0; i < dataset.n_cols; ++i)
    for (size_t j = 0; j < dataset.n_cols; ++j)
      kernel(i, j) = gk.Evaluate(dataset.col(i), dataset.col(j));

  for (size_t trial = 0; trial < 5; ++trial)
  {
    // We will repeat each trial 20 times.
    double avgError = 0.0;
    for (size_t z = 0; z < 20; ++z)
    {
      NystroemMethod<GaussianKernel, KMeansSelection<> > nm(dataset, gk,
          size_t((double((trial + 1) * 2) / 100.0) * dataset.n_cols));
      arma::mat g;
      nm.Apply(g);

      // Reconstruct kernel matrix.
      arma::mat approximation = g * g.t();

      const double error = arma::norm(kernel - approximation, "fro");
      if (error != error)
      {
        // Sometimes K' is singular.  Unlucky.
        --z;
        continue;
      }
      else
      {
        Log::Debug << "Trial " << trial << ": error " << error << ".\n";
        avgError += arma::norm(kernel - approximation, "fro");
      }
    }

    avgError /= 20;

    // Ensure that this is within tolerance, which is at least as good as the
    // paper's results (plus a little bit for noise).
    BOOST_REQUIRE_SMALL(avgError, results[trial]);
  }
}

BOOST_AUTO_TEST_SUITE_END();


//------------------------------------------------------------------------------
/// \file ContinuationMonad_tests.cpp
/// \author Ernest Yeung
//------------------------------------------------------------------------------
//#include "Categories/Monads/ContinuationMonad.h"

#define BOOST_THREAD_PROVIDES_FUTURE
#define BOOST_THREAD_PROVIDES_FUTURE_CONTINUATION

#include "Categories/Monads/ContinuationMonad.h"

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <future>
#include <optional>
#include <string>
#include <utility>

using Categories::Monads::ContinuationMonad::AsLambdas::eval;
using Categories::Monads::ContinuationMonad::AsLambdas::return_;
using Categories::Monads::ContinuationMonad::AsLambdas::runContinuation;
using Categories::Monads::ContinuationMonad::apply_endomorphism;
using Categories::Monads::ContinuationMonad::evaluate;
using Categories::Monads::ContinuationMonad::unit;

using namespace Categories::Monads::ContinuationMonad;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(Monads)
BOOST_AUTO_TEST_SUITE(ContinuationMonad_tests)

// Test morphisms.

std::optional<std::string> user_full_name(const std::string name)
{
  if (name.empty() || name == "None")
  {
    return {};
  }
  else
  {
    return std::make_optional("Username: " + name);
  }
}

std::optional<std::string> to_html(const std::string name)
{
  if (name.empty() || name == "NA")
  {
    return {};
  }
  else
  {
    return std::make_optional("http://" + name);
  }
}

std::pair<bool, std::string> h(const std::optional<std::string> text)
{
  if (!text)
  {
    return std::make_pair(false, "");
  }
  if (text.value() == "What you want.")
  {
    return std::make_pair(true, text.value());
  }
  else
  {
    return std::make_pair(false, text.value());
  }
}

template <int Divisor>
std::pair<bool, int> integer_division(const int input)
{
  if (Divisor == 0)
  {
    return std::make_pair(false, input);
  }
  else
  {
    return std::make_pair(true, input / Divisor);
  }
}

BOOST_AUTO_TEST_SUITE(Endomorphisms)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ApplyEndomorphismAppliesInternalHomToInput)
{
  {
    const auto result = apply_endomorphism(user_full_name, "Sainte-Foy");
    BOOST_TEST(result.value() == "Username: Sainte-Foy");
  }
  {
    const auto result = apply_endomorphism(to_html, "Sainte-Foy");
    BOOST_TEST(result.value() == "http://Sainte-Foy");
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(RunContinuationAppliesInternalHomToInput)
{
  {
    const auto result = runContinuation(user_full_name, "Sainte-Foy");
    BOOST_TEST(result.value() == "Username: Sainte-Foy");
  }
  {
    const auto result = runContinuation(to_html, "Sainte-Foy");
    BOOST_TEST(result.value() == "http://Sainte-Foy");
  }
}

BOOST_AUTO_TEST_SUITE_END() // Endomorphisms

BOOST_AUTO_TEST_SUITE(Return)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(TestMorphismsWork)
{
  {
    auto result = user_full_name("Sainte-Foy");
    BOOST_TEST(result.value() == "Username: Sainte-Foy");
  }
  {
    const auto result1 = integer_division<2>(5);
    BOOST_TEST(result1.first);
    BOOST_TEST(result1.second == 2);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ReturnReturnsEndomorphismObject)
{
  auto returned_unit = return_("Sainte-Foy");
  {
    const auto result = returned_unit(user_full_name);
    BOOST_TEST(result.value() == "Username: Sainte-Foy");
  }
  {
    const auto result = returned_unit(to_html);
    BOOST_TEST(result.value() == "http://Sainte-Foy");
  }
  auto returned_unit1 = return_(10);
  {
  }
}

BOOST_AUTO_TEST_SUITE_END() // Return

// cf. https://github.com/dbrattli/OSlash/blob/master/tests/test_cont.py

BOOST_AUTO_TEST_SUITE(UnitComponent)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(
  UnitComponentReturnsInternalHomMappingInternalHomToReturnType)
{
  auto returned_endomorphism = unit<std::string>("Helene Desportes");
  {
    const auto result = returned_endomorphism(user_full_name);
    BOOST_TEST(result.value() == "Username: Helene Desportes");
  }
  {
    const auto result = returned_endomorphism(to_html);
    BOOST_TEST(result.value() == "http://Helene Desportes");
  }
}

// cf. https://github.com/dbrattli/OSlash/blob/master/tests/test_cont.py
// The following reproduces the unit tests in the referenced link.

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitComponentDelaysComputation)
{
  auto test_internal_hom = [](const int value)
  {
    return std::to_string(value);
  };
  {
    auto addition = [](const auto x, const auto y)
    {
      return unit(x + y);
    };
    auto square = [](const auto x)
    {
      return unit(x * x);
    };
    // Results is of type [[X, Y], Y]
    auto delay_addition = addition(25, 9);
    auto delay_square = square(5);

    const auto addition_result = delay_addition(test_internal_hom);
    const auto square_result = delay_square(test_internal_hom);

    BOOST_TEST_REQUIRE(addition_result == "34");
    BOOST_TEST_REQUIRE(square_result == "25");
    BOOST_TEST(addition(25, 9)(evaluate<int>) == 34);
    {
      auto create_add_x = [](const int x)
      {
        return [x](const auto y)
        {
          return (x + y);
        };
      };
      auto add_6 = create_add_x(6);
      auto add_7 = create_add_x(7);

      BOOST_TEST(delay_square(add_6) == 31);
      BOOST_TEST(delay_square(add_7) == 32);
      BOOST_TEST(square(5)(add_6) == 31);
      BOOST_TEST(square(5)(add_7) == 32);

      auto intermediate = square(5)(
        [addition](const int xx)
        {
          return addition(xx, 8);
        });

      BOOST_TEST_REQUIRE(intermediate(evaluate<int>) == 33);

      auto intermediate_f = [square, addition](const int x, const int y)
      {
        return square(x)(
          [addition, y](const int xx)
          {
            return addition(xx, y);
          });
      };

      auto intermediate_f_result = intermediate_f(3, 6)(evaluate<int>);
      BOOST_TEST(intermediate_f_result == 15);

      auto pythagoras_formula = [addition, square](const int x, const int y)
      {
        return square(x)(
          [addition, square, y](const int xx)
          {
            return square(y)(
              [addition, xx](const int yy)
              {
                return addition(xx, yy);
              });
          });
      };
      BOOST_TEST(pythagoras_formula(5, 6)(evaluate<int>) == 61);
      BOOST_TEST(pythagoras_formula(4, 4)(evaluate<int>) == 32);
    }
  }
  {
    auto addition = [](const auto x, const auto y)
    {
      return return_(x + y);
    };
    auto square = [](const auto x)
    {
      return return_(x * x);
    };
    auto delay_addition = addition(25, 9);
    auto delay_square = square(5);

    const auto addition_result = delay_addition(test_internal_hom);
    const auto square_result = delay_square(test_internal_hom);

    BOOST_TEST_REQUIRE(addition_result == "34");
    BOOST_TEST_REQUIRE(square_result == "25");

    {
      auto pythagoras_formula = [addition, square](const int x, const int y)
      {
        return square(x)(
          [addition, square, y](const int xx)
          {
            return square(y)(
              [addition, xx](const int yy)
              {
                return addition(xx, yy);
              });
          });
      };
      BOOST_TEST(pythagoras_formula(5, 6)(evaluate<int>) == 61);
      BOOST_TEST(pythagoras_formula(4, 4)(evaluate<int>) == 32);
    }
  }
}

int pair_add(const std::pair<int, int> inputs)
{
  return inputs.first + inputs.second;
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UnitComponentTreatsFunctionsAsFirstClassValues)
{
  {
    auto pair_addition = unit(pair_add);
    BOOST_TEST_REQUIRE(pair_addition(eval)(std::make_pair<int, int>(40, 2)));
    BOOST_TEST(unit(pair_add)(eval)(std::make_pair<int, int>(40, 2)));
  }
  {
    auto pair_addition = return_(pair_add);
    BOOST_TEST_REQUIRE(pair_addition(eval)(std::make_pair<int, int>(40, 2)));
    BOOST_TEST(return_(pair_add)(eval)(std::make_pair<int, int>(40, 2)));
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SimpleUnitComponentExamples)
{
  auto morphism_c = [](const auto x) -> std::string
  {
    return "Done: " + std::to_string(x);
  };
  {
    auto f = [](const auto x)
    {
      return unit(std::pow(x, 3));
    };
    auto g = [](const auto x)
    {
      return unit(x - 2);
    };
    auto h = [f, g](const auto x)
    {
      return (x == 5) ? f(x) : g(x);
    };
    auto do_c = unit(4.0)(h);
    BOOST_TEST_REQUIRE(do_c(evaluate<float>) == 2.0);
    BOOST_TEST(do_c(morphism_c) == "Done: 2.000000");
  }
  {
    auto f = [](const auto x)
    {
      return return_(std::pow(x, 3));
    };
    auto g = [](const auto x)
    {
      return return_(x - 2);
    };
    auto h = [f, g](const auto x)
    {
      return (x == 5) ? f(x) : g(x);
    };
    auto do_c = return_(4.0)(h);
    BOOST_TEST_REQUIRE(do_c(evaluate<float>) == 2.0);
    BOOST_TEST(do_c(morphism_c) == "Done: 2.000000");
  }
}

BOOST_AUTO_TEST_SUITE_END() // UnitComponent

BOOST_AUTO_TEST_SUITE(Bind)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BindAsLambdas)
{
  auto composed_fg = AsLambdas::bind(to_html, user_full_name);
//  composed_fg(h);

}

BOOST_AUTO_TEST_SUITE_END() // Bind

BOOST_AUTO_TEST_SUITE_END() // ContinuationMonad_tests
BOOST_AUTO_TEST_SUITE_END() // Monads
BOOST_AUTO_TEST_SUITE_END() // Categories


#pragma once
#include "Optimizer.hpp"

#include <Eigen/Dense>
#include <array>
#include <cmath>
#include <nlohmann/json.hpp>

namespace yavque
{
class SGD : public Optimizer
{
public:
	static constexpr std::array<double, 3> DEFAULT_PARAMS = {0.01, 0.0, 1e-4};

private:
	const double alpha_;
	const double p_;
	const double min_alpha_;
	int t_ = 0;

public:
	explicit SGD(double alpha = DEFAULT_PARAMS[0], double p = DEFAULT_PARAMS[1],
	             double min_alpha = DEFAULT_PARAMS[2])
		: alpha_{alpha}, p_{p}, min_alpha_{min_alpha}
	{
	}

	explicit SGD(const nlohmann::json& params)
		: alpha_{params.value("alpha", DEFAULT_PARAMS[0])}, p_{params.value(
																"p", DEFAULT_PARAMS[1])},
		  min_alpha_{params.value("min_alpha", DEFAULT_PARAMS[2])}
	{
	}

	[[nodiscard]] nlohmann::json desc() const override
	{
		return nlohmann::json{{"name", "SGD"},
		                      {"alhpa", alpha_},
		                      {"p", p_},
		                      {"min_alpha", min_alpha_}};
	}

	Eigen::VectorXd getUpdate(const Eigen::VectorXd& v) override
	{
		using std::pow;
		++t_;
		double eta = std::max((alpha_ / pow(t_, p_)), min_alpha_);
		return -eta * v;
	}
};
} // namespace yavque


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

#include "ArithmeticEncoder.hpp"
#include "ArithmeticDecoder.hpp"

#include "catch.hpp"

#include <random>
#include <boost/iostreams/stream.hpp>

#include "UniformEncodingDistribution.hpp"
#include "NormalEncodingDistribution.hpp"
#include "LaplaceEncodingDistribution.hpp"

// This tests the boundries between values
void testDistribution(EncodingDistribution& _encodeDist, std::pair<int, int> _encodeRange)
{
	for(int v = _encodeRange.first; v < _encodeRange.second; ++v)
	{
		std::pair<ArithmeticEncoder::Range, ArithmeticEncoder::Range>
			range = _encodeDist.getRange(v);

		std::pair<int, std::pair<ArithmeticEncoder::Range, ArithmeticEncoder::Range>>
			v2_ = _encodeDist.getValue(range.first);

		if(range.second - range.first > 0)
		{
			CHECK( v == (int)v2_.first );
			CHECK( range.first == v2_.second.first );
			CHECK( range.second == v2_.second.second );

			v2_ = _encodeDist.getValue(range.second-1);

			CHECK( v == (int)v2_.first );
			CHECK( range.first == v2_.second.first );
			CHECK( range.second == v2_.second.second );
		}
	}
}

void testEncoding(int _seed)
{
	std::default_random_engine generator;
	generator.seed(_seed);
	std::uniform_int_distribution<int> distribution(0,255);

	NormalEncodingDistribution encodeDist({0, 256}, distribution(generator), distribution(generator)+100);

	testDistribution(encodeDist, {0, 256});

	std::vector<char> encodedData(1000);

	boost::iostreams::stream<boost::iostreams::basic_array_sink<char>>
		outDataStream(encodedData.data(),encodedData.size());
	auto streamStart = outDataStream.tellp();

	ArithmeticEncoder encoder(outDataStream);

	std::vector<unsigned char> values(100);
	for(auto& v : values)
		v = distribution(generator);

	for(auto v : values)
		encodeDist.encode(encoder, v);
	encoder.close();

	std::size_t writeSize = outDataStream.tellp() - streamStart;

	outDataStream.close();

	boost::iostreams::stream<boost::iostreams::basic_array_source<char>>
		inDataStream(encodedData.data(),encodedData.size());
	streamStart = inDataStream.tellg();

	ArithmeticDecoder decoder(inDataStream);
	decoder.open();

	for(auto& v : values)
	{
		unsigned char decodedV = encodeDist.decode(decoder);

		REQUIRE( static_cast<int>(decodedV) == static_cast<int>(v) );
	}

	std::size_t readSize = inDataStream.tellg() - streamStart;
	REQUIRE( readSize == writeSize );
}

TEST_CASE( "Test normal distribution", "[normal_distribution]" )
{
	std::pair<int, int> encodeRange{0, 256};

	NormalEncodingDistribution encodeDist(encodeRange, 100, 100);

	testDistribution(encodeDist, encodeRange);
}

TEST_CASE( "Test laplace distribution", "[laplace_distribution]" )
{
	std::pair<int, int> encodeRange{0, 256};

	LaplaceEncodingDistribution encodeDist(encodeRange, 70.239466369334451, 1.2228440911461289);

	testDistribution(encodeDist, encodeRange);
}

TEST_CASE( "Test arithmetic encoding and decoding", "[arithmetic_encoding_decoding]" )
{
	for(int seed = 0; seed < 1000; ++seed)
		testEncoding(seed);
}


#include "benchmarks_general.h"
using benchmarks_general::println;
using benchmarks_general::rtimeit;
using benchmarks_general::unused;

#include <Eigen/Core>

template<typename T, size_t M, size_t K, size_t N>
T single_test() {
    using namespace Eigen;
    Matrix<T,M,K,RowMajor> a; a.setConstant(3);
    Matrix<T,K,N,RowMajor> b; b.setConstant(4);
    Matrix<T,M,N,RowMajor> c = a*b;

    Matrix<T,M,N,RowMajor> tmp; tmp.setConstant(0);
    benchmarks_general::matmul_ref<M,K,N>(a,b,tmp);
    return std::abs(tmp.array().sum() - c.array().sum());
}

template<typename T, size_t M, size_t K, size_t N>
void run_single_test() {
    T value = single_test<T,M,K,N>();
    benchmarks_general::EXIT_ASSERT(std::abs(value) < 1e-10, "TEST FAILED");
}

template<typename T>
void run_tests() {
    TEST_RUN_MATMUL_BENCHMARK(run_single_test, T)
}

template<typename T, size_t M, size_t K, size_t N>
void single_benchmark() {
    using namespace Eigen;
    Matrix<T,M,K,RowMajor> a; a.setConstant(3);
    Matrix<T,K,N,RowMajor> b; b.setConstant(4);
    Matrix<T,M,N,RowMajor> c = a*b;
    unused(c);
}

template<typename T, size_t M, size_t K, size_t N>
void run_single_benchmark() {

    double elapsed_time = rtimeit(static_cast<void (*)()>(&single_benchmark<T,M,K,N>));
    double max_gflops = 2.0 * M * N * K / (elapsed_time * 1.0e9);
    println("(M, N, K):", M, N, K, "GFLOPS:", max_gflops, "minimum runtime:", elapsed_time,'\n');
}

template<typename T>
void run_benchmarks() {
    TEST_RUN_MATMUL_BENCHMARK(run_single_benchmark, T)
}

int main() {

#ifdef RUN_SINGLE
    println("Running eigen benchmark: single precision\n");
    run_tests<float>();
    run_benchmarks<float>();
#else
    println("Running eigen benchmark: double precision\n");
    run_tests<double>();
    run_benchmarks<double>();
#endif

    return 0;
}


// This example consists of a single constant velocity target which
// moves under piecewise constant velocity in 3D. Its position is
// measured by an idealised GPS receiver.

#include <Eigen/StdVector>
#include <iostream>

#include <stdint.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes6D.hpp"
#include "continuous_to_discrete.h"

using namespace Eigen;
using namespace std;
using namespace g2o;

int main()
{
  // Set up the parameters of the simulation
  int numberOfTimeSteps = 1000;
  const double processNoiseSigma = 1;
  const double accelerometerNoiseSigma = 1;
  const double gpsNoiseSigma = 1;
  const double dt = 1;

  // Set up the optimiser and block solver
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  typedef BlockSolver< BlockSolverTraits<6, 6> > BlockSolver;
  BlockSolver::LinearSolverType * linearSolver
      = new LinearSolverCholmod<BlockSolver::PoseMatrixType>();
  BlockSolver* blockSolver = new BlockSolver(linearSolver);
  OptimizationAlgorithm* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(optimizationAlgorithm);

  // Sample the start location of the target
  Vector6d state;
  state.setZero();
  for (int k = 0; k < 3; k++)
    {
      state[k] = 1000 * sampleGaussian();
    }

  // Construct the first vertex; this corresponds to the initial
  // condition and register it with the optimiser
  VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();
  stateNode->setEstimate(state);
  stateNode->setId(0);
  optimizer.addVertex(stateNode);

  // Set up last estimate
  VertexPositionVelocity3D* lastStateNode = stateNode;

  // Iterate over the simulation steps
  for (int k = 1; k <= numberOfTimeSteps; ++k)
    {
      // Simulate the next step; update the state and compute the observation
      Vector3d processNoise(processNoiseSigma*sampleGaussian(),
                            processNoiseSigma*sampleGaussian(),
                            processNoiseSigma*sampleGaussian());

      for (int m = 0; m < 3; m++)
        {
          state[m] += dt * (state[m+3] + 0.5 * dt * processNoise[m]);
        }

      for (int m = 0; m < 3; m++)
        {
          state[m+3] += dt * processNoise[m];
        }

      // Construct the accelerometer measurement
      Vector3d accelerometerMeasurement;
      for (int m = 0; m < 3; m++)
        {
          accelerometerMeasurement[m] = processNoise[m] + accelerometerNoiseSigma * sampleGaussian();
        }

      // Construct the GPS observation
      Vector3d gpsMeasurement;
      for (int m = 0; m < 3; m++)
        {
          gpsMeasurement[m] = state[m] + gpsNoiseSigma * sampleGaussian();
        }

      // Construct vertex which corresponds to the current state of the target
      VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();

      stateNode->setId(k);
      stateNode->setMarginalized(false);
      optimizer.addVertex(stateNode);

      TargetOdometry3DEdge* toe = new TargetOdometry3DEdge(dt, accelerometerNoiseSigma);
      toe->setVertex(0, lastStateNode);
      toe->setVertex(1, stateNode);
      VertexPositionVelocity3D* vPrev= dynamic_cast<VertexPositionVelocity3D*>(lastStateNode);
      VertexPositionVelocity3D* vCurr= dynamic_cast<VertexPositionVelocity3D*>(stateNode);
      toe->setMeasurement(accelerometerMeasurement);
      optimizer.addEdge(toe);

      // compute the initial guess via the odometry
      g2o::OptimizableGraph::VertexSet vPrevSet;
      vPrevSet.insert(vPrev);
      toe->initialEstimate(vPrevSet,vCurr);

      lastStateNode = stateNode;

      // Add the GPS observation
      GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
      goe->setVertex(0, stateNode);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);
  cerr << "number of vertices:" << optimizer.vertices().size() << endl;
  cerr << "number of edges:" << optimizer.edges().size() << endl;

  // Print the results

  cout << "state=\n" << state << endl;

#if 0
  for (int k = 0; k < numberOfTimeSteps; k++)
    {
      cout << "computed estimate " << k << "\n"
           << dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(k)->second)->estimate() << endl;
       }
#endif

  Vector6d v1 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-2,0))->second)->estimate();
  Vector6d v2 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();
  cout << "v1=\n" << v1 << endl;
  cout << "v2=\n" << v2 << endl;
  cout << "delta state=\n" << v2-v1 << endl;
}