#include <gnuplot-iostream/gnuplot-iostream.h>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <iomanip>
#include <functional>
#include <vector>
#include <cmath>
#include "Tools.hpp"
#define PI 3.14159265
using namespace std;

/***********************************************************************

Second Case: 2D drag! DragObject2D is a class for a point particle in
2D coordinates. Instead of individual accessor methods like Euler1D,
getAttrs returns a vector of the four instance variables. The Euler
step and updating code is all built in as well. A frequently used data
type in this program is vector<vector<double>>, usually called path.
The data stored is a vector of x values and y values in a projectile's
trajectory, ex. {{x1,x2,x3...},{y1,y2,y3...}}. Passing a data type like
this around is much easier than writing two methods or using evil
libraries to make functions return multiple values.

The getPath function takes an initial x and y velocity and performs the
Euler method on an instance of DragObject2D, saving the x and y values
in vectors. It simulates until a sign change is detected in y, meaning
the object has returned to the ground. For that reason,
making v_i negative is a bad idea. After simulation, the function
returns a path. plotPath just plots the path given with axes of path[0],
path[1].

RangeVAngle is more complicated, as it performs all calculation and
plots the range vs. angle graph. It iterates from -pi/2 to pi/2 with a
step size of dth. For each iteration, it gets the path with getPath. The
final x value is the range when the object returns to the ground, by
the definition of the getPath function.

***********************************************************************/

const double v_i = 10;		// Initial velocity for both the trajectory and the range v. angle plot
const double th = PI/6;		// Firing angle for the trajectory plot
const double y_0 = 1;		// Global starting height, Should be either zero or something positive, as the range vs. angle graph must be complete.

const double g = 9.8;		// Gravitational acceleration (negative)
const double b = 0;//.56832;	// Calculated drag constant for Syd Miyasaki
const double dt = 0.001;	// Timestep
const double dth = 0.001;	// Step in angle for the range v. angle plot

class DragObject2D{	// Problem-specific class
public:
	DragObject2D(double vx_0, double vy_0);	// Constructor
	vector<double> getAttrs() const;		// Accessor method for instance variables
	double getSpeed() const;				// Accessor / calculation method for speed
	void update();	// Euler step and updating
private:
	double x, y, vx, vy;
};

DragObject2D::DragObject2D(double vx_0, double vy_0){	// Constructor sets x to zero, y to starting height, initial velocities as arguments
	x = 0;
	y = y_0;
	vx = vx_0;
	vy = vy_0;
}

vector<double> DragObject2D::getAttrs() const {	// Usage of anonymously declared vectors to return all instance variables
	return vector<double>{x,y,vx,vy};
}

double DragObject2D::getSpeed() const {	// *trivial* mechanics (component velocity to speed)
	return sqrt(vx*vx + vy*vy);
}

void DragObject2D::update(){	// Update all the things!
	double v = this->getSpeed();// Get speed on the current object instance. "this" returns a pointer to the instance, and -> is an abbreviation of (*this).getSpeed()
	x += vx*dt;				// Update x and y
	y += vy*dt;

	vx += -(b*v*vx)*dt;		// Euler step for x and y
	vy += -(g+b*v*vy)*dt;
}

vector<vector<double>> getPath(double vx_0, double vy_0){	// Get the path thing
	vector<double> x_n;	// Vectors for the path
	vector<double> y_n;
	double yval = 0;	// The value of y

	DragObject2D object(vx_0, vy_0);// The object to track

	while(yval >= 0){				// Only track until the object hits the ground
		vector<double> attrs = object.getAttrs();	// Get attributes (x, y, vx, vy)
		x_n.push_back(attrs[0]);	// Add attributes to vectors, update y value
		y_n.push_back(attrs[1]);
		yval = attrs[1];

		object.update();			// Perform Euler step and updates
	}

	return vector<vector<double>>{x_n,y_n};	// Return the two vectors of x and y together
}

void plotPath(vector<vector<double>>& path){	// Plot the path; pretty basic stuff
	Gnuplot gp;

	gp << setprecision(3);
	gp << "set xrange [0:" << path[0].back() << "]\n";
	gp << "set yrange [0:" << getMaxVal(path[1])*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"x (m)\"\n";
	gp << "set ylabel \"y (m)\"\n";
	gp << "set title \"Trajectory of Syd Under Rayleigh's Drag Equation\\nb = " << b;
	gp << ", v_i = " << v_i << ", {/Symbol q}_i = " << th << "\"\n";
	gp << "set output \"Trajectory.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(path[0],path[1]));
}

