#include <bits/stdc++.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#define EIGEN_DONT_PARALLELIZE

const double PI	=	3.1415926535897932384;
#include <map>
struct pts2D {
	double x,y;
};

class kernel {
public:
  double a;
  std::vector<pts2D> particles_X;
	std::vector<pts2D> particles_Y;

	kernel(std::vector<pts2D>& particles_X, std::vector<pts2D>& particles_Y) {
			this->particles_X = particles_X;
			this->particles_Y = particles_Y;
	}

	virtual double getMatrixEntry(const unsigned i, const unsigned j) {
		std::cout << "virtual getInteraction" << std::endl;
		return 0.0;
	}

	Eigen::VectorXd getRow(const int j, std::vector<int> col_indices) {
		int n_cols = col_indices.size();
		Eigen::VectorXd row(n_cols);
    #pragma omp parallel for
    for(int k = 0; k < n_cols; k++) {
        row(k) = this->getMatrixEntry(j, col_indices[k]);
    }
    return row;
  }

  Eigen::VectorXd getCol(const int k, std::vector<int> row_indices) {
		int n_rows = row_indices.size();
    Eigen::VectorXd col(n_rows);
    #pragma omp parallel for
    for (int j=0; j<n_rows; ++j) {
			col(j) = this->getMatrixEntry(row_indices[j], k);
    }
    return col;
  }

  Eigen::MatrixXd getMatrix(std::vector<int> row_indices, std::vector<int> col_indices) {
		int n_rows = row_indices.size();
		int n_cols = col_indices.size();
    Eigen::MatrixXd mat(n_rows, n_cols);
    #pragma omp parallel for
    for (int j=0; j < n_rows; ++j) {
        #pragma omp parallel for
        for (int k=0; k < n_cols; ++k) {
            mat(j,k) = this->getMatrixEntry(row_indices[j], col_indices[k]);
        }
    }
    return mat;
  }
  ~kernel() {};
};

class userkernel: public kernel {
public:
	double chargesFunction(const pts2D r) {
		double q = r.x; //user defined
		return q;
	};
	// #ifdef ONEOVERR
	// userkernel(std::vector<pts2D>& particles_X, std::vector<pts2D>& particles_Y): kernel(particles_X, particles_Y) {
	// };
	// double getMatrixEntry(const unsigned i, const unsigned j) {
	// 	pts2D r1 = particles_X[i];
	// 	pts2D r2 = particles_X[j];
	// 	double R2	=	(r1.x-r2.x)*(r1.x-r2.x) + (r1.y-r2.y)*(r1.y-r2.y);
	// 	double R	=	sqrt(R2);
	// 	if (R < a) {
	// 		return R/a;
	// 	}
	// 	else {
	// 		return a/R;
	// 	}
	// }
	// #elif LOGR
	userkernel(std::vector<pts2D> particles_X, std::vector<pts2D> particles_Y): kernel(particles_X, particles_Y) {
	};
	double getMatrixEntry(const unsigned i, const unsigned j) {
		pts2D r1 = particles_X[i];
		pts2D r2 = particles_X[j];
		double R2	=	(r1.x-r2.x)*(r1.x-r2.x) + (r1.y-r2.y)*(r1.y-r2.y);
		if (R2 < 1e-10) {
			return 0.0;
		}
		else if (R2 < a*a) {
			return 0.5*R2*log(R2)/a/a;
		}
		else {
			return 0.5*log(R2);
		}
	}
	// #endif
	~userkernel() {};
};


#include <boost/program_options.hpp>

#include <iostream>
#include <string>

#include "spf/graph.h"
#include "spf/web_socket_rpc.h"

namespace po = boost::program_options;

using namespace spf;

void run(std::string const& host, unsigned short port) {
  WebSocketRpc service{host, port};
  Graph g{service};
  g.addVertex(); g.addVertex(); g.addVertex(); g.addVertex();
  g.addVertex(); g.addVertex();
  g.setEdge(0, 1, 7); g.setEdge(0, 2, 9); g.setEdge(0, 5, 14);
  g.setEdge(1, 0, 7); g.setEdge(1, 2, 10); g.setEdge(1, 3, 15);
  g.setEdge(2, 0, 9); g.setEdge(2, 1, 10); g.setEdge(2, 5, 2);
  g.setEdge(3, 1, 15); g.setEdge(3, 2, 11); g.setEdge(3, 4, 6);
  g.setEdge(4, 3, 6); g.setEdge(4, 5, 9);
  g.setEdge(5, 0, 14); g.setEdge(5, 2, 2); g.setEdge(5, 4, 9);

  auto path = g.path(0, 4);

  std::cout << "Path: ";
  for (Id id: path) std::cout << id << ' ';
  std::cout << '\n';
}

void async_run(std::string const& host, unsigned short port) {
  WebSocketRpc service{host, port};
  Graph g{service};

  auto add_error = [](Id, auto error) {
    if (error) std::cout << error.what() << '\n';
  };
  g.addVertex(add_error); g.addVertex(add_error); g.addVertex(add_error);
  g.addVertex(add_error); g.addVertex(add_error); g.addVertex(add_error);

  auto set_error = [](auto error) {
    if (error) std::cout << error.what() << '\n';
  };
  g.setEdge(0, 1, 7, set_error);
  g.setEdge(0, 2, 9, set_error);
  g.setEdge(0, 5, 14, set_error);
  g.setEdge(1, 0, 7, set_error);
  g.setEdge(1, 2, 10, set_error);
  g.setEdge(1, 3, 15, set_error);
  g.setEdge(2, 0, 9, set_error);
  g.setEdge(2, 1, 10, set_error);
  g.setEdge(2, 5, 2, set_error);
  g.setEdge(3, 1, 15, set_error);
  g.setEdge(3, 2, 11, set_error);
  g.setEdge(3, 4, 6, set_error);
  g.setEdge(4, 3, 6, set_error);
  g.setEdge(4, 5, 9, set_error);
  g.setEdge(5, 0, 14, set_error);
  g.setEdge(5, 2, 2, set_error);
  g.setEdge(5, 4, 9, set_error);

  g.path(0, 4, [](auto path, auto error) {
    if (!error) {
      std::cout << "Path: ";
      for (Id id: path) std::cout << id << ' ';
      std::cout << '\n';
    } else {
      std::cout << error.what() << '\n';
    }
  });
}

int main(int argc, char* argv[]) {
  bool async{false};
  std::string host{"localhost"};
  unsigned short port{8080};

  po::options_description args("Using");
  args.add_options()
    ("help", "produce help message")
    ("async", po::value<bool>(&async)->default_value(false), "asynchronous client")
    ("host", po::value<std::string>(&host)->default_value("localhost"), "host to connect")
    ("port", po::value<unsigned short>(&port)->default_value(8080), "port to connect");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, args), vm);
  po::notify(vm);

  if (vm.count("help")) {
      std::cout << args << "\n";
      return 1;
  }

  if (async) {
    async_run(host, port);
  } else {
    run(host, port);
  }

  return 0;
}


// BLAS level 2
// benchmarks

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

//#define USE_STD_VECTOR
//#define BOUNDED 100*100

//#define PRINT
//#define PRINT_M

//#define MODIFY

#include <stddef.h>
#include <iostream>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#include <boost/numeric/bindings/atlas/cblas2.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>
#ifdef USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include <boost/timer.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;

using std::cout;
using std::cin;
using std::endl;

typedef double real_t;

#ifdef USE_STD_VECTOR
typedef std::vector<real_t> storage_t;
#else
#ifndef BOUNDED
typedef ublas::unbounded_array<real_t> storage_t;
#else
typedef ublas::bounded_array<real_t, BOUNDED> storage_t;
#endif
#endif

typedef ublas::vector<real_t, storage_t> vct_t;

typedef ublas::matrix<real_t, ublas::column_major, storage_t> cm_t;
typedef ublas::matrix<real_t, ublas::row_major, storage_t> rm_t;

typedef ublas::symmetric_adaptor<cm_t, ublas::upper> ucsa_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::lower> lcsa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::upper> ursa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::lower> lrsa_t;

typedef ublas::symmetric_matrix<
  real_t, ublas::upper, ublas::column_major
> ucsymm_t;
typedef ublas::symmetric_matrix<
  real_t, ublas::lower, ublas::column_major
> lcsymm_t;
typedef ublas::symmetric_matrix<
  real_t, ublas::upper, ublas::row_major
> ursymm_t;
typedef ublas::symmetric_matrix<
  real_t, ublas::lower, ublas::row_major
> lrsymm_t;

#ifdef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
using ublas::prod;
#endif

////////////////////////////////////////////////////
// general matrix: gemv()

template <typename M>
void bench_gemv (size_t n, size_t runs, char* msg) {

  cout << msg << endl;

  vct_t x (n);
  atlas::set (1., x);
  vct_t y (n);

  M a (n, n);
  init_symm (a);
#ifdef PRINT_M
  print_m (a, "a");
  cout << endl;
#endif

  boost::timer (t);
  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    a (0, 2) = r;
    a (2, 0) = r;
#endif
    atlas::gemv (a, x, y);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  gemv:        " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    a (0, 2) = r;
    a (2, 0) = r;
#endif
    atlas::gemv (CblasTrans, 1., a, x, 0., y);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  gemv trans:  " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    a (0, 2) = r;
    a (2, 0) = r;
#endif
    y = prod (a, x);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  = prod:      " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    a (0, 2) = r;
    a (2, 0) = r;
#endif
    y.assign (prod (a, x));
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  assign prod: " << t.elapsed() << endl;
  cout << endl;
}

/////////////////////////////////////////////////////////
// symmetric adaptor: symv()

template <typename M, typename SA>
void bench_symv (size_t n, size_t runs, char* msg) {

  cout << msg << endl;

  vct_t x (n);
  atlas::set (1., x);
  vct_t y (n);

  M a (n, n);
  SA sa (a);
  init_symm (sa, 'l');
#ifdef PRINT_M
  print_m (sa, "sa");
  cout << endl;
  print_m (a, "a");
  cout << endl;
#endif

  boost::timer (t);
  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    sa (0, 2) = r;
#endif
    atlas::symv (sa, x, y);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  symv:        " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    sa (0, 2) = r;
#endif
    y = prod (sa, x);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  = prod:      " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    sa (0, 2) = r;
#endif
    y.assign (prod (sa, x));
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  assign prod: " << t.elapsed() << endl;
  cout << endl;
}

/////////////////////////////////////////////////////////
// symmetric matrix: spmv()

template <typename SM>
void bench_spmv (size_t n, size_t runs, char* msg) {

  cout << msg << endl;

  vct_t x (n);
  atlas::set (1., x);
  vct_t y (n);

  SM sa (n, n);
  init_symm (sa, 'l');
#ifdef PRINT_M
  cout << sa << endl;
  cout << endl;
#endif

  boost::timer (t);
  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    sa (0, 2) = r;
#endif
    atlas::spmv (sa, x, y);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  spmv:        " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    sa (0, 2) = r;
#endif
    y = prod (sa, x);
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  = prod:      " << t.elapsed() << endl;

  t.restart();
  for (size_t r = 0; r < runs; ++r) {
#ifdef MODIFY
    sa (0, 2) = r;
#endif
    y.assign (prod (sa, x));
#ifdef PRINT
    print_v (y, "y");
#endif
  }
  cout << "  assign prod: " << t.elapsed() << endl;
  cout << endl;
}

//////////////////////////////////////////////////////
int main() {

  cout << endl;

  size_t n, r;
  cout << "n -> ";
  cin >> n;
  cout << "r -> ";
  cin >> r;
  cout << endl;

  bench_gemv<rm_t> (n, r, "row major");
  bench_gemv<cm_t> (n, r, "column major");

  bench_symv<rm_t, ursa_t> (n, r, "symmetric ad, row, upper");
  bench_symv<rm_t, lrsa_t> (n, r, "symmetric ad, row, lower");
  bench_symv<cm_t, ucsa_t> (n, r, "symmetric ad, column, upper");
  bench_symv<cm_t, lcsa_t> (n, r, "symmetric ad, column, lower");

  bench_spmv<ursymm_t> (n, r, "symmetric, row, upper");
  bench_spmv<lrsymm_t> (n, r, "symmetric, row, lower");
  bench_spmv<ucsymm_t> (n, r, "symmetric, column, upper");
  bench_spmv<lcsymm_t> (n, r, "symmetric, column, lower");

}


#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <quadrotor_msgs/SO3Command.h>
#include <quadrotor_simulator/Quadrotor.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <uav_utils/geometry_utils.h>

typedef struct _Control
{
  double rpm[4];
} Control;

typedef struct _Command
{
  float force[3];
  float qx, qy, qz, qw;
  float kR[3];
  float kOm[3];
  float corrections[3];
  float current_yaw;
  bool  use_external_yaw;
} Command;

typedef struct _Disturbance
{
  Eigen::Vector3d f;
  Eigen::Vector3d m;
} Disturbance;

static Command     command;
static Disturbance disturbance;

void stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State& state,
                    nav_msgs::Odometry&                         odom);
void quadToImuMsg(const QuadrotorSimulator::Quadrotor& quad,
                  sensor_msgs::Imu&                    imu);

static Control
getControl(const QuadrotorSimulator::Quadrotor& quad, const Command& cmd)
{
  const double _kf = quad.getPropellerThrustCoefficient();
  const double _km = quad.getPropellerMomentCoefficient();
  const double kf  = _kf - cmd.corrections[0];
  const double km  = _km / _kf * kf;

  const double          d       = quad.getArmLength();
  const Eigen::Matrix3f J       = quad.getInertia().cast<float>();
  const float           I[3][3] = { { J(0, 0), J(0, 1), J(0, 2) },
                          { J(1, 0), J(1, 1), J(1, 2) },
                          { J(2, 0), J(2, 1), J(2, 2) } };
  const QuadrotorSimulator::Quadrotor::State state = quad.getState();

  // Rotation, may use external yaw
  Eigen::Vector3d _ypr = uav_utils::R_to_ypr(state.R);
  Eigen::Vector3d ypr  = _ypr;
  if (cmd.use_external_yaw)
    ypr[0] = cmd.current_yaw;
  Eigen::Matrix3d R;
  R = Eigen::AngleAxisd(ypr[0], Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(ypr[1], Eigen::Vector3d::UnitY()) *
      Eigen::AngleAxisd(ypr[2], Eigen::Vector3d::UnitX());
  float R11 = R(0, 0);
  float R12 = R(0, 1);
  float R13 = R(0, 2);
  float R21 = R(1, 0);
  float R22 = R(1, 1);
  float R23 = R(1, 2);
  float R31 = R(2, 0);
  float R32 = R(2, 1);
  float R33 = R(2, 2);
  /*
    float R11 = state.R(0,0);
    float R12 = state.R(0,1);
    float R13 = state.R(0,2);
    float R21 = state.R(1,0);
    float R22 = state.R(1,1);
    float R23 = state.R(1,2);
    float R31 = state.R(2,0);
    float R32 = state.R(2,1);
    float R33 = state.R(2,2);
  */
  float Om1 = state.omega(0);
  float Om2 = state.omega(1);
  float Om3 = state.omega(2);

  float Rd11 =
    cmd.qw * cmd.qw + cmd.qx * cmd.qx - cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd12 = 2 * (cmd.qx * cmd.qy - cmd.qw * cmd.qz);
  float Rd13 = 2 * (cmd.qx * cmd.qz + cmd.qw * cmd.qy);
  float Rd21 = 2 * (cmd.qx * cmd.qy + cmd.qw * cmd.qz);
  float Rd22 =
    cmd.qw * cmd.qw - cmd.qx * cmd.qx + cmd.qy * cmd.qy - cmd.qz * cmd.qz;
  float Rd23 = 2 * (cmd.qy * cmd.qz - cmd.qw * cmd.qx);
  float Rd31 = 2 * (cmd.qx * cmd.qz - cmd.qw * cmd.qy);
  float Rd32 = 2 * (cmd.qy * cmd.qz + cmd.qw * cmd.qx);
  float Rd33 =
    cmd.qw * cmd.qw - cmd.qx * cmd.qx - cmd.qy * cmd.qy + cmd.qz * cmd.qz;

  float Psi = 0.5f * (3.0f - (Rd11 * R11 + Rd21 * R21 + Rd31 * R31 +
                              Rd12 * R12 + Rd22 * R22 + Rd32 * R32 +
                              Rd13 * R13 + Rd23 * R23 + Rd33 * R33));

  float force = 0;
  if (Psi < 1.0f) // Position control stability guaranteed only when Psi < 1
    force = cmd.force[0] * R13 + cmd.force[1] * R23 + cmd.force[2] * R33;

  float eR1 = 0.5f * (R12 * Rd13 - R13 * Rd12 + R22 * Rd23 - R23 * Rd22 +
                      R32 * Rd33 - R33 * Rd32);
  float eR2 = 0.5f * (R13 * Rd11 - R11 * Rd13 - R21 * Rd23 + R23 * Rd21 -
                      R31 * Rd33 + R33 * Rd31);
  float eR3 = 0.5f * (R11 * Rd12 - R12 * Rd11 + R21 * Rd22 - R22 * Rd21 +
                      R31 * Rd32 - R32 * Rd31);

  float eOm1 = Om1;
  float eOm2 = Om2;
  float eOm3 = Om3;

  float in1 = Om2 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3) -
              Om3 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3);
  float in2 = Om3 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3) -
              Om1 * (I[2][0] * Om1 + I[2][1] * Om2 + I[2][2] * Om3);
  float in3 = Om1 * (I[1][0] * Om1 + I[1][1] * Om2 + I[1][2] * Om3) -
              Om2 * (I[0][0] * Om1 + I[0][1] * Om2 + I[0][2] * Om3);
  /*
    // Robust Control --------------------------------------------
    float c2       = 0.6;
    float epsilonR = 0.04;
    float deltaR   = 0.1;
    float eA1 = eOm1 + c2 * 1.0/I[0][0] * eR1;
    float eA2 = eOm2 + c2 * 1.0/I[1][1] * eR2;
    float eA3 = eOm3 + c2 * 1.0/I[2][2] * eR3;
    float neA = sqrt(eA1*eA1 + eA2*eA2 + eA3*eA3);
    float muR1 = -deltaR*deltaR * eA1 / (deltaR * neA + epsilonR);
    float muR2 = -deltaR*deltaR * eA2 / (deltaR * neA + epsilonR);
    float muR3 = -deltaR*deltaR * eA3 / (deltaR * neA + epsilonR);
    // Robust Control --------------------------------------------
  */
  float M1 = -cmd.kR[0] * eR1 - cmd.kOm[0] * eOm1 + in1; // - I[0][0]*muR1;
  float M2 = -cmd.kR[1] * eR2 - cmd.kOm[1] * eOm2 + in2; // - I[1][1]*muR2;
  float M3 = -cmd.kR[2] * eR3 - cmd.kOm[2] * eOm3 + in3; // - I[2][2]*muR3;

  float w_sq[4];
  w_sq[0] = force / (4 * kf) - M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[1] = force / (4 * kf) + M2 / (2 * d * kf) + M3 / (4 * km);
  w_sq[2] = force / (4 * kf) + M1 / (2 * d * kf) - M3 / (4 * km);
  w_sq[3] = force / (4 * kf) - M1 / (2 * d * kf) - M3 / (4 * km);

  Control control;
  for (int i = 0; i < 4; i++)
  {
    if (w_sq[i] < 0)
      w_sq[i] = 0;

    control.rpm[i] = sqrtf(w_sq[i]);
  }
  return control;
}

static void
cmd_callback(const quadrotor_msgs::SO3Command::ConstPtr& cmd)
{
  command.force[0]         = cmd->force.x;
  command.force[1]         = cmd->force.y;
  command.force[2]         = cmd->force.z;
  command.qx               = cmd->orientation.x;
  command.qy               = cmd->orientation.y;
  command.qz               = cmd->orientation.z;
  command.qw               = cmd->orientation.w;
  command.kR[0]            = cmd->kR[0];
  command.kR[1]            = cmd->kR[1];
  command.kR[2]            = cmd->kR[2];
  command.kOm[0]           = cmd->kOm[0];
  command.kOm[1]           = cmd->kOm[1];
  command.kOm[2]           = cmd->kOm[2];
  command.corrections[0]   = cmd->aux.kf_correction;
  command.corrections[1]   = cmd->aux.angle_corrections[0];
  command.corrections[2]   = cmd->aux.angle_corrections[1];
  command.current_yaw      = cmd->aux.current_yaw;
  command.use_external_yaw = cmd->aux.use_external_yaw;
}

static void
force_disturbance_callback(const geometry_msgs::Vector3::ConstPtr& f)
{
  disturbance.f(0) = f->x;
  disturbance.f(1) = f->y;
  disturbance.f(2) = f->z;
}

static void
moment_disturbance_callback(const geometry_msgs::Vector3::ConstPtr& m)
{
  disturbance.m(0) = m->x;
  disturbance.m(1) = m->y;
  disturbance.m(2) = m->z;
}

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "quadrotor_simulator_so3");

  ros::NodeHandle n("~");

  ros::Publisher  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 100);
  ros::Publisher  imu_pub  = n.advertise<sensor_msgs::Imu>("imu", 10);
  ros::Subscriber cmd_sub =
    n.subscribe("cmd", 100, &cmd_callback, ros::TransportHints().tcpNoDelay());
  ros::Subscriber f_sub =
    n.subscribe("force_disturbance", 100, &force_disturbance_callback,
                ros::TransportHints().tcpNoDelay());
  ros::Subscriber m_sub =
    n.subscribe("moment_disturbance", 100, &moment_disturbance_callback,
                ros::TransportHints().tcpNoDelay());

  QuadrotorSimulator::Quadrotor quad;
  double                        _init_x, _init_y, _init_z;
  n.param("simulator/init_state_x", _init_x, 0.0);
  n.param("simulator/init_state_y", _init_y, 0.0);
  n.param("simulator/init_state_z", _init_z, 1.0);

  Eigen::Vector3d position = Eigen::Vector3d(_init_x, _init_y, _init_z);
  quad.setStatePos(position);

  double simulation_rate;
  n.param("rate/simulation", simulation_rate, 1000.0);
  ROS_ASSERT(simulation_rate > 0);

  double odom_rate;
  n.param("rate/odom", odom_rate, 100.0);
  const ros::Duration odom_pub_duration(1 / odom_rate);

  std::string quad_name;
  n.param("quadrotor_name", quad_name, std::string("quadrotor"));

  QuadrotorSimulator::Quadrotor::State state = quad.getState();

  ros::Rate    r(simulation_rate);
  const double dt = 1 / simulation_rate;

  Control control;

  nav_msgs::Odometry odom_msg;
  odom_msg.header.frame_id = "/world";
  odom_msg.child_frame_id  = "/" + quad_name;

  sensor_msgs::Imu imu;
  imu.header.frame_id = "/simulator";

  /*
  command.force[0] = 0;
  command.force[1] = 0;
  command.force[2] = quad.getMass()*quad.getGravity() + 0.1;
  command.qx = 0;
  command.qy = 0;
  command.qz = 0;
  command.qw = 1;
  command.kR[0] = 2;
  command.kR[1] = 2;
  command.kR[2] = 2;
  command.kOm[0] = 0.15;
  command.kOm[1] = 0.15;
  command.kOm[2] = 0.15;
  */

  ros::Time next_odom_pub_time = ros::Time::now();
  while (n.ok())
  {
    ros::spinOnce();

    auto last = control;
    control   = getControl(quad, command);
    for (int i = 0; i < 4; ++i)
    {
      //! @bug might have nan when the input is legal
      if (std::isnan(control.rpm[i]))
        control.rpm[i] = last.rpm[i];
    }
    quad.setInput(control.rpm[0], control.rpm[1], control.rpm[2],
                  control.rpm[3]);
    quad.setExternalForce(disturbance.f);
    quad.setExternalMoment(disturbance.m);
    quad.step(dt);

    ros::Time tnow = ros::Time::now();

    if (tnow >= next_odom_pub_time)
    {
      next_odom_pub_time += odom_pub_duration;
      odom_msg.header.stamp = tnow;
      state                 = quad.getState();
      stateToOdomMsg(state, odom_msg);
      quadToImuMsg(quad, imu);
      odom_pub.publish(odom_msg);
      imu_pub.publish(imu);
    }

    r.sleep();
  }

  return 0;
}