void rangeVAngle(){			// Calculation and plotting of the range vs. angle plot
	vector<double> ranges;	// Declare vectors
	vector<double> angles;

	for(double ang = -PI/2; ang < PI/2; ang += dth){	// From -pi/2 to pi/2, steps of dth

		double vx_0 = v_i*cos(ang);	// From v_i and ang, determine the initial velocities
		double vy_0 = v_i*sin(ang);
		vector<vector<double>> path = getPath(vx_0, vy_0);	// Generate path
		double range = path[0].back();	// The last x value is the range

		angles.push_back(ang);	// Save angle and range
		ranges.push_back(range);
	}

	Gnuplot gp;		// Now plot!

	gp << setprecision(3);
	gp << "set xrange [" << -PI/2 << ":" << PI/2 << "]\n";
	gp << "set yrange [0:" << getMaxVal(ranges)*1.1 << "]\n";
	gp << "set format y \"%.1f\"\n";
	gp << "set term png size 720,480 font \"FreeSerif,12\"\n";
	gp << "set xlabel \"angle (rad)\"\n";
	gp << "set ylabel \"range (m)\"\n";
	gp << "set title \"Range vs. Firing Angle of Syd Under Rayleigh's Drag Equation, b = " << b << "\"\n";
	gp << "set output \"RangeAngle.png\"\n";
	gp << "plot '-' with dots lc rgb \"black\" notitle\n";
	gp.send1d(boost::make_tuple(angles,ranges));
}

int main(){		// Main function
	double vx_0 = v_i*cos(th);	// Use v_i and the launch angle th to determine initial velocities by components
	double vy_0 = v_i*sin(th);
	vector<vector<double>> path = getPath(vx_0, vy_0);	// Generate the path
	plotPath(path);	// Plot it

	rangeVAngle();	// And make the range vs. angle plot!

	return 0;	// If it all worked, return 0
}


/*
 * CurvatureFilter.cpp
 *
 *  Created on: Sep 23, 2017
 *      Author: Peter Fankhauser
 *   Institute: ETH Zurich, ANYbotics
 */

#include <grid_map_filters/CurvatureFilter.hpp>

#include <grid_map_core/grid_map_core.hpp>
#include <pluginlib/class_list_macros.h>

#include <Eigen/Dense>

using namespace filters;

namespace grid_map {

template<typename T>
CurvatureFilter<T>::CurvatureFilter()
{
}

template<typename T>
CurvatureFilter<T>::~CurvatureFilter()
{
}

template<typename T>
bool CurvatureFilter<T>::configure()
{
  if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
    ROS_ERROR("Curvature filter did not find parameter `input_layer`.");
    return false;
  }
  ROS_DEBUG("Curvature filter input layer is = %s.", inputLayer_.c_str());

  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
    ROS_ERROR("Curvature filter did not find parameter `output_layer`.");
    return false;
  }
  ROS_DEBUG("Curvature filter output_layer = %s.", outputLayer_.c_str());

  return true;
}