void
stateToOdomMsg(const QuadrotorSimulator::Quadrotor::State& state,
               nav_msgs::Odometry&                         odom)
{
  odom.pose.pose.position.x = state.x(0);
  odom.pose.pose.position.y = state.x(1);
  odom.pose.pose.position.z = state.x(2);

  Eigen::Quaterniond q(state.R);
  odom.pose.pose.orientation.x = q.x();
  odom.pose.pose.orientation.y = q.y();
  odom.pose.pose.orientation.z = q.z();
  odom.pose.pose.orientation.w = q.w();

  odom.twist.twist.linear.x = state.v(0);
  odom.twist.twist.linear.y = state.v(1);
  odom.twist.twist.linear.z = state.v(2);

  odom.twist.twist.angular.x = state.omega(0);
  odom.twist.twist.angular.y = state.omega(1);
  odom.twist.twist.angular.z = state.omega(2);
}

void
quadToImuMsg(const QuadrotorSimulator::Quadrotor& quad, sensor_msgs::Imu& imu)

{
  QuadrotorSimulator::Quadrotor::State state = quad.getState();
  Eigen::Quaterniond                   q(state.R);
  imu.orientation.x = q.x();
  imu.orientation.y = q.y();
  imu.orientation.z = q.z();
  imu.orientation.w = q.w();

  imu.angular_velocity.x = state.omega(0);
  imu.angular_velocity.y = state.omega(1);
  imu.angular_velocity.z = state.omega(2);

  imu.linear_acceleration.x = quad.getAcc()[0];
  imu.linear_acceleration.y = quad.getAcc()[1];
  imu.linear_acceleration.z = quad.getAcc()[2];
}


#include "headers/Sphere.hpp"
#include "headers/Object.hpp"
#include "headers/Hittable.hpp"
#include <armadillo>
#include <vector>

using namespace arma;
using namespace std;

Sphere::Sphere() {
}

Sphere::Sphere(vec esfera){

    mat transf;
    vec kd, ks;

    transf << esfera(11) << esfera(12) << esfera(13) << esfera(14) << endr
	   << esfera(15) << esfera(16) << esfera(17) << esfera(18) << endr
           << esfera(19) << esfera(20) << esfera(21) << esfera(22) << endr
           << 0 << 0 << 0 << 1;

    setTransf(transf);

    this->centro << esfera(0) << esfera(1) << esfera(2) << 1;
    this->centro = transf * centro;
    this->centro.shed_row(3);

    this->raio = esfera(3);

    kd << esfera(4) << esfera(5) << esfera(6);
    ks << esfera(7) << esfera(8) << esfera(9);

    setKd(kd);
    setKs(ks);

    setP(esfera(10));

    this->min = this->centro - this->raio;
    this->max = this->centro + this->raio;

    setTipo(0);
}

bool Sphere::colide(const vec &d, double &T, const vec &origem){
    double a, b, c, delta, x, xx;

    a = dot(origem-d,origem-d);
    b = 2*dot(origem-d, this->centro - origem);
    c = dot(this->centro - origem, this->centro - origem) - (this->raio * this->raio);

    delta = (b*b) - (4 * a * c);

    if (delta>=0){
	x  = (-b + sqrt(delta)) / (2 * a);
	xx = (-b - sqrt(delta)) / (2 * a);

	if (x < xx) {
            T=x;
        }

        else {
            T=xx;
        }

        return true;
    }

    else {
        return false;
    }
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_id_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>

#include <CGAL/boost/graph/dijkstra_shortest_paths.h>
#include <boost/graph/filtered_graph.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;

typedef CGAL::Triangulation_vertex_base_with_id_2<K> Tvb;
typedef CGAL::Triangulation_face_base_2<K> Tfb;
typedef CGAL::Triangulation_data_structure_2<Tvb,Tfb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Triangulation;

// consider finite vertices and edges.

template <typename T>
struct Is_finite {

  const T* t_;

  Is_finite()
    : t_(NULL)
  {}

  Is_finite(const T& t)
    : t_(&t)
  { }

  template <typename VertexOrEdge>
  bool operator()(const VertexOrEdge& voe) const {
    return ! t_->is_infinite(voe);
  }
};

typedef Is_finite<Triangulation> Filter;
typedef boost::filtered_graph<Triangulation,Filter,Filter> Finite_triangulation;
typedef boost::graph_traits<Finite_triangulation>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Finite_triangulation>::vertex_iterator vertex_iterator;

int
main(int,char*[])
{
  Triangulation t;
  Filter is_finite(t);
  Finite_triangulation ft(t, is_finite, is_finite);

  t.insert(Point(0,0));
  t.insert(Point(1,0));
  t.insert(Point(0.2,0.2));
  t.insert(Point(0,1));
  t.insert(Point(0,2));

  vertex_iterator vit, ve;
  // associate indices to the vertices
  int index = 0;
  for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){
    vertex_descriptor  vd = *vit;
    vd->id()= index++;
  }

  typedef boost::property_map<Triangulation, boost::vertex_index_t>::type VertexIdPropertyMap;
  VertexIdPropertyMap vertex_index_pmap = get(boost::vertex_index, ft);

  // Dijkstra's shortest path needs property maps for the predecessor and distance
  std::vector<vertex_descriptor> predecessor(boost::num_vertices(ft));
  boost::iterator_property_map<std::vector<vertex_descriptor>::iterator, VertexIdPropertyMap>
    predecessor_pmap(predecessor.begin(), vertex_index_pmap);

  std::vector<double> distance(boost::num_vertices(ft));
  boost::iterator_property_map<std::vector<double>::iterator, VertexIdPropertyMap>
    distance_pmap(distance.begin(), vertex_index_pmap);

  vertex_descriptor source =  *boost::vertices(ft).first;
  std::cout << "\nStart dijkstra_shortest_paths at " << source->point() << std::endl;

  boost::dijkstra_shortest_paths(ft, source ,
				 distance_map(distance_pmap)
				 .predecessor_map(predecessor_pmap));

  for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){
    vertex_descriptor vd = *vit;
    std::cout << vd->point() << " [" << vd->id() << "] ";
    std::cout << " has distance = "  << get(distance_pmap,vd) << " and predecessor ";
    vd = get(predecessor_pmap,vd);
    std::cout << vd->point() << " [" << vd->id() << "]\n";
  }
  return 0;
}


/*
fragmentLengthModel.cpp - This file is part of the Bayesembler (v1.1.1)

The MIT License (MIT)

Copyright (c) 2014 Lasse Maretty and Jonas Andreas Sibbesen

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.
*/

#include "fragmentLengthModel.h"
#include <math.h>
#include <fstream>
#include <sstream>

#include "boost/math/constants/constants.hpp"
#include <boost/filesystem.hpp>

GaussianFragmentLengthModel::GaussianFragmentLengthModel() {}

GaussianFragmentLengthModel::GaussianFragmentLengthModel(double mean_in, double sd_in) {

	mean = mean_in;
	sd = sd_in;
	var = pow(sd, 2);

	norm_const = 1/(sd*sqrt(2*boost::math::constants::pi<double>()));

	cout << "[" << getLocalTime() << "] Using Gaussian fragment length distribution with parameters: Mean=" << mean << " and SD=" << sd << endl;

}

GaussianFragmentLengthModel::GaussianFragmentLengthModel(map<uint,uint> fragment_lengths) {

	pair<double,double> parameters = estimateParameters(fragment_lengths);

	mean = parameters.first;
	sd = 1.4826 * parameters.second;

	var = pow(sd, 2);
	norm_const = 1/(sd*sqrt(2*boost::math::constants::pi<double>()));

	cout << "[" << getLocalTime() << "] Using Gaussian fragment length distribution with parameters: Mean=" << mean << " and SD=" << sd << endl;
}

double GaussianFragmentLengthModel::pmf(uint fragment_length) {

	return norm_const * exp(-(pow((double) fragment_length - mean, 2)/(2*var)));
}

template <typename DataType>
double GaussianFragmentLengthModel::estimateMedian(map<DataType,uint> observations) {

	int observation_count = 0;

	for (typename map<DataType,uint>::iterator observation_iter = observations.begin(); observation_iter != observations.end(); observation_iter++) {

		observation_count += observation_iter->second;
	}

	assert(observation_count > 0);

	// Find median observation index for uneven observation number and first median observation for even observation number
	int median_observation_idx;

	if (observation_count % 2 == 0) {

		median_observation_idx = (int) round((double) observation_count/2.0);

	} else {

		median_observation_idx = (int) ceil((double) observation_count/2.0);
	}

	// Iterate to median observation index
	typename map<DataType,uint>::iterator observation_iter = observations.begin();
	int observation_count_running = observation_iter->second;

	while (observation_count_running < median_observation_idx) {

		observation_iter++;
		observation_count_running += observation_iter->second;
	}

	if (observation_count % 2 == 0 and observation_count_running == median_observation_idx) {

		DataType median = observation_iter->first;
		observation_iter++;
		assert(observation_iter != observations.end());
		median += observation_iter->first;
		return (double) median / 2;

	} else {

		assert(observation_count_running >= median_observation_idx);

		return (double) observation_iter->first;
	}
}

pair<double,double> GaussianFragmentLengthModel::estimateParameters(map<uint,uint> fragment_lengths) {

	double median = estimateMedian(fragment_lengths);

	map<double,uint> abs_distances;
	int observation_count = 0;

	for (map<uint,uint>::iterator fragment_lengths_iter = fragment_lengths.begin(); fragment_lengths_iter != fragment_lengths.end(); fragment_lengths_iter++) {

		assert(fragment_lengths_iter->first > 0);
		observation_count += fragment_lengths_iter->second;

		double distance = abs((double) fragment_lengths_iter->first - median);

		if (abs_distances.count(distance) == 0) {

			abs_distances[distance] = fragment_lengths_iter->second;

		} else {

			abs_distances[distance] += fragment_lengths_iter->second;

		}
	}

	double median_abs_deviation = estimateMedian(abs_distances);

	if (observation_count < 10000) {

		cout << "\n[" << getLocalTime() << "] WARNING: Only " << observation_count << " observation(s) were used for estimation of the fragment length distribution!\n" << endl;
	}

	cout << "[" << getLocalTime() << "] Estimated fragment length \"median\"=" << median << " and \"median absolute deviation\"=" << median_abs_deviation << " using " << observation_count << " observations" << endl;

	return pair<double,double> (median, median_abs_deviation);
}

EmpiricalFragmentLengthModel::EmpiricalFragmentLengthModel(string distribution_filename) {

	assert(boost::filesystem::exists(distribution_filename));

	ifstream distribution_file(distribution_filename.c_str());
    assert(distribution_file.is_open());

    string distribution_line;

    uint fragment_length;
    double probability;
    uint line_count = 0;

    while (getline(distribution_file, distribution_line)) {

    	line_count++;
        stringstream linestream(distribution_line);

	    // Read the integers using the operator >>
	    linestream >> fragment_length >> probability;

	    assert(distribution.count(fragment_length) == 0);
	    distribution[fragment_length] = probability;
    }

    cout << "[" << getLocalTime() << "] Loaded empirical fragment length distribution containing " << line_count << " fragment length values - the remaining will have point-zero probability " << endl;
}

double EmpiricalFragmentLengthModel::pmf(uint fragment_length) {

	if (distribution.count(fragment_length)) {

		return distribution[fragment_length];

	} else {

		return 0;
	}
}


#include "CrtbpSystem.hpp"
#include <assert.h>
#include <boost/numeric/odeint.hpp>
#include <iostream>

struct push_back_state_and_time
{
	std::vector<state_type> &m_states;
	std::vector<double> &m_times;

	push_back_state_and_time(std::vector<state_type> &states,
													 std::vector<double> &times)
			: m_states(states), m_times(times)
	{
	}

	void operator()(const state_type &x, double t)
	{
		m_states.push_back(x);
		m_times.push_back(t);
	}
};

int main()
{
	using namespace boost::numeric::odeint;

	CrtbpSystem sys(0.012);
	size_t steps;
	state_type x(6);
	x[0] = 0.8;
	x[1] = 0.0;
	x[2] = 0.0;
	x[3] = 0.0;
	x[4] = 0.7;
	x[5] = 0.0;

	std::vector<state_type> x_vec;
	std::vector<double> times;

	steps =
			integrate(sys, x, 0.0, 20.0, 0.1, push_back_state_and_time(x_vec, times));

	/* output */
	for (size_t i = 0; i <= steps; i++) {
		std::cout << x_vec[i][0] << ' ' << x_vec[i][1] << ' ' << x_vec[i][2]
							<< '\n';
	}
  return 0;
}


#include "../src/streamingcc_include/util.h"
#define BOOST_TEST_MODULE ClassTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <random>
#include <map>
#include <iostream>
#include <vector>

using namespace streamingcc::util;

BOOST_AUTO_TEST_CASE(util_Test) {
  std::vector<int> vec = {6, 2, 7, 4, 5, 1, 3};
  int median = CalcMedian(vec);
  BOOST_CHECK(median == 4);
}


/////////////////////////////////////////////////////////
// (c) 2021 Tetsuro Nagai
/////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <random>
#include <cstdio>
#include <cmath>
#include <ctime>
#include <iomanip>
#include <cstdlib>
#include<vector>
#include<algorithm>
#include<numeric>
#include<sstream>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <prettyprint.hpp>

#define DEBUG (false)
namespace po=boost::program_options;

int L;
int N;

std::vector<int> sites;
std::vector<int> parent;

// The functions init, find, unite, connect, pos2index
// were taken or adopted from https://github.com/kaityo256/mc/tree/master/percolation/
// (c) 2012-2019 H. Watanabe

void init(int size){
    L=size;
    N = L * L * L;
    parent.resize(N);
    sites.resize(N);
    for(int i = 0; i< N; ++i ){
        parent[i] = i ;
    }
}

int find(int i){
    if (i != parent[i]){
        parent[i] = find(parent[i]); // tree is flattened; https://algo-logic.info/union-find-tree/
    }
    return parent[i] ;
}

void unite(int i, int j){  //can be optimized more if necessary
    i = find(i);
    j = find(j);
    parent[j] = i ;
}

void connect(int i, int j){
    if(sites[i] == 0){
        return ;
    }
    if(sites[j] == 0){
        return ;
    }
    unite(i,j);
}

int pos2index(int ix, int iy, int iz){
    ix =(ix + L) % L;
    iy =(iy + L) % L;
    iz =(iz + L) % L;
    return ix *L*L + iy *L + iz ;
}

double crossing_probability_z(void){
    for(int ix1 = 0; ix1 < L ; ix1++){
    for(int iy1 = 0; iy1 < L ; iy1++){
        int i = pos2index(ix1, iy1, 0);
        int ci = find(i);
        for(int ix2 = 0 ; ix2 < L; ++ix2){
        for(int iy2 = 0 ; iy2 < L; ++iy2){
            int j = (pos2index(ix2, iy2, L-1));
            int cj = find(j);
            if(ci == cj){
                return 1.0;
            }
        }
        }
    }
    }
    return 0.0;
}

double crossing_probability_y(void){
    for(int ix1 = 0; ix1 < L ; ix1++){
    for(int iy1 = 0; iy1 < L ; iy1++){
        int i = pos2index(ix1, 0, iy1);
        int ci = find(i);
        for(int ix2 = 0 ; ix2 < L; ++ix2){
        for(int iy2 = 0 ; iy2 < L; ++iy2){
            int j = (pos2index(ix2, L-1, iy2));
            int cj = find(j);
            if(ci == cj){
                return 1.0;
            }
        }
        }
    }
    }
    return 0.0;
}

double crossing_probability_x(void){
    for(int ix1 = 0; ix1 < L ; ix1++){
    for(int iy1 = 0; iy1 < L ; iy1++){
        int i = pos2index(0, ix1,  iy1);
        int ci = find(i);
        for(int ix2 = 0 ; ix2 < L; ++ix2){
        for(int iy2 = 0 ; iy2 < L; ++iy2){
            int j = (pos2index(L-1, ix2, iy2));
            int cj = find(j);
            if(ci == cj){
                return 1.0;
            }
        }
        }
    }
    }
    return 0.0;
}

int count_max_cluster(void) {
      std::vector<int> size(N, 0);
      for (int i = 0; i < N; i++) {
        int ci = find(i);
        size[ci]++;
      }
      int max = *std::max_element(size.begin(), size.end());
      return max;
}

int count_max_cluster_id(void) {
      std::vector<int> size(N, 0);
      for (int i = 0; i < N; i++) {
        int ci = find(i);
        size[ci]++;
      }
      auto max_el = std::max_element(size.begin(), size.end());
      int max_id = std::distance(size.begin(), max_el);
      return max_id;
}

int count_active_sites(void) {
    int sum = std::accumulate(sites.begin(), sites.end(), 0);
  return sum;
}

int count_trees(void) {
    int sum = 0;
    for (int i = 0; i < N; i++) {
        if(sites[i] == 1 and parent[i] == i){
            sum++ ;
        }
    }
    return sum;
}

void put_string_of_time(std::string &starting_date){
    std::time_t t = std::time(nullptr);
    std::stringstream ss ;
    ss<< std::put_time(std::localtime(&t), "%c %Z");
    starting_date+=ss.str();
}

int main(int argc, char** argv){
    // getting and printing time
    std::string starting_date;
    put_string_of_time(starting_date);
    std::cout << "Execution start at " << starting_date << std::endl;

    // making command line options
    std::string  fname_input;
    double  G0 ;
    std::string  fout_prefix;

    po::options_description opt("This program analyze percolation cluster");
    opt.add_options()
      ("help,h" ,                                          "show help")
      ("finp"   ,       po::value<std::string>(),          "file name of input")
      ("G0"     ,po::value<double>()->default_value(5.0),  "threshold of G, blow which sites to be connected")
      ("fout_prefix",   po::value<std::string>(),          "fout_prefix, a number of files will be made with this prefix  ");

     // analyze argc and argv and results are stored in vm
    try{
      po::variables_map vm;
      store(parse_command_line(argc, argv, opt), vm);
      notify(vm);

      if(vm.count("help")){
        std::cout << opt << std::endl; // show help
        exit(1);
      }
      else if(!vm.count("finp")){
        std::cerr << "finp is mandatory " << std::endl;
        std::cerr << "exit!!" << std::endl;
        exit(1);
      }
      else if(!vm.count("fout_prefix")){
        std::cerr << vm.count("fout_prefix") << std::endl;
        std::cerr << "fout_prefix is mandatory " << std::endl;
        std::cerr << "exit!!" << std::endl;
        exit(1);
      }
      else
      {
        fname_input = vm["finp"].as<std::string>();
        G0 = vm["G0"].as<double>();
        fout_prefix = vm["fout_prefix"].as<std::string>();
        std::cout << "**** Input parameters ****"  << std::endl;
        std::cout << "input file name: " << fname_input << std::endl;
        std::cout << "fout_prefix: " << fout_prefix << std::endl;
        std::cout << "G0: " << G0 << std::endl;
        std::cout << "*************************\n"  << std::endl;
      }
    }
    catch (boost::bad_any_cast &e) {
        std::cout <<"something wrong and buggy happened!!"  << std::endl;
        std::cout <<"exit!!"  << std::endl;
        exit(1);
    }
    catch (std::exception  &e) {
        std::cout << e.what() << std::endl;
        std::cout <<"exit!!"  << std::endl;
        exit(1);
    }

    int nixyz[3];

    //open fval files
    std::string buf;
    std::ifstream ifs_fmap(fname_input.c_str());
    if(!ifs_fmap){ std::cerr << "can not open " << fname_input << std::endl; exit(1);}

    // first line is number of grids in each direction
    std::getline(ifs_fmap, buf);
    sscanf(buf.c_str(), "%d %d %d", &nixyz[0], &nixyz[1], &nixyz[2]);

    if(nixyz[0] != nixyz[1] or nixyz[0] != nixyz[2]){
        std::cerr << "ERROR: currently, n_x = n_y = n_z should hold. exit!" << std::endl;
        return  EXIT_FAILURE;
    }
    init(nixyz[0]);

    //data follows
    while(std::getline(ifs_fmap, buf)){
        if(DEBUG) std::cout << buf <<std::endl;

        int  ix, iy, iz ;
        double tmp;
        sscanf(buf.c_str(), "%d %d %d %lf", &ix, &iy, &iz, &tmp);

        if( ix  < 0 or nixyz[0] <= ix ){
          std::cout << "error in ix " << ix << std::endl;
        }
        if( iy  < 0 or nixyz[1] <= iy ){
          std::cout << "error in iy " << iy << std::endl;
        }
        if( iz  < 0 or nixyz[2] <= iz ){
          std::cout << "error in iz " << iz << std::endl;
        }

        if(tmp > G0){
            sites[pos2index(ix,iy,iz)] = 0 ;
        }
        else{
            sites[pos2index(ix,iy,iz)] = 1 ;
        }

    }
    ifs_fmap.close();

    const std::string fname_log_name = fout_prefix+".log";

    std::ofstream ofs_log(fname_log_name.c_str());
    if(!ofs_log){
      std::cerr << "can not open: " << fname_log_name << std::endl;
      exit(1);
    }
      ofs_log << "Excecution starts at " << starting_date << "\n" <<std::endl;

    ofs_log <<  "executed command should be like: \n";
    for(int i = 0; argv[i] != NULL; i++){
      ofs_log << boost::format("%s ")% argv[i];
    }

    for (int ix = 0; ix < L; ++ix){
        for (int iy = 0; iy < L; ++iy){
            for (int iz = 0; iz < L; ++iz){
                int i = pos2index(ix,iy,iz);
                if(ix+1 < L){
                    connect(i, pos2index(ix+1, iy, iz));
                }
                if(iy+1 < L){
                    connect(i, pos2index(ix, iy+1, iz));
                }
                if(iz+1 < L){
                    connect(i, pos2index(ix, iy, iz+1));
                }
            }
        }
    }

    std::cout << "G0 corss_x cross_y cross_z max_cluster #activesite #tree #sites maxid " <<std::endl;
    std::cout << G0 << " " ;
    std::cout << crossing_probability_x() << " " ;
    std::cout << crossing_probability_y() << " " ;
    std::cout << crossing_probability_z() << " " ;
    std::cout << count_max_cluster() << " " ;
    std::cout << count_active_sites() << " " ;
    std::cout << count_trees() << " " ;
    std::cout << N << " " ;
    std::cout << count_max_cluster_id() << " " ;
    std::cout << std::endl;

    const std::string fname_cluster_id = fout_prefix+"_cluster_id.dat";
    std::ofstream ofs_cluster(fname_cluster_id.c_str());
    if(!ofs_cluster){
      std::cerr << "can not open: " << fname_log_name << std::endl;
      exit(1);
    }
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 1999
 */

// @sect3{Include files}

// 同样，前几个include文件已经知道了，所以我们不会对它们进行评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/grid/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// 这个是新的。我们想从磁盘上读取一个三角图，做这个的类在下面的文件中声明。

#include <deal.II/grid/grid_in.h>

// 我们将使用一个圆形域，而描述其边界的对象来自这个文件。

#include <deal.II/grid/manifold_lib.h>

// 这是C++ ...

#include <fstream>
#include <iostream>

// 最后，这在以前的教程程序中已经讨论过了。

using namespace dealii;
// @sect3{The Step5 class template}

// 主类大部分和前面的例子一样。最明显的变化是删除了 make_grid 函数，因为现在创建网格是在 run 函数中完成的，其余功能都在 setup_system 中。除此以外，一切都和以前一样。

template <int dim>
class Step5
{
public:
  Step5();
  void run();

private:
  void setup_system();
  void assemble_system();
  void solve();
  void output_results(const unsigned int cycle) const;

  Triangulation<dim> triangulation;
  FE_Q<dim>          fe;
  DoFHandler<dim>    dof_handler;

  SparsityPattern      sparsity_pattern;
  SparseMatrix<double> system_matrix;

  Vector<double> solution;
  Vector<double> system_rhs;
};
// @sect3{Working with nonconstant coefficients}

// 在  step-4  中，我们展示了如何使用非恒定边界值和右手边。 在这个例子中，我们想在椭圆算子中使用一个可变系数来代替。由于我们有一个只取决于空间中的点的函数，我们可以做得更简单一些，使用一个普通的函数而不是继承自Function。

// 这是对单点的系数函数的实现。如果与原点的距离小于0.5，我们让它返回20，否则返回1。

template <int dim>
double coefficient(const Point<dim> &p)
{
  if (p.square() < 0.5 * 0.5)
    return 20;
  else
    return 1;
}
// @sect3{The Step5 class implementation}
// @sect4{Step5::Step5}

// 这个函数和以前一样。

template <int dim>
Step5<dim>::Step5()
  : fe(1)
  , dof_handler(triangulation)
{}

//  @sect4{Step5::setup_system}

// 这是前面例子中的函数 make_grid ，减去了生成网格的部分。其他一切都没有变化。

template <int dim>
void Step5<dim>::setup_system()
{
  dof_handler.distribute_dofs(fe);

  std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;

  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dsp);
  sparsity_pattern.copy_from(dsp);

  system_matrix.reinit(sparsity_pattern);

  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());
}