template<typename T>
bool CurvatureFilter<T>::update(const T& mapIn, T& mapOut)
{
  if (!mapIn.isDefaultStartIndex()) throw std::runtime_error(
      "CurvatureFilter cannot be used with grid maps that don't have a default buffer start index.");

  mapOut = mapIn;
  mapOut.add(outputLayer_);
  auto& input = mapOut[inputLayer_];
  auto& curvature = mapOut[outputLayer_];
  const float L2 = mapOut.getResolution() * mapOut.getResolution();

  for (Eigen::Index j{0}; j < input.cols(); ++j) {
    for (Eigen::Index i{0}; i < input.rows(); ++i) {
      // http://help.arcgis.com/en/arcgisdesktop/10.0/help/index.html#/How_Curvature_works/00q90000000t000000/
      if (!std::isfinite(input(i, j))) continue;
      float D = ((input(i, j==0 ? j : j-1) + input(i, j==input.cols()-1 ? j : j + 1)) / 2.0 - input(i, j)) / L2;
      float E = ((input(i==0 ? i : i-1, j) + input(i==input.rows()-1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2;
      if (!std::isfinite(D)) D = 0.0;
      if (!std::isfinite(E)) E = 0.0;
      curvature(i, j) = -2.0 * (D + E);
    }
  }

  return true;
}

} /* namespace */

PLUGINLIB_EXPORT_CLASS(grid_map::CurvatureFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)


/* Boost example/filter.cpp
 * two examples of filters for computing the sign of a determinant
 * the second filter is based on an idea presented in
 * "Interval arithmetic yields efficient dynamic filters for computational
 * geometry" by Brönnimann, Burnikel and Pion, 2001
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
#include <cstring>
#include <iostream>

namespace dummy {
  using namespace boost;
  using namespace numeric;
  using namespace interval_lib;
  typedef save_state<rounded_arith_opp<double> > R;
  typedef checking_no_nan<double, checking_no_empty<double> > P;
  typedef interval<double, policies<R, P> > I;
}

template<class T>
class vector {
  T* ptr;
public:
  vector(int d) { ptr = (T*)malloc(sizeof(T) * d); }
  ~vector() { free(ptr); }
  const T& operator[](int i) const { return ptr[i]; }
  T& operator[](int i) { return ptr[i]; }
};

template<class T>
class matrix {
  int dim;
  T* ptr;
public:
  matrix(int d): dim(d) { ptr = (T*)malloc(sizeof(T) * dim * dim); }
  ~matrix() { free(ptr); }
  int get_dim() const { return dim; }
  void assign(const matrix<T> &a) { memcpy(ptr, a.ptr, sizeof(T) * dim * dim); }
  const T* operator[](int i) const { return &(ptr[i * dim]); }
  T* operator[](int i) { return &(ptr[i * dim]); }
};

typedef dummy::I I_dbl;

/* compute the sign of a determinant using an interval LU-decomposition; the
   function answers 1 or -1 if the determinant is positive or negative (and
   more importantly, the result must be provable), or 0 if the algorithm was
   unable to get a correct sign */
int det_sign_algo1(const matrix<double> &a) {
  int dim = a.get_dim();
  vector<int> p(dim);
  for(int i = 0; i < dim; i++) p[i] = i;
  int sig = 1;
  I_dbl::traits_type::rounding rnd;
  typedef boost::numeric::interval_lib::unprotect<I_dbl>::type I;
  matrix<I> u(dim);
  for(int i = 0; i < dim; i++) {
    const double* line1 = a[i];
    I* line2 = u[i];
    for(int j = 0; j < dim; j++)
      line2[j] = line1[j];
  }
  // computation of L and U
  for(int i = 0; i < dim; i++) {
    // partial pivoting
    {
      int pivot = i;
      double max = 0;
      for(int j = i; j < dim; j++) {
        const I &v = u[p[j]][i];
        if (zero_in(v)) continue;
        double m = norm(v);
        if (m > max) { max = m; pivot = j; }
      }
      if (max == 0) return 0;
      if (pivot != i) {
        sig = -sig;
        int tmp = p[i];
        p[i] = p[pivot];
        p[pivot] = tmp;
      }
    }
    // U[i,?]
    {
      I *line1 = u[p[i]];
      const I &pivot = line1[i];
      if (boost::numeric::interval_lib::cerlt(pivot, 0.)) sig = -sig;
      for(int k = i + 1; k < dim; k++) {
        I *line2 = u[p[k]];
        I fact = line2[i] / pivot;
        for(int j = i + 1; j < dim; j++) line2[j] -= fact * line1[j];
      }
    }
  }
  return sig;
}

/* compute the sign of a determinant using a floating-point LU-decomposition
   and an a posteriori interval validation; the meaning of the answer is the
   same as previously */
int det_sign_algo2(const matrix<double> &a) {
  int dim = a.get_dim();
  vector<int> p(dim);
  for(int i = 0; i < dim; i++) p[i] = i;
  int sig = 1;
  matrix<double> lui(dim);
  {
    // computation of L and U
    matrix<double> lu(dim);
    lu.assign(a);
    for(int i = 0; i < dim; i++) {
      // partial pivoting
      {
        int pivot = i;
        double max = std::abs(lu[p[i]][i]);
        for(int j = i + 1; j < dim; j++) {
          double m = std::abs(lu[p[j]][i]);
          if (m > max) { max = m; pivot = j; }
        }
        if (max == 0) return 0;
        if (pivot != i) {
          sig = -sig;
          int tmp = p[i];
          p[i] = p[pivot];
          p[pivot] = tmp;
        }
      }
      // L[?,i] and U[i,?]
      {
        double *line1 = lu[p[i]];
        double pivot = line1[i];
        if (pivot < 0) sig = -sig;
        for(int k = i + 1; k < dim; k++) {
          double *line2 = lu[p[k]];
          double fact = line2[i] / pivot;
          line2[i] = fact;
          for(int j = i + 1; j < dim; j++) line2[j] -= line1[j] * fact;
        }
      }
    }

    // computation of approximate inverses: Li and Ui
    for(int j = 0; j < dim; j++) {
      for(int i = j + 1; i < dim; i++) {
        double *line = lu[p[i]];
        double s = - line[j];
        for(int k = j + 1; k < i; k++) s -= line[k] * lui[k][j];
        lui[i][j] = s;
      }
      lui[j][j] = 1 / lu[p[j]][j];
      for(int i = j - 1; i >= 0; i--) {
        double *line = lu[p[i]];
        double s = 0;
        for(int k = i + 1; k <= j; k++) s -= line[k] * lui[k][j];
        lui[i][j] = s / line[i];
      }
    }
  }

  // norm of PAUiLi-I computed with intervals
  {
    I_dbl::traits_type::rounding rnd;
    typedef boost::numeric::interval_lib::unprotect<I_dbl>::type I;
    vector<I> m1(dim);
    vector<I> m2(dim);
    for(int i = 0; i < dim; i++) {
      for(int j = 0; j < dim; j++) m1[j] = 0;
      const double *l1 = a[p[i]];
      for(int j = 0; j < dim; j++) {
        double v = l1[j];    // PA[i,j]
        double *l2 = lui[j]; // Ui[j,?]
        for(int k = j; k < dim; k++) {
          using boost::numeric::interval_lib::mul;
          m1[k] += mul<I>(v, l2[k]); // PAUi[i,k]
        }
      }
      for(int j = 0; j < dim; j++) m2[j] = m1[j]; // PAUi[i,j] * Li[j,j]
      for(int j = 1; j < dim; j++) {
        const I &v = m1[j];  // PAUi[i,j]
        double *l2 = lui[j]; // Li[j,?]
        for(int k = 0; k < j; k++)
          m2[k] += v * l2[k]; // PAUiLi[i,k]
      }
      m2[i] -= 1; // PAUiLi-I
      double ss = 0;
      for(int i = 0; i < dim; i++) ss = rnd.add_up(ss, norm(m2[i]));
      if (ss >= 1) return 0;
    }
  }
  return sig;
}

int main() {
  int dim = 20;
  matrix<double> m(dim);
  for(int i = 0; i < dim; i++) for(int j = 0; j < dim; j++)
    m[i][j] = /*1 / (i-j-0.001)*/ cos(1+i*sin(1 + j)) /*1./(1+i+j)*/;

  // compute the sign of the determinant of a "strange" matrix with the two
  // algorithms, the first should fail and the second succeed
  std::cout << det_sign_algo1(m) << " " << det_sign_algo2(m) << std::endl;
}


/*
 * @brief Functions for computing Bezier curves and their derivatives.
 * @author Jenna Reher (jreher@caltech.edu)
 */

#ifndef BEZIER_TOOLS_HPP
#define BEZIER_TOOLS_HPP

#include <Eigen/Dense>

namespace bezier_tools {

double singleterm_bezier(int m, int k, double s);
double bezier(const Eigen::VectorXd &coeff, double s);
void bezier(const Eigen::MatrixXd &coeffs, double s, Eigen::VectorXd &out);
double dbezier(const Eigen::VectorXd &coeff, double s);
void dbezier(const Eigen::MatrixXd &coeffs, double s, Eigen::VectorXd &out);
double d2bezier(const Eigen::VectorXd &coeff, double s);
void d2bezier(const Eigen::MatrixXd &coeffs, double s, Eigen::VectorXd &out);

}

#endif // BEZIER_TOOLS_HPP


//
// Created by jhwangbo on 30.11.16.
//

#ifndef RAI_TYPEDEF_HPP
#define RAI_TYPEDEF_HPP

#include <Eigen/Core>

namespace rai {

typedef typename Eigen::Matrix<double, 4, 1> Quaternion;
typedef typename Eigen::Matrix<double, 3, 1> EulerVector;
typedef typename Eigen::Matrix<double, 3, 3> RotationMatrix;
typedef typename Eigen::Matrix<double, 4, 4> HomogeneousTransform;
typedef typename Eigen::Matrix<double, 3, 1> Position;
typedef typename Eigen::Matrix<double, 3, 1> AngularVelocity;
typedef typename Eigen::Matrix<double, 3, 1> AngularAcceleration;
typedef typename Eigen::Matrix<double, 3, 1> LinearVelocity;
typedef typename Eigen::Matrix<double, 3, 1> LinearAcceleration;
typedef typename Eigen::Matrix<double, 3, 1> Axis;
typedef typename Eigen::Matrix<double, 3, 1> Torque;
typedef typename Eigen::Matrix<double, 3, 1> Force;
typedef typename Eigen::Matrix<double, 3, 3> Inertia;

typedef typename Eigen::Matrix<float, 4, 1> Quaternionf;
typedef typename Eigen::Matrix<float, 3, 1> EulerVectorf;
typedef typename Eigen::Matrix<float, 3, 3> RotationMatrixf;
typedef typename Eigen::Matrix<float, 3, 1> Positionf;
typedef typename Eigen::Matrix<float, 3, 1> AngularVelocityf;
typedef typename Eigen::Matrix<float, 3, 1> AngularAccelerationf;
typedef typename Eigen::Matrix<float, 3, 1> LinearVelocityf;
typedef typename Eigen::Matrix<float, 3, 1> LinearAccelerationf;
typedef typename Eigen::Matrix<float, 3, 1> Axisf;
typedef typename Eigen::Matrix<float, 3, 1> Torquef;
typedef typename Eigen::Matrix<float, 3, 1> Forcef;
typedef typename Eigen::Matrix<float, 3, 3> Inertiaf;

typedef typename Eigen::MatrixXd MatrixXd;
typedef typename Eigen::VectorXd VectorXd;

}

#endif //RAI_TYPEDEF_HPP


/*
 * @Description: ceres residual block for LIO IMU pre-integration measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_IMU_PRE_INTEGRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_IMU_PRE_INTEGRATION_HPP_

#include <ceres/ceres.h>

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include "glog/logging.h"

namespace sliding_window {

class FactorPRVAGIMUPreIntegration : public ceres::SizedCostFunction<15, 15, 15> {
public:
	static const int INDEX_P = 0;
	static const int INDEX_R = 3;
	static const int INDEX_V = 6;
	static const int INDEX_A = 9;
	static const int INDEX_G = 12;

  FactorPRVAGIMUPreIntegration(void) {};

	void SetT(const double &T) {
		T_ = T;
	}

	void SetGravitiy(const Eigen::Vector3d &g) {
		g_ = g;
	}

  void SetMeasurement(const Eigen::VectorXd &m) {
		m_ = m;
	}

  void SetInformation(const Eigen::MatrixXd &I) {
    I_ = I;
  }

	void SetJacobian(const Eigen::MatrixXd &J) {
		J_ = J;
	}

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    //
    // parse parameters:
    //
    // a. pose i
    Eigen::Map<const Eigen::Vector3d>     pos_i(&parameters[0][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori_i(&parameters[0][INDEX_R]);
    const Sophus::SO3d                    ori_i = Sophus::SO3d::exp(log_ori_i);
		Eigen::Map<const Eigen::Vector3d>     vel_i(&parameters[0][INDEX_V]);
		Eigen::Map<const Eigen::Vector3d>     b_a_i(&parameters[0][INDEX_A]);
		Eigen::Map<const Eigen::Vector3d>     b_g_i(&parameters[0][INDEX_G]);

    // b. pose j
    Eigen::Map<const Eigen::Vector3d>     pos_j(&parameters[1][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori_j(&parameters[1][INDEX_R]);
    const Sophus::SO3d                    ori_j = Sophus::SO3d::exp(log_ori_j);
		Eigen::Map<const Eigen::Vector3d>     vel_j(&parameters[1][INDEX_V]);
		Eigen::Map<const Eigen::Vector3d>     b_a_j(&parameters[1][INDEX_A]);
		Eigen::Map<const Eigen::Vector3d>     b_g_j(&parameters[1][INDEX_G]);

    //
    // parse measurement:
    //
		const Eigen::Vector3d &alpha_ij = m_.block<3, 1>(INDEX_P, 0);
		const Eigen::Vector3d &theta_ij = m_.block<3, 1>(INDEX_R, 0);
		const Eigen::Vector3d  &beta_ij = m_.block<3, 1>(INDEX_V, 0);

    //
    // TODO: get square root of information matrix:
    //

    //
    // TODO: compute residual:
    //

    //
    // TODO: compute jacobians:
    //
    if ( jacobians ) {
      // compute shared intermediate results:

      if ( jacobians[0] ) {
        // a. residual, position:

        // b. residual, orientation:

        // c. residual, velocity:

        // d. residual, bias accel:

        // d. residual, bias accel:
      }

      if ( jacobians[1] ) {
        // a. residual, position:

        // b. residual, orientation:

        // c. residual, velocity:

        // d. residual, bias accel:

        // d. residual, bias accel:
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //

    return true;
  }

private:
  static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {
      Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();

      double theta = w.norm();

      if ( theta > 1e-5 ) {
          Eigen::Vector3d k = w.normalized();
          Eigen::Matrix3d K = Sophus::SO3d::hat(k);

          J_r_inv = J_r_inv
                    + 0.5 * K
                    + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K;
      }

      return J_r_inv;
  }

	double T_ = 0.0;

	Eigen::Vector3d g_ = Eigen::Vector3d::Zero();

  Eigen::VectorXd m_;
  Eigen::MatrixXd I_;

	Eigen::MatrixXd J_;
};

} // namespace sliding_window

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_IMU_PRE_INTEGRATION_HPP_


/*
 * Copyright 2020 Netherlands eScience Center
 *
 * 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 <Eigen/Core>
#include <Eigen/Dense>
#include <utility>

#include <Spectra/GenEigsComplexShiftSolver.h>
#include <Spectra/GenEigsRealShiftSolver.h>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/DenseGenComplexShiftSolve.h>
#include <Spectra/MatOp/DenseGenMatProd.h>
#include <Spectra/MatOp/DenseGenRealShiftSolve.h>
#include <Spectra/MatOp/DenseSymMatProd.h>
#include <Spectra/MatOp/DenseSymShiftSolve.h>
#include <Spectra/MatOp/SymShiftInvert.h>
#include <Spectra/SymEigsShiftSolver.h>
#include <Spectra/SymEigsSolver.h>
#include <Spectra/SymGEigsShiftSolver.h>

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>

namespace py = pybind11;

using ComplexMatrix = Eigen::MatrixXcd;
using ComplexVector = Eigen::VectorXcd;
using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using Eigen::Index;

Spectra::SortRule string_to_sortrule(const std::string& name)
{
    std::unordered_map<std::string, Spectra::SortRule> rules = {
        {"LargestMagn", Spectra::SortRule::LargestMagn},
        {"LargestReal", Spectra::SortRule::LargestReal},
        {"LargestImag", Spectra::SortRule::LargestImag},
        {"LargestAlge", Spectra::SortRule::LargestAlge},
        {"SmallestMagn", Spectra::SortRule::SmallestMagn},
        {"SmallestReal", Spectra::SortRule::SmallestReal},
        {"SmallestImag", Spectra::SortRule::SmallestImag},
        {"SmallestAlge", Spectra::SortRule::SmallestAlge},
        {"BothEnds", Spectra::SortRule::BothEnds}};
    auto it = rules.find(name);
    if (it != rules.cend())
    {
        return it->second;
    }
    else
    {
        std::ostringstream oss;
        oss << "There is no selection rule named: " << name << "\n"
            << "Available selection rules:\n";
        for (const auto& pair : rules)
        {
            oss << pair.first << "\n";
        }
        throw std::runtime_error(oss.str());
    }
}

/// \brief Run the computation and throw and error if it fails
template <typename ResultVector, typename ResultMatrix, typename Solver>
std::pair<ResultVector, ResultMatrix> compute_and_check(
    Solver& eigs, const std::string& selection)
{
    // Initialize and compute
    eigs.init();
    // Compute using the user provided selection rule
    eigs.compute(string_to_sortrule(selection));

    // Retrieve results
    if (eigs.info() == Spectra::CompInfo::Successful)
    {
        return std::make_pair(eigs.eigenvalues(), eigs.eigenvectors());
    }
    else
    {
        throw std::runtime_error(
            "The Spectra SymEigsSolver calculation has failed!");
    }
}

/// \brief Call the Spectra::GenEigsSolver eigensolver
std::pair<ComplexVector, ComplexMatrix> geneigssolver(
    const Matrix& mat, Index nvalues, Index nvectors,
    const std::string& selection)
{
    using DenseOp = Spectra::DenseGenMatProd<double>;

    // Construct matrix operation object using the wrapper class DenseSymMatProd
    Spectra::DenseGenMatProd<double> op(mat);
    Spectra::GenEigsSolver<double, DenseOp> eigs(op, nvalues, nvectors);
    return compute_and_check<ComplexVector, ComplexMatrix>(eigs, selection);
}

/// \brief Call the Spectra::GenEigsRealShiftSolver eigensolver
std::pair<ComplexVector, ComplexMatrix> geneigsrealshiftsolver(
    const Matrix& mat, Index nvalues, Index nvectors, double sigma,
    const std::string& selection)
{
    using DenseOp = Spectra::DenseGenRealShiftSolve<double>;
    DenseOp op(mat);
    Spectra::GenEigsRealShiftSolver<double, DenseOp> eigs(op, nvalues, nvectors,
                                                          sigma);
    return compute_and_check<ComplexVector, ComplexMatrix>(eigs, selection);
}

/// \brief Call the Spectra::GenEigsComplexShiftSolver eigensolver
std::pair<ComplexVector, ComplexMatrix> geneigscomplexshiftsolver(
    const Matrix& mat, Index nvalues, Index nvectors, double sigmar,
    double sigmai, const std::string& selection)
{
    using DenseOp = Spectra::DenseGenComplexShiftSolve<double>;
    DenseOp op(mat);
    Spectra::GenEigsComplexShiftSolver<double, DenseOp> eigs(
        op, nvalues, nvectors, sigmar, sigmai);
    return compute_and_check<ComplexVector, ComplexMatrix>(eigs, selection);
}

/// \brief Call the Spectra::DenseSymMatProd eigensolver
std::pair<Vector, Matrix> symeigssolver(const Matrix& mat, Index nvalues,
                                        Index nvectors,
                                        const std::string& selection)
{
    using DenseSym = Spectra::DenseSymMatProd<double>;
    // Construct matrix operation object using the wrapper class DenseSymMatProd
    DenseSym op(mat);
    Spectra::SymEigsSolver<double, DenseSym> eigs(op, nvalues, nvectors);

    return compute_and_check<Vector, Matrix>(eigs, selection);
}

/// \brief Call the Spectra::SymEigsShiftSolver eigensolver
std::pair<Vector, Matrix> symeigsshiftsolver(const Matrix& mat, Index nvalues,
                                             Index nvectors, double sigma,
                                             const std::string& selection)
{
    using DenseSymShift = Spectra::DenseSymShiftSolve<double>;
    // Construct matrix operation object using the wrapper class DenseSymMatProd
    DenseSymShift op(mat);
    Spectra::SymEigsShiftSolver<double, DenseSymShift> eigs(op, nvalues, nvectors,
                                                            sigma);

    return compute_and_check<Vector, Matrix>(eigs, selection);
}

/// \brief Call the Spectra::SymGEigsShiftSolver eigensolver
std::pair<Vector, Matrix> symgeneigsshiftsolver(const Matrix& mat_A,
                                                const Matrix& mat_B, Index nvalues,
                                                Index nvectors, double sigma,
                                                const std::string& selection)
{
    using SymShiftInvert =
        Spectra::SymShiftInvert<double, Eigen::Dense, Eigen::Dense>;
    using DenseSym = Spectra::DenseSymMatProd<double>;

    // Construct matrix operation object using the wrapper class DenseSymMatProd
    SymShiftInvert op_A(mat_A, mat_B);
    DenseSym op_B(mat_B);
    Spectra::SymGEigsShiftSolver<double, SymShiftInvert, DenseSym, Spectra::GEigsMode::ShiftInvert>
        eigs(op_A, op_B, nvalues, nvectors, sigma);

    return compute_and_check<Vector, Matrix>(eigs, selection);
}

PYBIND11_MODULE(spectra_dense_interface, m)
{
    m.doc() =
        "Interface to the C++ spectra library, see: "
        "https://github.com/yixuan/spectra";

    m.def("general_eigensolver", &geneigssolver,
          py::return_value_policy::reference_internal);

    m.def("general_real_shift_eigensolver", &geneigsrealshiftsolver,
          py::return_value_policy::reference_internal);

    m.def("general_complex_shift_eigensolver", &geneigscomplexshiftsolver,
          py::return_value_policy::reference_internal);

    m.def("symmetric_eigensolver", &symeigssolver,
          py::return_value_policy::reference_internal);

    m.def("symmetric_shift_eigensolver", &symeigsshiftsolver,
          py::return_value_policy::reference_internal);

    m.def("symmetric_generalized_shift_eigensolver", &symgeneigsshiftsolver,
          py::return_value_policy::reference_internal);
}


/** View a conflict graph as an implication graph and run Tarjan's strongly connected components algorithm */

#include "cliquetable_scc.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/strong_components.hpp>

using namespace boost;

void make_domain_consistent(CliqueTableInstance* inst, IntSet* state_intset)
{
	typedef adjacency_list<vecS, vecS, bidirectionalS,
	        property<vertex_color_t, default_color_type, property<vertex_degree_t,int>>> ImplGraph;
	typedef graph_traits<ImplGraph>::vertex_descriptor Vertex;

	ImplGraph graph(2 * inst->nvars);

	// cout << "Clique table:" << endl;
	// inst->print();

	// For every edge (i,j), we have two arcs (i, not j) and (j, not i)
	for (int i = 0; i < inst->nvars; ++i) {
		for (int j = inst->adj[i].get_first(); j != inst->adj[i].get_end(); j = inst->adj[i].get_next(j)) {
			if (i < j && i != inst->get_complement(j)) {
				add_edge(i, inst->get_complement(j), graph);
				add_edge(j, inst->get_complement(i), graph);
			}
		}
	}

	typedef graph_traits<ImplGraph>::edge_iterator edge_iterator;
	pair<edge_iterator, edge_iterator> ei = edges(graph);

	// Debugging info
	// cout << endl << "Implication graph:" << endl;
	// for (edge_iterator it = ei.first; it != ei.second; ++it) {
	// 	cout << "(" << source(*it, graph) << ", " << target(*it, graph) << ")" << " ";
	// }
	// cout << endl;

	// Find strongly connected components
	int nv = num_vertices(graph);
	vector<int> component(nv), discover_time(nv);
	vector<default_color_type> color(nv);
	vector<Vertex> root(nv);
	int num = strong_components(graph, make_iterator_property_map(component.begin(), get(vertex_index, graph)),
	                            root_map(make_iterator_property_map(root.begin(), get(vertex_index, graph))).
	                            color_map(make_iterator_property_map(color.begin(), get(vertex_index, graph))).
	                            discover_time_map(make_iterator_property_map(discover_time.begin(), get(vertex_index, graph))));

	// cout << num << " strong components" << endl;

	// for (int j = num - 1; j >= 0; --j) {
	// 	cout << "[ ";
	// 	for (int i = 0; i < component.size(); ++i) {
	// 		if (j == component[i]) {
	// 			cout << i << " ";
	// 		}
	// 	}
	// 	cout << "] ";
	// }

	// Assert reverse topological ordering
	for (edge_iterator it = ei.first; it != ei.second; ++it) {
		assert(component[source(*it, graph)] >= component[target(*it, graph)]);
	}

	vector<int> topological_order;
	for (int j = num - 1; j >= 0; --j) {
		for (int i = 0; i < (int) component.size(); ++i) {
			if (j == component[i]) {
				topological_order.push_back(i);
			}
		}
	}

	// cout << "Topological order:  ";
	// for (int i : topological_order) {
	// 	cout << i << " ";
	// }
	// cout << endl;

	vector<vector<int>> vertices_in_component(component.size());
	for (int i = 0; i < nv; ++i) {
		vertices_in_component[component[i]].push_back(i);
	}

	// // Debugging info
	// for (int i = 0; i < component.size(); ++i) {
	// 	cout << "Component " << i << ":  ";
	// 	for (int j : vertices_in_component[i]) {
	// 		cout << j << " ";
	// 	}
	// 	cout << endl;
	// }

	// Traverse components in topological order
	vector<set<int>> ancestors(component.size());
	for (int c = nv - 1; c >= 0; --c) {
		for (int v : vertices_in_component[c]) {
			typename graph_traits<ImplGraph>::adjacency_iterator u, u_end;
			for (tie(u, u_end) = adjacent_vertices(v, graph); u != u_end; ++u) {
				// Add current component and its own ancestors to ancestor list of adjacent component
				ancestors[component[*u]].insert(c);
				for (int ancestor : ancestors[c]) {
					ancestors[component[*u]].insert(ancestor);
				}
			}
		}
	}

	// // Debugging info
	// for (int i = 0; i < ancestors.size(); ++i) {
	// 	cout << "Component " << i << " - ancestors:  ";
	// 	for (int j : ancestors[i]) {
	// 		cout << j << " ";
	// 	}
	// 	cout << endl;
	// }

	// If the same component contains both complement nodes, remove them
	for (int v = 0; v < inst->nvars; ++v) {
		if (component[v] == component[inst->get_complement(v)]) {
			state_intset->remove(v);
			state_intset->remove(inst->get_complement(v));
		}
	}

	// For each vertex, check if it precedes its component; if so, remove the vertex from the domain
	for (int v = 0; v < nv; ++v) {
		int complement = component[inst->get_complement(v)];
		if (ancestors[complement].find(component[v]) != ancestors[complement].end()) {
			// There exists a path from v to complement of v
			state_intset->remove(v);
		}
	}
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "all_pairs_distances.h"
#include <Eigen/Dense>

template <typename Mat>
IGL_INLINE void igl::all_pairs_distances(
  const Mat & V,
  const Mat & U,
  const bool squared,
  Mat & D)
{
  // dimension should be the same
  assert(V.cols() == U.cols());
  // resize output
  D.resize(V.rows(),U.rows());
  for(int i = 0;i<V.rows();i++)
  {
    for(int j=0;j<U.rows();j++)
    {
      D(i,j) = (V.row(i)-U.row(j)).squaredNorm();
      if(!squared)
      {
        D(i,j) = sqrt(D(i,j));
      }
    }
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::all_pairs_distances<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, bool, Eigen::Matrix<double, -1, -1, 0, -1, -1>&);
#endif


////////////////////////////////////////////////////////////////////////////////
#include "vertex_degree.h"
#include <igl/edges.h>
#include <Eigen/Dense>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////

namespace cellogram {

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

void vertex_degree(const Eigen::MatrixXi &F, Eigen::VectorXi &degree)
{
	degree = Eigen::VectorXi::Zero(F.maxCoeff()+1);
	/*for (int i = 0; i < F.rows(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			degree(F(i, j))++;
			degree(F(i, (j + 1) % 3))++;
		}
	}*/

	Eigen::MatrixXi E;
	igl::edges(F, E);

	//std::cout << E.transpose() << std::endl;

	for (int i = 0; i < E.rows(); i++)
	{
		for (int j = 0; j < 2; j++)
		{
			degree(E(i, j))++;
			degree(E(i, (j + 1) % 2))++;
		}

	}
	degree /= 2;
}

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

} // namespace cellogram