//  @sect4{Step5::assemble_system}

// 和前面的例子一样，这个函数在功能上没有太大变化，但仍有一些优化，我们将展示这些优化。对此，需要注意的是，如果使用高效的求解器（如预设条件的CG方法），组装矩阵和右手边会花费相当的时间，你应该考虑在某些地方使用一到两个优化。

// 该函数的前几部分与之前完全没有变化。

template <int dim>
void Step5<dim>::assemble_system()
{
  QGauss<dim> quadrature_formula(fe.degree + 1);

  FEValues<dim> fe_values(fe,
                          quadrature_formula,
                          update_values | update_gradients |
                            update_quadrature_points | update_JxW_values);

  const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);

  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

// 接下来是对所有单元的典型循环，以计算局部贡献，然后将它们转移到全局矩阵和向量中。与 step-4 相比，这部分的唯一变化是我们将使用上面定义的 coefficient() 函数来计算每个正交点的系数值。

  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      cell_matrix = 0.;
      cell_rhs    = 0.;

      fe_values.reinit(cell);

      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          const double current_coefficient =
            coefficient(fe_values.quadrature_point(q_index));
          for (const unsigned int i : fe_values.dof_indices())
            {
              for (const unsigned int j : fe_values.dof_indices())
                cell_matrix(i, j) +=
                  (current_coefficient *              // a(x_q)
                   fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                   fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                   fe_values.JxW(q_index));           // dx

              cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                              1.0 *                               // f(x_q)
                              fe_values.JxW(q_index));            // dx
            }
        }

      cell->get_dof_indices(local_dof_indices);
      for (const unsigned int i : fe_values.dof_indices())
        {
          for (const unsigned int j : fe_values.dof_indices())
            system_matrix.add(local_dof_indices[i],
                              local_dof_indices[j],
                              cell_matrix(i, j));

          system_rhs(local_dof_indices[i]) += cell_rhs(i);
        }
    }

// 有了这样构建的矩阵，我们再次使用零边界值。

  std::map<types::global_dof_index, double> boundary_values;
  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           Functions::ZeroFunction<dim>(),
                                           boundary_values);
  MatrixTools::apply_boundary_values(boundary_values,
                                     system_matrix,
                                     solution,
                                     system_rhs);
}
// @sect4{Step5::solve}

// 求解过程看起来又和前面的例子差不多。然而，我们现在将使用一个预设条件的共轭梯度算法。做出这种改变并不难。事实上，我们唯一需要改变的是，我们需要一个作为预处理程序的对象。我们将使用SSOR（对称连续过度放松），放松系数为1.2。为此， SparseMatrix 类有一个函数可以做一个SSOR步骤，我们需要把这个函数的地址和它应该作用的矩阵（也就是要反转的矩阵）以及松弛因子打包成一个对象。 PreconditionSSOR 类为我们做了这个。(  PreconditionSSOR  类需要一个模板参数，表示它应该工作的矩阵类型。默认值是 SparseMatrix@<double@> ，这正是我们在这里需要的，所以我们只需坚持使用默认值，不在角括号中指定任何东西。)

// 请注意，在目前的情况下，SSOR的表现并不比其他大多数预处理程序好多少（尽管比没有预处理好）。在下一个教程程序  step-6  的结果部分，将对不同的预处理进行简要比较。

// 有了这个，函数的其余部分就很简单了：我们现在使用我们声明的预处理程序，而不是之前创建的 PreconditionIdentity 对象，CG求解器将为我们完成其余的工作。

template <int dim>
void Step5<dim>::solve()
{
  SolverControl            solver_control(1000, 1e-12);
  SolverCG<Vector<double>> solver(solver_control);

  PreconditionSSOR<SparseMatrix<double>> preconditioner;
  preconditioner.initialize(system_matrix, 1.2);

  solver.solve(system_matrix, solution, system_rhs, preconditioner);

  std::cout << "   " << solver_control.last_step()
            << " CG iterations needed to obtain convergence." << std::endl;
}
// @sect4{Step5::output_results and setting output flags}

// 将输出写入文件的方法与上一个教程中的基本相同。唯一不同的是，我们现在需要为每个细化周期构建一个不同的文件名。

// 这个函数以VTU格式写入输出，这是VTK格式的一个变种，因为它压缩了数据，所以需要更少的磁盘空间。当然，如果你希望使用一个不理解VTK或VTU的可视化程序，DataOut类还支持许多其他格式。

template <int dim>
void Step5<dim>::output_results(const unsigned int cycle) const
{
  DataOut<dim> data_out;

  data_out.attach_dof_handler(dof_handler);
  data_out.add_data_vector(solution, "solution");

  data_out.build_patches();

  std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
  data_out.write_vtu(output);
}

//  @sect4{Step5::run}

// 在这个程序中，倒数第二件事是对 run() 函数的定义。与之前的程序不同，我们将在一个网格序列上进行计算，在每次迭代后都会进行全局细化。因此，该函数由6个周期的循环组成。在每个循环中，我们首先打印循环编号，然后决定如何处理网格。如果这不是第一个周期，我们就简单地对现有的网格进行一次全局精炼。然而，在运行这些循环之前，我们必须先生成一个网格。

// 在前面的例子中，我们已经使用了 GridGenerator 类中的一些函数。在这里，我们想从一个存储单元的文件中读取网格，这个文件可能来自其他人，也可能是一个网格生成工具的产物。

// 为了从文件中读取网格，我们生成一个数据类型为GridIn的对象，并将三角剖分与之相关联（也就是说，当我们要求它读取文件时，我们告诉它要填充我们的三角剖分对象）。然后我们打开相应的文件，用文件中的数据初始化三角剖分。

template <int dim>
void Step5<dim>::run()
{
  GridIn<dim> grid_in;
  grid_in.attach_triangulation(triangulation);
  std::ifstream input_file("circle-grid.inp");

// 我们现在想读取该文件。但是，输入文件只针对二维三角图，而这个函数是一个任意维度的模板。由于这只是一个演示程序，我们不会为不同的维度使用不同的输入文件，而是在不在二维的情况下迅速杀死整个程序。当然，由于下面的主函数假定我们是在二维空间工作，我们可以跳过这个检查，在这个版本的程序中，不会有任何不良影响。

// 事实证明，90%以上的编程错误都是无效的函数参数，如无效的数组大小等，所以我们在整个deal.II中大量使用断言来捕捉此类错误。为此， Assert 宏是一个很好的选择，因为它确保作为第一个参数的条件是有效的，如果不是，就抛出一个异常（它的第二个参数），通常会终止程序，并给出错误发生的位置和原因的信息。关于 @p Assert 宏的具体作用，可以在 @ref Exceptions "异常文档模块 "中找到更详细的讨论）。这通常会大大减少发现编程错误的时间，我们发现断言是快速编程的宝贵手段。

// 另一方面，如果你想做大的计算，所有这些检查（目前库中有超过10000个）不应该使程序太慢。为此， Assert 宏只在调试模式下使用，如果在优化模式下则扩展为零。因此，当你在小问题上测试你的程序并进行调试时，断言会告诉你问题出在哪里。一旦你的程序稳定了，你可以关闭调试，程序将在没有断言的情况下以最大速度运行你的实际计算。更准确地说：通过在优化模式下编译你的程序，关闭库中的所有检查（这些检查可以防止你用错误的参数调用函数，从数组中走出来，等等），通常可以使程序的运行速度提高四倍左右。即使优化后的程序性能更高，我们仍然建议在调试模式下开发，因为它允许库自动发现许多常见的编程错误。对于那些想尝试的人来说。从调试模式切换到优化模式的方法是用make release命令重新编译你的程序。现在 make 程序的输出应该向你表明，该程序现在是以优化模式编译的，以后也会被链接到已经为优化模式编译的库。为了切换回调试模式，只需用  make debug  命令重新编译。

  Assert(dim == 2, ExcInternalError());

// ExcInternalError是一个全局定义的异常，每当出现严重的错误时就会抛出。通常，人们希望使用更具体的异常，特别是在这种情况下，如果 dim 不等于2，人们当然会尝试做其他事情，例如使用库函数创建一个网格。终止程序通常不是一个好主意，断言实际上只应该用于不应该发生的特殊情况，但由于程序员、用户或其他人的愚蠢而可能发生。上面的情况并不是对Assert的巧妙使用，但是再次强调：这是一个教程，也许值得展示一下什么是不应该做的，毕竟。

// 所以，如果我们通过了断言，我们就知道dim==2，现在我们就可以真正地读取网格。它的格式是UCD（非结构化单元数据）（尽管惯例是使用UCD文件的后缀 inp ）。

  grid_in.read_ucd(input_file);

// 如果你想使用其他输入格式，你必须使用其他 grid_in.read_xxx 函数之一。(参见  GridIn  类的文档，以了解目前支持哪些输入格式)。

// 文件中的网格描述了一个圆。因此，我们必须使用一个流形对象，告诉三角计算在细化网格时将边界上的新点放在哪里。与 step-1 不同的是，由于GridIn不知道域的边界是圆形的（与 GridGenerator::hyper_shell) 不同的是，我们必须在创建三角网格后明确地将流形附加到边界上，以便在细化网格时获得正确的结果。

  const SphericalManifold<dim> boundary;
  triangulation.set_all_manifold_ids_on_boundary(0);
  triangulation.set_manifold(0, boundary);

  for (unsigned int cycle = 0; cycle < 6; ++cycle)
    {
      std::cout << "Cycle " << cycle << ':' << std::endl;

      if (cycle != 0)
        triangulation.refine_global(1);

// 现在我们有了一个确定的网格，我们写一些输出，做所有我们在前面的例子中已经看到的事情。

      std::cout << "   Number of active cells: "  //
                << triangulation.n_active_cells() //
                << std::endl                      //
                << "   Total number of cells: "   //
                << triangulation.n_cells()        //
                << std::endl;

      setup_system();
      assemble_system();
      solve();
      output_results(cycle);
    }
}
// @sect3{The main function}

// 主函数看起来和前面的例子中的函数差不多，所以我们就不进一步评论了。

int main()
{
  Step5<2> laplace_problem_2d;
  laplace_problem_2d.run();
  return 0;
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Rotation.hpp
/// \brief Header file for a rotation matrix class.
/// \details Light weight rotation class, intended to be fast, and not to provide
///          unnecessary functionality.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////
///
/// A note on EIGEN_MAKE_ALIGNED_OPERATOR_NEW (Sean Anderson, as of May 23, 2013)
/// (also see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
///
/// Fortunately, Eigen::Matrix3d and Eigen::Vector3d are NOT 16-byte vectorizable,
/// therefore this class should not require alignment, and can be used normally in STL.
///
/// To inform others of the issue, classes that include *fixed-size vectorizable Eigen types*,
/// see http://eigen.tuxfamily.org/dox-devel/group__TopicFixedSizeVectorizable.html,
/// must include the above macro! Furthermore, special considerations must be taken if
/// you want to use them in STL containers, such as std::vector or std::map.
/// The macro overloads the dynamic "new" operator so that it generates
/// 16-byte-aligned pointers, this MUST be in the public section of the header!
///
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef LGM_ROTATION_HPP
#define LGM_ROTATION_HPP

#include <Eigen/Dense>

namespace lgmath {
namespace so3 {

class Rotation
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation(const Rotation&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move constructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation(Rotation&& C) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy constructor (from Eigen)
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation(const Eigen::Matrix3d& C);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The rotation will be C_ba = vec2rot(aaxis_ab)
  //////////////////////////////////////////////////////////////////////////////////////////////
  // explicit because we want operator*(Eigen::Vector3d) and operator*(this) --- ambiguous
  explicit Rotation(const Eigen::Vector3d& aaxis_ab, unsigned int numTerms = 0);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor. The rotation will be C_ba = vec2rot(aaxis_ab), aaxis_ab must be 3x1
  //////////////////////////////////////////////////////////////////////////////////////////////
  // explicit because we want operator*(Eigen::Vector3d) and operator*(this) --- ambiguous
  explicit Rotation(const Eigen::VectorXd& aaxis_ab);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Destructor.
  //////////////////////////////////////////////////////////////////////////////////////////////
  ~Rotation() = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Copy assignment operator.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation& operator=(const Rotation&) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Move assignment operator.  Manually implemented as Eigen doesn't support moving.
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation& operator=(Rotation&& C) = default;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Gets the underlying rotation matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  const Eigen::Matrix3d& matrix() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the corresponding Lie algebra (axis-angle) using the logarithmic map
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d vec() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get the inverse (transpose) matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation inverse() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Reproject the rotation matrix back onto SO(3). Setting force to false triggers
  ///        a conditional reproject that only happens if the determinant is of the rotation
  ///        matrix is poor; this is more efficient than always performing it.
  //////////////////////////////////////////////////////////////////////////////////////////////
  void reproject(bool force = true);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply C_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation& operator*=(const Rotation& C_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply C_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation operator*(const Rotation& C_rhs) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief In-place right-hand side multiply this matrix by the inverse of C_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation& operator/=(const Rotation& C_rhs);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply this matrix by the inverse of C_rhs
  //////////////////////////////////////////////////////////////////////////////////////////////
  Rotation operator/(const Rotation& C_rhs) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Right-hand side multiply this matrix by the point vector p_a
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Vector3d operator*(const Eigen::Ref<const Eigen::Vector3d>& p_a) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// Rotation matrix from a to b
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix3d C_ba_;

};

} // so3
} // lgmath

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief print transformation
//////////////////////////////////////////////////////////////////////////////////////////////
std::ostream& operator<<(std::ostream& out, const lgmath::so3::Rotation& T);

#endif // LGM_ROTATION_HPP


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/core/ignore_unused.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/numeric/conversion/cast.hpp>
using namespace boost;

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

void case1()
{
    cout << numeric_limits<short>::min();               //-32768
    cout << numeric_limits<short>::max();               //32767

    cout << numeric_limits<unsigned short>::min();      //0
    cout << numeric_limits<unsigned short>::max();      //65535

    cout << numeric_limits<float>::min();               //1.17549e-38
    cout << numeric_limits<float>::max();               //3.40282e+38

    cout << endl;

    using namespace boost::numeric;

    cout << bounds<short>::lowest();                        //-32768
    cout << bounds<short>::highest();                        //32767;
    cout << bounds<short>::smallest();                  //1

    cout << bounds<float>::lowest();                        //-3.40282e+38
    cout << bounds<float>::highest();                       //3.40282e+38
    cout << bounds<float>::smallest();                  //1.17549e-38

    assert(bounds<short>::lowest()==numeric_limits<short>::min());
    assert(bounds<float>::lowest()==-numeric_limits<float>::max());

    assert(bounds<float>::lowest()==numeric_limits<float>::lowest());

    cout << endl;
}

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

void case2()
{
    using namespace boost::numeric;

    short   s = bounds<short>::highest();
    int     i = numeric_cast<int>(s);
    assert(i == s);

    try
    {
        char c = numeric_cast<char>(s);

        ignore_unused(c);
    }
    catch (std::bad_cast& e)
    {
        cout << e.what() << endl;
    }

}

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

int main()
{
    std::cout << "hello numeric" << std::endl;

    case1();
    case2();
    //case3();
}


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>

//____________________________________________________________________________//

#include <cmath>

BOOST_AUTO_TEST_CASE( test )
{
    double res = std::sin( 45. ); // sin 45 radians is actually ~ 0.85, sin 45 degrees is ~0.707

    BOOST_WARN_MESSAGE( res < 0.71, "sin(45){" << res << "} is > 0.71. Arg is not in radian?" );
}

//____________________________________________________________________________//


#pragma once
#include <iostream>
#include <boost/math/common_factor_rt.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <chrono>

#include "globals.hpp"
#include "gcd_algorithms.hpp"

//the Test_Suite namespace contains functions that aid in
//verifying the correctness of gcd algorithms as well as
//testing the performance of generic gcd routines
namespace Test_Suite{

	//these functions make sure that the gcd algorithms are correct
	template <typename IntegerType=TestType> bool Check_Algorithm_Validity(IntegerType(*fun)(IntegerType, IntegerType));
	template <typename IntegerType> bool Check_Answer_Validity(IntegerType num1, IntegerType num2, IntegerType answer);

	//utility
	template <typename IntegerType=TestType> boost::random::uniform_int_distribution<IntegerType> Get_Specified_Bit_Length_Distribution(int bit_length);

};

//these functions make sure that the gcd algorithms are correct
template <typename IntegerType> bool Test_Suite::Check_Algorithm_Validity(IntegerType(*fun)(IntegerType, IntegerType)){
	int fail_rate=0;

	IntegerType a;
	IntegerType b;

	//test some edge cases
	a=1; b=1;
	fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

	a=1; b=0;
	fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

	a=0; b=1;
	fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

	a=0; b=0;
	fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

	//test different sized bit numbers
	static boost::random::mt19937 gen(std::time(0));
	boost::random::uniform_int_distribution<IntegerType> dist;
	for (int i = 0; i < constant::number_of_tests_per_bit_length; ++i){

		//6 bit test
		dist = Get_Specified_Bit_Length_Distribution(6);
		a=dist(gen); b=dist(gen);
		fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

		//14 bit test
		dist = Get_Specified_Bit_Length_Distribution(14);
		a=dist(gen); b=dist(gen);
		fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

		//22 bit test
		dist = Get_Specified_Bit_Length_Distribution(22);
		a=dist(gen); b=dist(gen);
		fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));

		//30 bit test
		dist = Get_Specified_Bit_Length_Distribution(30);
		a=dist(gen); b=dist(gen);
		fail_rate+= !Check_Answer_Validity(a,b,fun(a,b));
	}

	bool failed = false;
	if (fail_rate > 0){failed = true;}
	return failed;
}
template <typename IntegerType> bool Test_Suite::Check_Answer_Validity(IntegerType a, IntegerType b, IntegerType d){

	//This is mostly bennett's implementation, but I added a bit to check for possible issues
	using namespace std;

	//avoid a floating point error
	if (a == 0 && b == 0){
		if (d == 0){
			return true;
		}
		else{
			std::cerr << "gcd(0,0) should be 0" << std::endl;
			return false;
		}
	}

	IntegerType r1, r2, d1, d2, p1, p2;

	r1 = a%d;
	r2 = b%d;
	d1 = a/d;
	d2 = b/d;
	p1 = d1*d;
	p2 = d2*d;

	if (r1 != 0) {
		cerr<< "Error " << a << "/" << d << " = " << d1 << " R " << r1 << endl;
		cerr << "The remainder should be 0  but is " << r1 << endl;

		return false;
	}

	if (r2 != 0) {
		cerr<< "Error " << b << "/" << d << " = " << d2 << " R " << r2 << endl;
		cerr << "The remainder should be 0  but is " << r2 << endl;

		return false;
	}

	if (p1 != a) {
		cerr << "Error, the proposed GCD is " << d << " but ("
		<< a << "/" << d << ") * " << d << "  = " <<  p1 << endl;
		cerr << "    and " << p1 << " !=  " << a << endl;
		return false;
	}

	if (p2 != b) {
		cerr << "Error, the proposed GCD is " << d << " but ("
		<< b << "/" << d << ") * " << d << "  = " <<  p2 << endl;
		cerr << "    and " << p2 << " !=  " << b << endl;
		return false;
	}

	//I also want to make sure that I'm getting the same answer as boost
	IntegerType boost_answer = boost::math::gcd(a,b);
	if (d != boost_answer){
		cerr << "the answer is " << d << ", but boost got " << boost_answer << endl;
		return false;
	}

	return true;
}

//utility
template <typename IntegerType> boost::random::uniform_int_distribution<IntegerType> Test_Suite::Get_Specified_Bit_Length_Distribution(int bit_length){
	IntegerType min = pow(2,bit_length-1);
	IntegerType max = pow(2,bit_length)-1;
	return boost::random::uniform_int_distribution<IntegerType>(min,max);
}


#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <Eigen/Dense>
#include <math.h>
#include <algorithm>

/**
 *  最初的lineTracking
 *
 *
*/

using namespace std;

const float fx = 760.4862674784594;
const float fy = 761.4971958529285;
const float cx = 631.6715834996345;
const float cy = 329.3054436037627;
const int imgWidth = 1280;
const int imgHeight = 720;
const float y = 129.5585;

inline void getXY(int u,int v,cv::Point2f& xy){
    xy.x = fy * y/(v-cy);
    xy.y = -(u-cx)*xy.x/fx;
}

void getLineworld(vector<cv::Point2f>& vIn,vector<cv::Point2f>& vOut){
    for(vector<cv::Point2f>::iterator in = vIn.begin();in!=vIn.end();++in){
        cv::Point2f out;
//        out.x = fy*y/(in->y-cy);
//        out.y = -(in->x-cx)*out.x/fx;
//        vOut.push_back(out);
        getXY(in->x,in->y,out);
        vOut.push_back(out);

    }
}

void image2PointCloud(vector<cv::Point2f> uv,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){
    int lastX =  fy * y/(uv[0].y-cy);
    int lastY = -(uv[0].x-cx)*lastX/fx;;
    for(auto p2:uv){
        pcl::PointXYZ point;
        // in camera frame  y=const z=fy * y/(v-cy) x=(u-cx)*z/fx
        // but in world frame z = y x = z y = -x
        point.x = fy * y/(p2.y-cy);
        point.y = -(p2.x-cx)*point.x/fx;
        point.z = 2;
        if(abs(point.y - lastY)>10)
            continue;
        cloud->push_back(point);
        lastY = point.y;
    }
}

enum LineState{Right=0,Left,None,Both};

Eigen::Vector3f fixCurve(pcl::PointCloud<pcl::PointXYZ>::Ptr xy){
    int size = xy->size();
    Eigen::Matrix<float,Eigen::Dynamic,3> Y;
    Eigen::Matrix<float,Eigen::Dynamic,1> X;
    for(int i = 0;i<xy->points.size();++i){
        Y(i,0) = std::pow(xy->points[i].y,2);Y(i,1) = xy->points[i].y;Y(i,2) = 1;
        X(i,0) = xy->points[i].x;
    }
    return (X.transpose()*X).inverse()*X.transpose()*Y;
}
//void calculateCurve(pcl::PointCloud<pcl::PointXYZ>::Ptr xyLeft,pcl::PointCloud<pcl::PointXYZ>::Ptr xyRight,LineState flag){
//    Eigen::Vector3f params;
//    if()
//}

float getOffset(int vline,int begin,int end){
    float offset=0;
    for(int i = begin;i<end;++i){
        cv::Point2f point;
        getXY(vline,i,point);
        offset += point.y;
    }
    return offset/(end-begin);
}

void getMiddleLine(const pcl::PointCloud<pcl::PointXYZ>::Ptr left,const pcl::PointCloud<pcl::PointXYZ>::Ptr right,pcl::PointCloud<pcl::PointXYZ>::Ptr middle){

//    int size = left->size()>right->size() ? right->size():left->size();
    bool lbigthanr = left->size()>right->size() ? 1:0;
    int ir = 0;
    int il = 0;
    if(lbigthanr){
        while(abs(left->points[il].x-right->points[ir].x)>20){
            il++;
        }
    } else{
        while(abs(left->points[il].x-right->points[ir].x)>20){
            ir++;
        }
    }
    for(ir,il; il<left->size()&&ir<right->size();++ir,++il){
        if(abs(left->points[il].x-right->points[ir].x)>10
            && abs(left->points[il].y -left->points[il-1].y)>5
               && abs(right->points[ir].y -right->points[ir-1].y)>5)
            continue;
        pcl::PointXYZ point;
        point.x = (left->points[il].x+right->points[ir].x)/2;
        point.y = (left->points[il].y+right->points[ir].y)/2;
        point.z = (left->points[il].z+right->points[ir].z)/2;
        middle->push_back(point);
    }
}

void test(){
    cv::Mat frame = cv::imread("./calib/image/distort.png");
    cv::Mat gray;
    cv::cvtColor(frame,gray,CV_RGBA2GRAY);
    cv::Mat binary;
    cv::threshold(gray,binary,0,255,CV_THRESH_OTSU);
    cv::erode(binary,binary,cv::Mat(),cv::Point(-1,-1),2);
    cv::imshow("show",binary);
    cv::waitKey(0);
}
void test1(){
    cv::Mat mat = cv::imread("./calib/image/42.png");
    if(mat.data){
        cv::imshow("test",mat);
        cv::waitKey(0);
        cv::Point2f uv1(656,674);
        cv::Point2f uv2(656,533);
        cv::Point2f uv3(1098,533);
        vector<cv::Point2f> uv{uv1,uv2,uv3};
        vector<cv::Point2f> xy;
        getLineworld(uv,xy);
        for(auto p:xy){
            cout<<p<<endl;
        }
    }
}
void test2(){
    /** 1.get the picture **/
    cv::Mat frame = cv::imread("./calib/image/51.png");
    if(!frame.data){
        cerr<<"the frame doesn't exist"<<endl;
        return;
    }
    /** 2.turn the picture to binary **/
    cv::Mat gray;
    cv::cvtColor(frame,gray,CV_RGB2GRAY);
    cv::Mat binary;
    cv::threshold(gray,binary,0,255,CV_THRESH_OTSU);
//    cv::erode(binary,binary,cv::Mat(),cv::Point(-1,-1),2);
    cv::imshow("show",binary);
    cv::waitKey(0);

    /** 3.calculate the two side point **/
    int vline = binary.cols/2;  // vertical line
    int pline = binary.rows*2/3-20; // parallel line
    vector<cv::Point2f> uvRight;
    vector<cv::Point2f> uvLeft;
    LineState flag;
    // right side
    for(int i = pline;i<binary.rows-1;++i){
        uchar* data = binary.ptr<uchar>(i);
        for(int j = vline;j<binary.cols-1;++j){
            if(data[j]==255&&data[j+1]==0){ // white to black
                data[j] = 150;
                data[j+1] = 150;
                data[j+2] = 150;
                data[j+3] = 150;
                data[j+4] = 150;
                data[j+5] = 150;
                data[j+6] = 150;
                data[j+7] = 150;
                data[j+8] = 150;
                data[j+9] = 150;
                data[j+10] = 150;
                uvRight.push_back(cv::Point2f(j,i));
                break;
            }
        }
    }
    // left side
    for(int i = pline;i<binary.rows-1;++i){
        uchar* data = binary.ptr<uchar>(i);
        for(int j = vline;j>0;--j){
            if(data[j]==255&&data[j-1]==0){ // white to black
                data[j] = 150;
                data[j-1] = 150;
                data[j-2] = 150;
                data[j-3] = 150;
                data[j-4] = 150;
                data[j-5] = 150;
                data[j-6] = 150;
                data[j-7] = 150;
                data[j-8] = 150;
                data[j-9] = 150;
                data[j-10] = 150;
                uvLeft.push_back(cv::Point2f(j,i));
                break;
            }
        }
    }
    if(uvLeft.size()>0&&uvRight.size()>0){
        flag = Both;
    }
    else if(uvLeft.size()>0){
        flag = Left;
    }
    else if(uvRight.size()>0){
        flag = Right;
    }
    else{
        flag = Both;
    }
//    cout<<cv::mean(uvLeft)<<endl;
//    cout<<uvLeft<<endl;
//    cout<<"******************"<<endl;
//    std::sort(uvLeft.begin(),uvLeft.end(),[](const cv::Point2f& p1,const cv::Point2f& p2){return p1.x>p2.x;});
    cout<<uvLeft<<endl;
    /** show the gray boundary **/
    cv::imshow("show",binary);
    cv::waitKey(0);
    cv::destroyAllWindows();

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr middle(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr average(new pcl::PointCloud<pcl::PointXYZ>());
    image2PointCloud(uvRight,cloud1);
    image2PointCloud(uvLeft,cloud2);
    pcl::copyPointCloud(*cloud1,*middle);
//    for(int i =0 ;i<middle->size();++i){
//        middle->points[i].y = 0;
//    }
    getMiddleLine(cloud1,cloud2,average);
//    float offset = getOffset(vline,pline,binary.rows-10);
//    cout<<offset<<endl;

    pcl::visualization::PCLVisualizer viewer("line");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source1(cloud1,255,0,0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source2(cloud2,0,255,0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source3(middle,255,255,255);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source4(average,0,0,255);
    viewer.addPointCloud<pcl::PointXYZ>(cloud1,source1,"cloud1");
    viewer.addPointCloud<pcl::PointXYZ>(cloud2,source2,"cloud2");
    viewer.addPointCloud<pcl::PointXYZ>(middle,source3,"cloud3");
    viewer.addPointCloud<pcl::PointXYZ>(average,source4,"cloud4");
    viewer.addCoordinateSystem(300,0,0,0);
//    for(auto p:*middle){
//        cout<<p<<endl;
//    }
    viewer.spin();
}

void test3(){
    /** 1.get the picture **/
    cv::VideoCapture cap  = cv::VideoCapture("");
    if(cap.isOpened()){
        cv::Mat frame  = cap.read();

        /** 2.turn the picture to binary **/
        cv::Mat gray;
        cv::cvtColor(frame,gray,CV_RGB2GRAY);
        cv::Mat binary;
        cv::threshold(gray,binary,0,255,CV_THRESH_OTSU);
    //    cv::erode(binary,binary,cv::Mat(),cv::Point(-1,-1),2);
        // cv::imshow("show",binary);
        // cv::waitKey(0);

        /** 3.calculate the two side point **/
        int vline = binary.cols/2;  // vertical line
        int pline = binary.rows*2/3-20; // parallel line
        vector<cv::Point2f> uvRight;
        vector<cv::Point2f> uvLeft;
        LineState flag;
        // right side
        for(int i = pline;i<binary.rows-1;++i){
            uchar* data = binary.ptr<uchar>(i);
            for(int j = vline;j<binary.cols-1;++j){
                if(data[j]==255&&data[j+1]==0){ // white to black
                    data[j] = 150;
                    data[j+1] = 150;
                    data[j+2] = 150;
                    data[j+3] = 150;
                    data[j+4] = 150;
                    data[j+5] = 150;
                    data[j+6] = 150;
                    data[j+7] = 150;
                    data[j+8] = 150;
                    data[j+9] = 150;
                    data[j+10] = 150;
                    uvRight.push_back(cv::Point2f(j,i));
                    break;
                }
            }
        }
        // left side
        for(int i = pline;i<binary.rows-1;++i){
            uchar* data = binary.ptr<uchar>(i);
            for(int j = vline;j>0;--j){
                if(data[j]==255&&data[j-1]==0){ // white to black
                    data[j] = 150;
                    data[j-1] = 150;
                    data[j-2] = 150;
                    data[j-3] = 150;
                    data[j-4] = 150;
                    data[j-5] = 150;
                    data[j-6] = 150;
                    data[j-7] = 150;
                    data[j-8] = 150;
                    data[j-9] = 150;
                    data[j-10] = 150;
                    uvLeft.push_back(cv::Point2f(j,i));
                    break;
                }
            }
        }
        if(uvLeft.size()>0&&uvRight.size()>0){
            flag = Both;
        }
        else if(uvLeft.size()>0){
            flag = Left;
        }
        else if(uvRight.size()>0){
            flag = Right;
        }
        else{
            flag = Both;
        }
    //    cout<<cv::mean(uvLeft)<<endl;
    //    cout<<uvLeft<<endl;
    //    cout<<"******************"<<endl;
    //    std::sort(uvLeft.begin(),uvLeft.end(),[](const cv::Point2f& p1,const cv::Point2f& p2){return p1.x>p2.x;});
        cout<<uvLeft<<endl;
        /** show the gray boundary **/
        cv::imshow("show",binary);
        cv::waitKey(0);
        cv::destroyAllWindows();

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::PointCloud<pcl::PointXYZ>::Ptr middle(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::PointCloud<pcl::PointXYZ>::Ptr average(new pcl::PointCloud<pcl::PointXYZ>());
        image2PointCloud(uvRight,cloud1);
        image2PointCloud(uvLeft,cloud2);
        pcl::copyPointCloud(*cloud1,*middle);
    //    for(int i =0 ;i<middle->size();++i){
    //        middle->points[i].y = 0;
    //    }
        getMiddleLine(cloud1,cloud2,average);
    //    float offset = getOffset(vline,pline,binary.rows-10);
    //    cout<<offset<<endl;

        pcl::visualization::PCLVisualizer viewer("line");
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source1(cloud1,255,0,0);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source2(cloud2,0,255,0);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source3(middle,255,255,255);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source4(average,0,0,255);
        viewer.addPointCloud<pcl::PointXYZ>(cloud1,source1,"cloud1");
        viewer.addPointCloud<pcl::PointXYZ>(cloud2,source2,"cloud2");
        viewer.addPointCloud<pcl::PointXYZ>(middle,source3,"cloud3");
        viewer.addPointCloud<pcl::PointXYZ>(average,source4,"cloud4");
        viewer.addCoordinateSystem(300,0,0,0);
    //    for(auto p:*middle){
    //        cout<<p<<endl;
    //    }
        viewer.spin();
    }

}

int main(){

    test3();

    return 0;
}

//int main() {
//
//    /** get the video **/
//    cv::VideoCapture cap = cv::VideoCapture("");
//    cv::Mat frame = cv::imread("./calib/image/50.png");
//    cap.set(CV_CAP_PROP_FRAME_WIDTH, imgWidth);
//    cap.set(CV_CAP_PROP_FRAME_HEIGHT, imgWidth);
//    LineState flag;
//    pcl::visualization::PCLVisualizer viewer("line");
//    while (cap.isOpened()) {
//        /** get frame and turn to binary **/
//        cv::Mat frame;
//        cap.read(frame);
//        cv::Mat gray;
//        cv::cvtColor(frame, gray, CV_RGB2GRAY);
//        cv::Mat binary;
//        cv::threshold(gray, binary, 0, 255, CV_THRESH_OTSU);
//        cv::erode(binary, binary, cv::Mat(), cv::Point(-1, -1), 2);
//        cv::imshow("show", binary);
//        cv::waitKey(0);
//
//        /** get point of two side **/
//        int vline = binary.cols / 2;
//        int pline = binary.rows * 2 / 3 - 20;
//        vector<cv::Point2f> uvRight;
//        vector<cv::Point2f> uvLeft;
//        // right side
//        for (int i = pline; i < binary.rows - 1; ++i) {
//            uchar *data = binary.ptr<uchar>(i);
//            for (int j = vline; j < binary.cols - 1; ++j) {
//                if (data[j] == 255 && data[j + 1] == 0) {
//                    data[j] = 150;
//                    data[j + 1] = 150;
//                    data[j + 2] = 150;
//                    data[j + 3] = 150;
//                    data[j + 4] = 150;
//                    data[j + 5] = 150;
//                    data[j + 6] = 150;
//                    data[j + 7] = 150;
//                    data[j + 8] = 150;
//                    data[j + 9] = 150;
//                    data[j + 10] = 150;
//                    uvRight.push_back(cv::Point2f(j, i));
//                    break;
//
//                }
//            }
//        }
//        // left side
//        for (int i = pline; i < binary.rows - 1; ++i) {
//            uchar *data = binary.ptr<uchar>(i);
//            for (int j = vline; j > 0; --j) {
//                if (data[j] == 255 && data[j - 1] == 0) {
//                    data[j] = 150;
//                    data[j - 1] = 150;
//                    data[j - 2] = 150;
//                    data[j - 3] = 150;
//                    data[j - 4] = 150;
//                    data[j - 5] = 150;
//                    data[j - 6] = 150;
//                    data[j - 7] = 150;
//                    data[j - 8] = 150;
//                    data[j - 9] = 150;
//                    data[j - 10] = 150;
//                    uvLeft.push_back(cv::Point2f(j, i));
//                    break;
//                }
//            }
//        }
//        if(uvLeft.size()>0&&uvRight.size()>0){
//            flag = Both;
//        }
//        else if(uvLeft.size()>0){
//            flag = Left;
//        }
//        else if(uvRight.size()>0){
//            flag = Right;
//        }
//        else{
//            flag = Both;
//        }
//
//        /** show the boundary **/
//        cv::imshow("show", binary);
//        cv::waitKey(0);
//
//        /** visualization **/
//        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>());
//        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>());
//        image2PointCloud(uvRight, cloud1);
//        image2PointCloud(uvLeft, cloud2);
//        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source1(cloud1, 255, 0, 0);
//        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source2(cloud2, 255, 255, 0);
//        viewer.addPointCloud<pcl::PointXYZ>(cloud1, source1, "cloud1");
//        viewer.addPointCloud<pcl::PointXYZ>(cloud2, source2, "cloud2");
//        viewer.addCoordinateSystem(1000, 0, 0, 0);
//        viewer.spin();
//
//    }
//    return 0;
//}


/**
 * ODE solver - JMU REU 2021
 *
 * @author Mike Lam
 */

// standard headers
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <cmath>
#include <functional>
using namespace std;

// Boost ODEint headers (individual files compile faster than the catch-all)
#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/integrate/integrate_const.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
using namespace boost::numeric::odeint;

// floating-point precision control (control via compiler parameter -Dreal_t)
//typedef double real_t;      // 64 bits
//typedef float real_t;       // 32 bits

// solver state
typedef std::vector<real_t> state_t;

// parameters (most read from command line)
real_t b, c, x0;
double t0 = 0.0;
double tn, dt;

// output stream
ofstream out;

// ODE system
void ode(const state_t &x, state_t &dxdt, const real_t /*t*/)
{
    dxdt[0] = b*x[0] + c;
}

// debug output helper
void observe(const state_t &x, const real_t t)
{
    // known closed-form solution
    real_t sol = (x0+c/b) * exp(b*t) - c/b;

    out << setw(8) << t << " " << setw(12) << x[0]
         //<< " " << setw(12) << sol
         << " " << setw(12) << fabs(sol-x[0]) << endl;
}

int main(int argc, const char* argv[])
{
    // check parameters
    if (argc != 6) {
        cout << "Usage: " << argv[0] << " <b> <c> <x0> <tn> <dt>" << endl;
        return EXIT_FAILURE;
    }

    // parse parameters
    b  = stod(argv[1], NULL);
    c  = stod(argv[2], NULL);
    x0 = stod(argv[3], NULL);
    tn = stod(argv[4], NULL);
    dt = stod(argv[5], NULL);
    // TODO: add stepper and t0?

    // initialize state
    state_t x(1);
    x[0] = x0;

    // show floating-point width
    //cout << "sizeof(real_t)=" << sizeof(real_t) << endl;

    // stepper
    runge_kutta4<state_t> stp;

    // integrate w/ debug output
    out.open("out.dat");
    /*size_t steps =*/ integrate_const(stp, ode, x, t0, tn, dt, observe);
    out.close();

    // show final output
    //cout << "steps=" << steps << " x=" << x[0] << endl;

    return EXIT_SUCCESS;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestPartition
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/partition.hpp>
#include <boost/compute/algorithm/partition_copy.hpp>
#include <boost/compute/algorithm/is_partitioned.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(partition_float_vector)
{
    bc::vector<float> vector(context);
    vector.push_back(1.0f, queue);
    vector.push_back(2.0f, queue);
    vector.push_back(-1.0f, queue);
    vector.push_back(-2.0f, queue);
    vector.push_back(3.0f, queue);
    vector.push_back(4.0f, queue);
    vector.push_back(-3.0f, queue);
    vector.push_back(-4.0f, queue);

    // verify is_partitioned()
    BOOST_VERIFY(bc::is_partitioned(vector.begin(),
                                    vector.end(),
                                    bc::signbit_<float>(),
                                    queue) == false);

    // partition by signbit
    bc::vector<float>::iterator iter = bc::partition(vector.begin(),
                                                     vector.end(),
                                                     bc::signbit_<float>(),
                                                     queue);
    queue.finish();
    BOOST_VERIFY(iter == vector.begin() + 4);
    BOOST_CHECK_LT(vector[0], 0.0f);
    BOOST_CHECK_LT(vector[1], 0.0f);
    BOOST_CHECK_LT(vector[2], 0.0f);
    BOOST_CHECK_LT(vector[3], 0.0f);
    BOOST_CHECK_GT(vector[4], 0.0f);
    BOOST_CHECK_GT(vector[5], 0.0f);
    BOOST_CHECK_GT(vector[6], 0.0f);
    BOOST_CHECK_GT(vector[7], 0.0f);

    // verify is_partitioned()
    BOOST_VERIFY(bc::is_partitioned(vector.begin(),
                                    vector.end(),
                                    bc::signbit_<float>(),
                                    queue) == true);
}

BOOST_AUTO_TEST_CASE(partition_small_vector)
{
    bc::vector<float> vector(context);
    bc::partition(vector.begin(), vector.end(), bc::signbit_<float>(), queue);

    vector.push_back(1.0f, queue);
    bc::partition(vector.begin(), vector.end(), bc::signbit_<float>(), queue);
    CHECK_RANGE_EQUAL(float, 1, vector, (1.0f));

    vector.push_back(-1.0f, queue);
    bc::partition(vector.begin(), vector.end(), bc::signbit_<float>(), queue);
    CHECK_RANGE_EQUAL(float, 2, vector, (-1.0f, 1.0f));
}

BOOST_AUTO_TEST_SUITE_END()


/**
 *  This file is part of dvo.
 *
 *  Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
 *  For more information see <http://vision.in.tum.de/data/software/dvo>.
 *
 *  dvo is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  dvo is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with dvo.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dvo/core/least_squares.h>

#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <dvo/core/math_sse.h>

static const dvo::core::NumType normalizer = 1.0 / (255.0 * 255.0);
static const dvo::core::NumType normalizer_inverse = 255.0 * 255.0;

// ------ Normal Equations Cholesky ------

dvo::core::NormalEquationsLeastSquares::~NormalEquationsLeastSquares() { }

void dvo::core::NormalEquationsLeastSquares::initialize(const size_t maxnum_constraints)
{
  A.setZero();
  A_opt.setZero();
  b.setZero();
  error = 0;
  this->num_constraints = 0;
  this->maxnum_constraints = maxnum_constraints;
}

void dvo::core::NormalEquationsLeastSquares::update(const dvo::core::Vector6& J, const NumType& res, const NumType& weight)
{
  NumType factor = weight;//weight * normalizer; // what happens without the normalizer? nothing!
  A_opt.rankUpdate(J, factor);
  //MathSse<Sse::Enabled, NumType>::addOuterProduct(A, J, factor);
  //A += J * J.transpose() * factor;
  MathSse<Sse::Enabled, NumType>::add(b, J, -res * factor); // not much difference :(
  //b -= J * res * factor;

  //error += res * res * factor;
  num_constraints += 1;
}

void dvo::core::NormalEquationsLeastSquares::update(const Eigen::Matrix<NumType, 2, 6>& J, const Eigen::Matrix<NumType, 2, 1>& res, const Eigen::Matrix<NumType, 2, 2>& weight)
{
  A_opt.rankUpdate(J, weight);
  b -= J.transpose() * weight * res;

  num_constraints += 1;
}

void dvo::core::NormalEquationsLeastSquares::combine(const dvo::core::NormalEquationsLeastSquares& other)
{
  A_opt += other.A_opt;
  b += other.b;
  //error += other.error;
  num_constraints += other.num_constraints;
}

void dvo::core::NormalEquationsLeastSquares::finish()
{
  A_opt.toEigen(A);
  //A /= (NumType) num_constraints;
  //b /= (NumType) num_constraints;
  //error /= (NumType) num_constraints;
}

void dvo::core::NormalEquationsLeastSquares::solve(dvo::core::Vector6& x)
{
  x = A.ldlt().solve(b);
}

// ------ Normal Equations EVD ------

dvo::core::EvdLeastSquares::~EvdLeastSquares() { }

void dvo::core::EvdLeastSquares::solve(dvo::core::Vector6& x)
{
  // eigen value decomposition seems to be equivalent to SVD for our matrix A
  Eigen::SelfAdjointEigenSolver<dvo::core::Matrix6x6> eigensolver(A);
  dvo::core::Vector6 eigenvalues = eigensolver.eigenvalues();
  dvo::core::Matrix6x6 eigenvectors = eigensolver.eigenvectors();

  bool singular = false;

  for(int i = 0; i < 6; ++i)
  {
    if(eigenvalues(i) < 0.05)
    {
      singular = true;
      throw std::exception();
    }
    else
    {
      eigenvalues(i) = 1.0 / eigenvalues(i);
    }
  }

  x = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose() * b;
}

// ------ SVD ------

dvo::core::SvdLeastSquares::~SvdLeastSquares() { }

void dvo::core::SvdLeastSquares::initialize(const size_t maxnum_constraints)
{
  J.resize(maxnum_constraints, Eigen::NoChange);
  residuals.resize(maxnum_constraints, Eigen::NoChange);

  current = 0;
}

void dvo::core::SvdLeastSquares::update(const dvo::core::Vector6& J, const NumType& res, const NumType& weight)
{
  this->J.row(current) = J;
  this->residuals(current) = res;

  current += 1;
}

void dvo::core::SvdLeastSquares::finish()
{
  J.conservativeResize(current, Eigen::NoChange);
  residuals.conservativeResize(current);
}

void dvo::core::SvdLeastSquares::solve(dvo::core::Vector6& x)
{
  x = J.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(residuals);
}

// ------ Precomputed Normal Equations Cholesky ------

dvo::core::PrecomputedNormalEquationsLeastSquares::~PrecomputedNormalEquationsLeastSquares() { }

void dvo::core::PrecomputedNormalEquationsLeastSquares::initialize(const size_t maxnum_constraints)
{
  hessian_.setZero();
  jacobian_cache_.resize(Eigen::NoChange, maxnum_constraints);
  mask_ = cv::Mat1b::zeros(maxnum_constraints, 1);

  this->num_constraints = 0;
  this->maxnum_constraints = maxnum_constraints;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::reset()
{
  A = hessian_;
  hessian_error_.setZero();
  b.setZero();
  error = 0;

  mask_ptr_ = mask_.ptr();
  num_constraints = 0;
}
void dvo::core::PrecomputedNormalEquationsLeastSquares::next()
{
  ++mask_ptr_;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::addConstraint(const size_t& idx, const dvo::core::Vector6& J)
{
  //hessian_cache_.block<6, 6>(idx * 6, 0) = J * J.transpose() * normalizer;
  //hessian_cache_[idx] = J * J.transpose() * normalizer;

  //hessian_ += J * J.transpose() * normalizer;
  MathSse<Sse::Enabled, NumType>::addOuterProduct(hessian_, J, normalizer);

  jacobian_cache_.col(idx) = J * normalizer;
  mask_.at<uchar>(idx) = 1;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::ignoreConstraint(const size_t& idx)
{
  if((*mask_ptr_) == 0) return;

  const Vector6& J = jacobian_cache_.col(idx);

  /**
   *  J is already multiplied with normalizer, so it is:
   *
   *    J' * J'.transpose() * normalizer * normalizer
   *
   *  multiplying with normalizer_inverse gives:
   *
   *    J' * J'.transpose() * normalizer * normalizer * normalizer_inverse
   *
   *  resulting in:
   *
   *    J' * J'.transpose() * normalizer
   */
  hessian_error_ -= J * J.transpose() * normalizer_inverse;
}

bool dvo::core::PrecomputedNormalEquationsLeastSquares::setResidualForConstraint(const size_t& idx, const NumType& res, const NumType& weight)
{
  if(*mask_ptr_ == 0) return false;

  //A += *hessian_cache_it_;
  b -= jacobian_cache_.col(idx) * res * weight;

  error += res * res * weight * normalizer;
  this->num_constraints +=1;

  return true;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::finish()
{
  A += hessian_error_;
  A /= (double) num_constraints;
  b /= (double) num_constraints;
  error /= (double) num_constraints;
}

void dvo::core::PrecomputedNormalEquationsLeastSquares::solve(dvo::core::Vector6 & x)
{
  x = A.ldlt().solve(b);
}