#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <kv/vleq.hpp>

namespace ub = boost::numeric::ublas;
typedef kv::interval<double> itv;

int main()
{
	ub::matrix<itv> a(2, 2);
	ub::matrix<itv> b(2, 1);
	ub::matrix<itv> x(2, 1);

	a(0, 0) = 1.; a(0, 1) = 2.;
	a(1, 0) = 3.; a(1, 1) = 4.;

	b(0, 0) = 5.; b(1, 0) = 6.;

	kv::vleq(a, b, x);

	std::cout.precision(20);
	std::cout << x << "\n";
	std::cout << prod(a, x) << "\n";
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 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://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <cstdlib>
#include <iostream>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/utility/source.hpp>

namespace compute = boost::compute;

// return a random float between lo and hi
float rand_float(float lo, float hi)
{
    float x = (float) std::rand() / (float) RAND_MAX;

    return (1.0f - x) * lo + x * hi;
}

// this example demostrates a black-scholes option pricing kernel.
int main()
{
    // number of options
    const int N = 4000000;

    // black-scholes parameters
    const float risk_free_rate = 0.02f;
    const float volatility = 0.30f;

    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    std::cout << "device: " << gpu.name() << std::endl;

    // initialize option data on host
    std::vector<float> stock_price_data(N);
    std::vector<float> option_strike_data(N);
    std::vector<float> option_years_data(N);

    std::srand(5347);
    for(int i = 0; i < N; i++){
        stock_price_data[i] = rand_float(5.0f, 30.0f);
        option_strike_data[i] = rand_float(1.0f, 100.0f);
        option_years_data[i] = rand_float(0.25f, 10.0f);
    }

    // create memory buffers on the device
    compute::vector<float> call_result(N, context);
    compute::vector<float> put_result(N, context);
    compute::vector<float> stock_price(N, context);
    compute::vector<float> option_strike(N, context);
    compute::vector<float> option_years(N, context);

    // copy initial values to the device
    compute::copy_n(stock_price_data.begin(), N, stock_price.begin(), queue);
    compute::copy_n(option_strike_data.begin(), N, option_strike.begin(), queue);
    compute::copy_n(option_years_data.begin(), N, option_years.begin(), queue);

    // source code for black-scholes program
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        // approximation of the cumulative normal distribution function
        static float cnd(float d)
        {
            const float A1 =  0.319381530f;
            const float A2 = -0.356563782f;
            const float A3 =  1.781477937f;
            const float A4 = -1.821255978f;
            const float A5 =  1.330274429f;
            const float RSQRT2PI = 0.39894228040143267793994605993438f;

            float K = 1.0f / (1.0f + 0.2316419f * fabs(d));
            float cnd =
                RSQRT2PI * exp(-0.5f * d * d) *
                (K * (A1 + K * (A2 + K * (A3 + K * (A4 + K * A5)))));

            if(d > 0){
                cnd = 1.0f - cnd;
            }

            return cnd;
        }

        // black-scholes option pricing kernel
        __kernel void black_scholes(__global float *call_result,
                                    __global float *put_result,
                                    __global const float *stock_price,
                                    __global const float *option_strike,
                                    __global const float *option_years,
                                    float risk_free_rate,
                                    float volatility)
        {
            const uint opt = get_global_id(0);

            float S = stock_price[opt];
            float X = option_strike[opt];
            float T = option_years[opt];
            float R = risk_free_rate;
            float V = volatility;

            float sqrtT = sqrt(T);
            float d1 = (log(S / X) + (R + 0.5f * V * V) * T) / (V * sqrtT);
            float d2 = d1 - V * sqrtT;
            float CNDD1 = cnd(d1);
            float CNDD2 = cnd(d2);

            float expRT = exp(-R * T);
            call_result[opt] = S * CNDD1 - X * expRT * CNDD2;
            put_result[opt] = X * expRT * (1.0f - CNDD2) - S * (1.0f - CNDD1);
        }
    );

    // build black-scholes program
    compute::program program = compute::program::create_with_source(source, context);
    program.build();

    // setup black-scholes kernel
    compute::kernel kernel(program, "black_scholes");
    kernel.set_arg(0, call_result);
    kernel.set_arg(1, put_result);
    kernel.set_arg(2, stock_price);
    kernel.set_arg(3, option_strike);
    kernel.set_arg(4, option_years);
    kernel.set_arg(5, risk_free_rate);
    kernel.set_arg(6, volatility);

    // execute black-scholes kernel
    queue.enqueue_1d_range_kernel(kernel, 0, N, 0);

    // print out the first option's put and call prices
    float call0, put0;
    compute::copy_n(put_result.begin(), 1, &put0, queue);
    compute::copy_n(call_result.begin(), 1, &call0, queue);

    std::cout << "option 0 call price: " << call0 << std::endl;
    std::cout << "option 0 put price: " << put0 << std::endl;

    // due to the differences in the random-number generators between linux and
    // mac os x, we will get different "expected" results for this example
#ifdef __APPLE__
    double expected_call0 = 0.000249461;
    double expected_put0 = 26.2798;
#else
    double expected_call0 = 0.0999f;
    double expected_put0 = 43.0524f;
#endif

    // check option prices
    if(std::abs(call0 - expected_call0) > 1e-4 || std::abs(put0 - expected_put0) > 1e-4){
        std::cerr << "error: option prices are wrong" << std::endl;
        return -1;
    }

    return 0;
}


#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/ceres.h"
#include "glog/logging.h"
#include <pangolin/pangolin.h>

using ceres::AutoDiffCostFunction;
using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
using Eigen::Isometry3d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using Eigen::Matrix3d;
using Eigen::cos;
using Eigen::sin;
using std::vector;

const double DT = 1.0 / 18;
// const Eigen::Vector3d GRAVITY{0, 0, 0};
const Eigen::Vector3d GRAVITY{0, 0, -9.8};

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

struct State {
  Eigen::Vector3d pos = Eigen::Vector3d::Random(); // position
  Eigen::Vector3d vel = Eigen::Vector3d::Random(); // velocity
  Eigen::Vector3d aaxis = Eigen::Vector3d::Random(); // Lie Algebra (Angle axis)
  // Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); // pose Qwr

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct Measurement{
  Eigen::Matrix3d Rwr;
  Eigen::Quaterniond qwr;
  Eigen::Vector3d twr;
  Eigen::Vector3d acc;
  Eigen::Vector3d omega;

  Measurement(Eigen::Matrix3d Rwr,
              Eigen::Quaterniond qwr,
              Eigen::Vector3d twr,
              Eigen::Vector3d acc,
              Eigen::Vector3d omega)
    : Rwr(Rwr), qwr(qwr), twr(twr), acc(acc), omega(omega) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct PoseError {
  PoseError(const Eigen::Vector3d& pos_measured,
            const Eigen::Matrix3d& R_measured)
    : pos_measured_(pos_measured), R_measured_(R_measured) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const aaxis_hat_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 6, 1> residuals;

    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
    Eigen::Matrix<T, 3, 1> pos_delta;
    residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();

    Eigen::Matrix<T, 3, 1> axis_hat(aaxis_hat_ptr);
    Eigen::AngleAxisd aaxis_m(R_measured_);

    T angle_hat = -axis_hat.norm();
    axis_hat.normalize();
    T sin_half_hat = sin(angle_hat / T(2.0));
    T cos_half_hat = cos(angle_hat / T(2.0));

    T angle_m = T(aaxis_m.angle());
    Eigen::Matrix<T, 3, 1> axis_m = aaxis_m.axis().template cast<T>();
    T sin_half_m = sin(angle_m / T(2.0));
    T cos_half_m = cos(angle_m / T(2.0));

    T cos_half_delta = cos_half_hat * cos_half_m - sin_half_hat * sin_half_m * axis_hat.dot(axis_m);
    Eigen::Matrix<T, 3, 1> axis_delta = sin_half_hat * cos_half_m * axis_hat +
                                        cos_half_hat * sin_half_m * axis_m +
                                        sin_half_hat * sin_half_m * axis_hat.cross(axis_m);

    residuals.template block<3, 1>(3, 0) = axis_delta;
    for (int i = 0; i < 6; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& pos_measured,
                              const Eigen::Matrix3d& R_measured) {
    return new AutoDiffCostFunction<PoseError, 6, 3, 3>(
      new PoseError(pos_measured, R_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d pos_measured_;
  const Eigen::Matrix3d R_measured_;
};

struct PredictionError{
  PredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const pos_b_ptr,
                  const T* const vel_b_ptr,
                  const T* const aaxis_b_ptr,
                  const T* const pos_e_ptr,
                  const T* const vel_e_ptr,
                  const T* const aaxis_e_ptr,
                  const T* const bias_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 9, 1> residuals;

    // rot error
    Eigen::Matrix<T, 3, 1> axis_b(aaxis_b_ptr);
    Eigen::Matrix<T, 3, 1> axis_e(aaxis_e_ptr);
    Eigen::Matrix<T, 3, 1> axis_d = omega_measured_.template cast<T>() * DT;

    int choice = 1;
    if (choice == 0) {
      // Eigen::AngleAxis<T> aaxis_b(axis_b.norm(), axis_b.normalized());
      // Eigen::AngleAxis<T> aaxis_e(axis_e.norm(), axis_e.normalized());
      // Eigen::AngleAxis<T> aaxis_d(axis_d.norm(), axis_d.normalized());

      // Eigen::AngleAxis<T> aaxis_delta(aaxis_d.inverse() * aaxis_b.inverse() * aaxis_e);
      // residuals.template block<3, 1>(6, 0) = aaxis_delta.angle() * aaxis_delta.axis();
    } else if (choice == 1) {
    }
    // Reference: https://math.stackexchange.com/questions/382760/composition-of-two-axis-angle-rotations
    T angle_b = -axis_b.norm();
    axis_b.normalize();
    T sin_half_b = sin(angle_b / T(2));
    T cos_half_b = cos(angle_b / T(2));

    T angle_e = axis_e.norm();
    axis_e.normalize();
    T sin_half_e = sin(angle_e / T(2));
    T cos_half_e = cos(angle_e / T(2));

    T cos_half_be = cos_half_b * cos_half_e - sin_half_b * sin_half_e * axis_b.dot(axis_e);
    Eigen::Matrix<T, 3, 1> axis_be = sin_half_b * cos_half_e * axis_b + cos_half_b * sin_half_e * axis_e + sin_half_b * sin_half_e * axis_b.cross(axis_e);
    T sin_half_be = axis_be.norm();
    axis_be.normalize();

    T angle_d = -axis_d.norm();
    axis_d.normalize();
    T sin_half_d = sin(angle_d / T(2));
    T cos_half_d = cos(angle_d / T(2));

    T cos_half_delta = cos_half_d * cos_half_be - sin_half_d * sin_half_be * axis_d.dot(axis_be);
    Eigen::Matrix<T, 3, 1> axis_delta = sin_half_d * cos_half_be * axis_d + cos_half_d * sin_half_be * axis_be + sin_half_d * sin_half_be * axis_d.cross(axis_be);

    residuals.template block<3, 1>(6, 0) = axis_delta;

    angle_b = -angle_b;
    T sin_b = sin(angle_b);
    T cos_b = cos(angle_b);
    Eigen::Matrix<T, 3, 3> axis_b_cross = Eigen::Matrix<T, 3, 3>::Zero();
    axis_b_cross(0, 1) = -axis_b[2];
    axis_b_cross(1, 0) = axis_b[2];
    axis_b_cross(0, 2) = axis_b[1];
    axis_b_cross(2, 0) = -axis_b[1];
    axis_b_cross(1, 2) = -axis_b[0];
    axis_b_cross(2, 1) = axis_b[0];

    Eigen::Matrix<T, 3, 3> rot_b = cos_b * Eigen::Matrix<T, 3, 3>::Identity() +
                                   (T(1.0) - cos_b) * axis_b * axis_b.transpose() +
                                   sin_b * axis_b_cross;
    // vel error
    const Eigen::Matrix<T, 3, 1> bias(bias_ptr);
    const Eigen::Matrix<T, 3, 1> vel_b(vel_b_ptr);
    const Eigen::Matrix<T, 3, 1> vel_e(vel_e_ptr);
    residuals.template block<3, 1>(3, 0) = vel_b + (rot_b * (acc_measured_.template cast<T>() - bias) - GRAVITY) * DT - vel_e;

    // pos error
    const Eigen::Matrix<T, 3, 1> pos_b(pos_b_ptr);
    const Eigen::Matrix<T, 3, 1> pos_e(pos_e_ptr);
    residuals.template block<3, 1>(0, 0) = pos_b + vel_b * DT - pos_e;

    for (int i = 0; i < 9; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<PredictionError, 9, 3, 3, 3, 3, 3, 3, 3>(
      new PredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

std::vector<Measurement, Eigen::aligned_allocator<Measurement>> readSensorData(std::string path) {
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      // std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    Eigen::Matrix3d Rwr;
    Eigen::Quaterniond qwr;
    Eigen::Vector3d twr;
    Eigen::Vector3d acc;
    Eigen::Vector3d omega;
    Rwr << row[0], row[1], row[2],
          row[4], row[5], row[6],
          row[8], row[9], row[10];
    qwr = Rwr;
    twr << row[3], row[7], row[11];
    acc << row[16], row[17], row[18];
    omega << row[19], row[20], row[21];
    std::cout << "Rwr: " << Rwr << std::endl;
    std::cout << "qwr: " << qwr.w() << " " << qwr.vec() << std::endl;
    std::cout << "twr: " << twr << std::endl;
    std::cout << "acc: " << acc << std::endl;
    std::cout << "omega: " << omega << std::endl;

    ret.push_back(Measurement(Rwr, qwr, twr, acc, omega));
  }

  return ret;
}

double abs_pos_error(const std::vector<State, Eigen::aligned_allocator<State>>& states,
                     const std::vector<State, Eigen::aligned_allocator<State>>& gt_states) {
  double err = 0.0;
  std::cout << "abs_pos_err by step: ";
  for (int i = 0; i < states.size(); i++) {
    double step_err = (states[i].pos - gt_states[i].pos).norm();
    err += step_err;
    std::cout << step_err << " ";
  }
  std::cout << std::endl;
  return err;
}

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "missing arg for the csv file" << std::endl;
  }

  std::string path = argv[1];
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> data = readSensorData(path);

  int cnt = data.size();
  // int cnt = 3;

  // Eigen::Vector3d bias = Eigen::Vector3d::Random();
  Eigen::Vector3d bias;
  bias << 0, 0, 0;
  std::vector<State, Eigen::aligned_allocator<State>> states(cnt);
  std::vector<State, Eigen::aligned_allocator<State>> gt_states(cnt);
  std::cout << "states size: " << states.size() << std::endl;

  for (int i = 0; i < gt_states.size(); i++) {
    gt_states[i].pos = data[i].twr;
    gt_states[i].vel = Eigen::Vector3d::Zero();
    Eigen::AngleAxisd temp(data[i].Rwr);
    gt_states[i].aaxis = temp.angle() * temp.axis();
  }

  Problem problem;

  ceres::LossFunction* loss_function = nullptr;

  for (int i = 0; i < cnt; i++) {
    ceres::CostFunction* pos_cost_function = PoseError::Create(data[i].twr, data[i].Rwr);
    problem.AddResidualBlock(pos_cost_function,
                             loss_function,
                             states[i].pos.data(),
                             states[i].aaxis.data());
    if (i > 0) {
      ceres::CostFunction* pred_cost_function = PredictionError::Create(data[i].acc, data[i].omega);
      problem.AddResidualBlock(pred_cost_function,
                               loss_function,
                               states[i - 1].pos.data(),
                               states[i - 1].vel.data(),
                               states[i - 1].aaxis.data(),
                               states[i].pos.data(),
                               states[i].vel.data(),
                               states[i].aaxis.data(),
                               bias.data());
    }
  }

  ceres::Solver::Options options;
	options.max_num_iterations = 200;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses;
  for (int i = 0; i < cnt; i++) {
    Eigen::AngleAxisd temp(states[i].aaxis.norm(), states[i].aaxis.normalized());
    Isometry3d Twr(temp.matrix());
    Twr.pretranslate(states[i].pos / 20); // manually divided by 20 to zoom out
    poses.push_back(Twr);
  }
  for (int i = 0; i < cnt; i++) {
    Eigen::AngleAxisd temp(gt_states[i].aaxis.norm(), gt_states[i].aaxis.normalized());
    Isometry3d Twr(temp.matrix());
    Twr.pretranslate(gt_states[i].pos / 20); // manually divided by 20 to zoom out
    gt_poses.push_back(Twr);
  }

  double err = abs_pos_error(states, gt_states);
  std::cout << "absolute position error: " << err << std::endl;
  DrawTrajectoryComparison(poses, gt_poses);
  return 0;
}

void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(1.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < gt_poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = gt_poses[i].translation();
      Vector3d Xw = gt_poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = gt_poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = gt_poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < gt_poses.size(); i++) {
      glColor3f(0.0, 1.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = gt_poses[i], p2 = gt_poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}


#include <mona/utility.hpp>
#include <mona/line.hpp>
#include <mona/targets/window.hpp>
#include <mona/axes.hpp>
#include <mona/axes3.hpp>
#include <mona/colors.hpp>
#include <mona/grid.hpp>
#include <mona/surface_mesh.hpp>
#include <mona/camera_steady_control.hpp>

#include <armadillo>

auto fit(const arma::fvec& x, const arma::fvec& y) -> arma::fvec
{
    auto xx = x - arma::mean(x);
    auto yy = y - arma::mean(y);
    float b = arma::sum(xx % yy) / arma::sum(xx % xx);
    float a = arma::mean(y) - b * arma::mean(x);
    return a + b * x;
}

auto random(arma::fvec x, float mean, float std)
{
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> dis(mean, std);

    for(auto& e: x)
    {
        e += dis(gen);
    }

    return x;
}

auto get_dots()
{
    arma::fvec x = mona::linspace(-4, 4, 30);
    arma::fvec y = random(x, 0, 0.9);
    return mona::dots(x, y, mona::colors::tomato);
}

auto get_line()
{
    auto f = [](double x)
    {
        return std::sin(x);
    };

    arma::fvec x = mona::linspace(-4, 4, 50);
    arma::fvec y = x;
    y.transform(f);

    return mona::line(x, y, mona::colors::midnight_blue);
}

auto f1(double x, double y)
{
    return (7 * x * y) / std::exp(x*x + y*y);
};

auto f2(double x, double y)
{
    return std::pow(x, 4) + std::pow(y, 4) - std::pow(x + y, 2);
};

template <typename F>
auto get_surface(F f)
{
    arma::fvec line = mona::linspace(-2, 2, 50);
    auto [x, y] = mona::meshgrid(line, line);
    auto z = mona::apply(f, x, y);

    auto mesh = mona::surface_mesh(x, y, z);

    return mesh;
}

int main()
{
    auto win  = mona::targets::window();
    auto view = mona::grid(2, 2);
    auto cam  = mona::camera();
    auto axes_dots = mona::axes();
    auto axes_line = mona::axes();
    auto axes_surf1 = mona::axes3({-4, 4}, {-4, 4}, {-4, 4}, 5);
    axes_surf1.set_camera_control(win.get_camera_control());

    auto axes_surf2 = mona::axes3({-4, 4}, {-4, 4}, {-4, 4}, 5);
    axes_surf2.set_camera_control(mona::camera_steady_control(0.2f, 0.f));

    auto dots = get_dots();
    auto line = get_line();
    auto surf1 = get_surface(f1);
    auto surf2 = get_surface(f2);

    while (win.active())
    {
        axes_dots.submit(dots);
        axes_line.submit(line);
        axes_surf1.submit(surf1);
        axes_surf2.submit(surf2);

        view(0, 0).submit(axes_dots);
        view(0, 1).submit(axes_surf1);
        view(1, 0).submit(axes_surf2);
        view(1, 1).submit(axes_line);

        win.submit(view);
        win.draw();
    }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
// Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// fixed_point_detail_constants.hpp implements templates
// for computing fixed-point representations of the
// mathematical constants sqrt(2), pi, log(2) and e.

#ifndef FIXED_POINT_DETAIL_CONSTANTS_2015_08_16_HPP_
  #define FIXED_POINT_DETAIL_CONSTANTS_2015_08_16_HPP_

  #include <cmath>
  #include <limits>

  #include <boost/config.hpp>

  namespace boost { namespace fixed_point { namespace detail {

  template<typename NumericType>
  NumericType calculate_root_two()
  {
    // Provide a very rough initial guess of 4/3, which is about
    // 5 percent off of the actual value. Use small integers here
    // because the underlying NumericType might have limited range.
    NumericType a(NumericType(4U) / 3U);

    // Estimate the zero'th term of the iteration with [1 / (2 * result)],
    // giving [1 / (2 * (4/3))] = 3/8.
    using std::ldexp;
    NumericType vi(ldexp(NumericType(3U), -3));

    // Compute the square root of x using coupled Newton iteration.
    // We begin with an estimate of 1 binary digit of precision and
    // double the number of binary digits of precision with each iteration.

    for(std::uint_fast16_t i = UINT16_C(1); i <= std::uint_fast16_t(std::numeric_limits<NumericType>::digits); i *= UINT16_C(2))
    {
      // Perform the next iteration of vi.
      vi += vi * (-((a * vi) * 2U) + NumericType(1U));

      // Perform the next iteration of the result.
      a += (vi * (-((a) * (a)) + NumericType(2U)));
    }

    return a;
  }

  template<typename NumericType>
  NumericType calculate_pi()
  {
    using std::fabs;
    using std::ldexp;
    using std::sqrt;

    // Use a quadratically converging Gauss-AGM method to compute pi.

    NumericType val_pi;

    NumericType a (1U);
    NumericType bB(ldexp(NumericType(1U), -1));
    NumericType s (bB);
    NumericType t (ldexp(NumericType(3U), -3));

    // This loop is designed for computing a maximum of a few million
    // decimal digits of pi. The number of digits roughly doubles
    // with each iteration of the loop. After 20 iterations,
    // the precision is about 2.8 million decimal digits.
    // We are, however, not using that many digits in this
    // application --- rarely more than a few thousand at most.

    const NumericType tolerance = ldexp(NumericType(1U), -int((long(std::numeric_limits<NumericType>::digits) * 3L) / 4L));

    for(std::uint_least8_t k = UINT8_C(1); k < UINT8_C(32); ++k)
    {
      // Perform the iteration steps of the Gauss AGM.

      a      += sqrt(bB);
      a      /= 2U;
      val_pi  = (a * a);
      bB      = (val_pi - t) * 2U;

      const NumericType iterate_term((bB - val_pi) * (UINT32_C(1) << k));

      s += iterate_term;

      const bool minimum_number_of_iterations_is_complete = (k > UINT8_C(4));

      if((minimum_number_of_iterations_is_complete) && (fabs(iterate_term) <= tolerance))
      {
        break;
      }

      t = (val_pi + bB) / 4U;
    }

    val_pi += bB;
    val_pi /= s;

    return val_pi;
  }

  template<typename NumericType>
  NumericType calculate_ln_two()
  {
    using std::fabs;
    using std::ldexp;
    using std::sqrt;

    // Use a quadratically converging Gauss-AGM method for computing log(2).

    // Choose m > (N * 1.661), where N is the number of decimal digits requested.
    BOOST_CONSTEXPR_OR_CONST int m((long(std::numeric_limits<NumericType>::digits10) * 17L) / 10L);

    // Set a0 = 1.
    // Set b0 = 4 / (2^m) = 1 / 2^(m - 2).
    NumericType ak(1);

    NumericType bk(ldexp(NumericType(1), -(m - 2)));

    const NumericType tolerance = ldexp(NumericType(1U), -int((long(std::numeric_limits<NumericType>::digits) * 3L) / 4L));

    for(std::uint_least8_t k = UINT8_C(0); k < UINT8_C(32); ++k)
    {
      const NumericType a(ak);

      ak += bk;
      ak /= 2U;

      bk = sqrt(bk * a);

      const bool minimum_number_of_iterations_is_complete = (k > UINT8_C(4));

      if((minimum_number_of_iterations_is_complete) && (fabs(ak - bk) <= tolerance))
      {
        break;
      }
    }

    // The iteration is now finished. Compute ln2 = pi / [AGM(1, 4 / 2^m) * 2m].
    // Note that the result of the AGM iteration is [ak = bk = AGM(...)].
    const NumericType val_ln_two = calculate_pi<NumericType>() / (ak * (2U * m));

    return val_ln_two;
  }

  template<typename NumericType>
  NumericType calculate_e()
  {
    using std::fabs;

    NumericType term(1U);
    NumericType sum (2U);

    BOOST_CONSTEXPR_OR_CONST std::uint32_t maximum_number_of_iterations = UINT32_C(10000);

    // Perform the Taylor series expansion of Euler's constant, e = exp(1).
    for(std::uint32_t n = UINT32_C(2); n < maximum_number_of_iterations; ++n)
    {
      term /= n;

      const bool minimum_number_of_iterations_is_complete = (n > UINT32_C(4));

      if(   (minimum_number_of_iterations_is_complete)
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      sum += term;
    }

    return sum;
  }
  } } } // namespace boost::fixed_point::detail

#endif // FIXED_POINT_DETAIL_CONSTANTS_2015_08_16_HPP_


/*=============================================================================

MPHYG0022CW1: CW1, 2019: Linear Regression.

Copyright (c) University College London (UCL). All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.

See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "mphyNormalEquationSolverStrategy.h"
#include "vectorPairTypes.h"
#include <memory>
#include <iostream>
#include <math.h>
#include <Eigen/StdVector>
#include <Eigen/Dense>
#include <map>

namespace mphy {

pairdd normSolver::FitData(vecPairdd data){
	/*
	Finds optimal theta using the Normal Equation.
	theta_best = inv(X^T . X) . X^T . y, where:

	data:		a vector of pairs (x, y)
	X:			a 2xnumData matrix of (1, x0; 1, x1; 1, x2...)
	 */

	Eigen::Matrix<double, Eigen::Dynamic, 2> xdata;
	Eigen::VectorXd ydata, theta_best;

	xdata = copyXtoEigen(data);
	ydata = copyYtoEigen(data);

	theta_best = ((xdata.transpose()*xdata).inverse())*xdata.transpose()*ydata;

	return pairdd(theta_best(0), theta_best(1));
}

} // end namespace mphy


// Eigen lib tutorial -- Array class and coefficient-wise operations
// Source:
// http://eigen.tuxfamily.org/dox/group__TutorialArrayClass.html
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    // Accessing values inside an Array
    ArrayXXf m(2,2);

    // assign some value coefficient by coefficient
    m(0,0) = 1.0; m(0,1) = 2.0;
    m(1,0) = 3.0; m(1,1) = m(0,1) + m(1,0);

    // print values to standard output
    std::cout << m << '\n' << std::endl;

    // using the comma-initializer is also allowed
    m << 1.0, 3.0,
         5.0, 7.0;
    std::cout << m  << std::endl;

    // Array addition and subtraction
    ArrayXXf a(3,3);
    ArrayXXf b(3,3);
    a << 1,2,3,
         4,5,6,
         7,8,9;
    b << 1,2,3,
         1,2,3,
         1,2,3;

    // Adding two arrays
    std::cout << "a + b = " << '\n' << a + b << std::endl;

    // Subtracting a scalar from an array
    std::cout << "a - 2 = " << '\n' << a -2 << std::endl;

    // Array multiplication
    ArrayXXf c(2,2);
    ArrayXXf d(2,2);
    c << 1,2,
         3,4;
    d << 5,6,
         7,8;
    std::cout << "c * d = " << '\n' << c * d << std::endl;

    // Converting b/w array and matrix expressions
    MatrixXf u(2,2);
    MatrixXf v(2,2);
    MatrixXf result(2,2);

    u << 1,2,
         3,4;
    v << 5,6,
         7,8;

    result = u * v;
    std::cout << "-- Matrix u * v: --\n" << result << '\n' << std::endl;

    result = u.array() * v.array();
    std::cout << "-- Array u * v: --\n" << result << '\n' << std::endl;

    result = u.cwiseProduct(v);
    std::cout << "-- With cwiseProduct: --\n" << result << '\n' << std::endl;

    result = u.array() + 4;
    std::cout << "-- Array u + 4: --\n" << result << '\n' << std::endl;

    result = (u.array() + 4).matrix() * u;
    std::cout << "-- Combination 1: --" << std::endl << result << std::endl << std::endl;
    result = (u.array() * v.array()).matrix() * u;
    std::cout << "-- Combination 2: --" << std::endl << result << std::endl << std::endl;

    return 0;
}


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

template <class Matrix>
void kron(const Matrix & A, const Matrix & B, Matrix & C)
{
	/*int n=A.cols();
	int m=A.rows();*/
	C= Matrix(A.rows()*B.rows(),A.cols()*B.cols());
	for (int i=0;i<A.rows();++i){
		for (int j=0;j<A.cols();++j){
			C.block(i*B.rows(),j*B.cols(),B.rows(),B.cols())=A(i,j)*B;
		}
	}
	// TODO
}


//
//  common.cpp
//  kcalg
//
//  Created by knightc on 2019/7/12.
//  Copyright © 2019 knightc. All rights reserved.
//

#include <stdio.h>

#include "opentsb/math.h"
#include <NTL/GF2X.h>
#include <NTL/ZZX.h>

using namespace NTL;
using namespace std;

 void lcm(ZZ& k, const ZZ& x, const ZZ& y) {
    ZZ gcd;
    mul(k, x, y);
    GCD(gcd, x, y);
    k /= gcd;
}

//一次性运算求 向量的 lcm/gcd
vec_ZZ lcm_gcd_vec(const vec_ZZ& v,int typemode){

    vec_ZZ vec_out;
    long m,n,j,i;
    ZZ temp;
    i=0;
    j=0;

     n= v.length();
    m= ceil(n/2.0);//向上取整，必须是非整数，即 2---》2.0
      vec_out.SetLength(m);
    switch (typemode) {
        case 1:
            for (i=0; i < n-1; i+=2) {

                lcm(temp,v[i],v[i+1]);
                // cout << temp<< endl;
                vec_out[j] = temp;
                //cout << vec_out << endl;
                j++;
                // cout << j << endl;
            }
            // cout << i << endl;
            if (n % 2 == 1 ) {
                lcm( vec_out[j],v[i],ZZ(1));
            }
            break;
         case 2:
            for (i=0; i < n-1; i+=2) {
                GCD(temp,v[i],v[i+1]);
                // cout << temp<< endl;
                vec_out[j] = temp;
                //cout << vec_out << endl;
                j++;
                // cout << j << endl;
            }
            // cout << i << endl;
            if (n % 2 == 1 ) {
                GCD( vec_out[j],v[i],v[i-1]);
            }
            break;
    }
    return vec_out;
}

//求 n 个数的最大公约数/最小公倍数
vec_ZZ lcm_gcd_vec(const vec_ZZ& v,int typemode,long endleng){

    vec_ZZ vec_out;

    vec_out= lcm_gcd_vec(v, typemode);
    endleng=vec_out.length();

    while (endleng != 1) {
       vec_out= lcm_gcd_vec(vec_out, typemode);
         endleng=vec_out.length();
    }

    return vec_out;
}

//array can use "for" to assignment，but vector can't。if must，can use vector.push_back
void array_to_vec(const ZZ arr[],vec_ZZ &v,long len) {
   // long n= v.length();

    v.SetLength(len);
    for (int i=0; i<=len-1; i++) {
        v[i]=arr[i];
    }
    //cout<<v<<endl;
}

void array_to_vec(const int arr[],vec_ZZ &v,long len) {
   // long n= v.length();

    for (int i=0; i<=len-1; i++) {
        v[i]=to_ZZ(arr[i]);

    }
    // cout<<v<<endl;
}

void array2_to_mat(const vector< vector<ZZ> > arr,mat_ZZ &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=arr[i][j];
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_ZZ &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_ZZ(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_GF2 &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_GF2(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<ZZ> > arr,mat_GF2 &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_GF2(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<GF2> > arr,mat_GF2 &v){
    long m= v.NumRows();
    long n= v.NumCols();

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=arr[i][j];
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_GF2E &v){

    long m= v.NumRows();
    long n= v.NumCols();

//    const GF2X P(INIT_MONO,8);
//    GF2E::init(P);
    GF2X P;
    SetCoeff(P, 8, 1);
    SetCoeff(P, 4, 1);
    SetCoeff(P, 3, 1);
    SetCoeff(P, 1, 1);
    SetCoeff(P, 0, 1);
    GF2E::init(P);

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_GF2E(arr[i][j]);
        }
    }

}

void array2_to_mat(const vector< vector<int> > arr,mat_ZZ_p &v){

    v.SetDims(arr.size(),arr[0].size());

    long m= v.NumRows();
    long n= v.NumCols();

    ZZ_p::init(ZZ(256));

    for (int i=0;i<=m-1;i++){
        for (int j=0; j<=n-1; j++) {
            v[i][j]=to_ZZ_p(long(arr[i][j]));
        }
    }

}

//void inv_array2_to_mat(const mat_GF2E &v, vector< vector<int> > &arr) //gf2e --> int  ?????

//to_ZZX(vec);可以把一个一维数组/向量 转为多项式（系数更换）
void kc_lagrange(vec_vec_ZZ_p &pointMat,vec_zz_p &coeffVec) {

}


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "stiffness_matrix.hpp"

//----------------AssembleMatrixBegin----------------
//! Assemble the stiffness matrix
//! for the linear system.
//!
//!
//! @param[in] vertices a list of triangle vertices
//! @param[in] triangles a list of triangles
SparseMatrix assembleStiffnessMatrix(
    const Eigen::MatrixXd &vertices,
    const Eigen::MatrixXi &triangles) {
	SparseMatrix         A(vertices.rows(), vertices.rows());
	std::vector<Triplet> triplets;
	const int            numberOfElements = triangles.rows();

	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = triangles.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::Matrix3d stiffnessMatrix;
		computeStiffnessMatrix(stiffnessMatrix, a, b, c);

		for (int n = 0; n < 3; ++n) {
			for (int m = 0; m < 3; ++m) {
				auto triplet = Triplet(indexSet(n), indexSet(m), stiffnessMatrix(n, m));
				triplets.push_back(triplet);
			}
		}
	}

	A.setFromTriplets(triplets.begin(), triplets.end());

	return A;
}
//----------------AssembleMatrixEnd----------------


/**
 * @file   gweyl.hpp
 * @author ALIKAWA Hidehisa <alleyhide@gmail.com>
 * @date   2018/04/30
 *
 * @brief  public header file of gweyl
 *
 * Released under the MIT license
 */
#pragma once

#include <boost/rational.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

//
// our namespace "gweyl"
//
namespace gweyl{

//
// rational numbers
//
using rational=boost::rational<int>;

//
// matrix of rational numbers
//
using matrix=boost::numeric::ublas::matrix<rational>;

//
// number vector
// this is vector of mathematics (primitive linear algera)
//
using NumberVector=boost::numeric::ublas::vector<rational>;

//
// we define equality of matirx and number vector
// because boost does not define the equality
//
bool equal(const matrix &X, const matrix &Y);
bool equal(const NumberVector &v, const NumberVector &w);

//
// gweyl treats simple Lie algebra types
// in Cartan's classification
//
enum class Type{
    invalid,A,B,C,D,E,F,G,
};

//
// Coordinate system
// There are at least two coordinates on root spaces.
// One is simple roots, and the other is fundamental weights.
//
enum class Coordinate {
    simple, fundamental,
};

//
// trace the message
// @param[in] msg message
//
//
void trace(std::string& msg);

///
/// The definition of Cartan Matrix is equivalent to
/// the definition of the root space, according to structure theory of Lie algebra,
/// that is, Cartan matrix is the source of "gweyl"
///
matrix CartanMatrix(Type X, unsigned n);
matrix InverseCartanMatrix(Type X, unsigned n);

class VectorRootSpace;

//
// class Cartan
//  other names are RootSpace, DynkinDiagram, ..
// fundamental class of gweyl
//
class Cartan
{
public:
    explicit Cartan(Type X, unsigned n);
    Cartan();
    Cartan(const Cartan &rhs);
    virtual ~Cartan();
    Cartan& operator=(const Cartan& rhs);

    //
    // @return Cartan matrix
    //
    matrix CartanMatrix();

    //
    // @return invers of Cartan matrix
    //
    matrix InverseCartanMatrix();

    //
    // @return the 'i'-th simple root
    //
    VectorRootSpace SimpleRoot(unsigned i);

    //
    // @return the 'i'-th fundamental weight
    //
    VectorRootSpace FundamentalWeight(unsigned i);

    //
    // @return highest root <not implemented>
    //
    VectorRootSpace HighestRoot();

    //
    // @return zero vector
    //
    VectorRootSpace Zero();

    //
    // @return half sum of positive roots
    //
    VectorRootSpace Rho();

    //
    // @return positive roots
    //
    std::vector<VectorRootSpace> PositiveRoots();

    //
    // @return roots <not implemented>
    //
    std::vector<VectorRootSpace> Roots();

    // getters
    Type type();
    Type type() const;
    unsigned rank();
    unsigned rank() const;

    // operators
    bool operator==(const Cartan& rhs);
    bool operator!=(const Cartan& rhs);

private:
    struct Impl;
    std::unique_ptr<Impl> pImpl;
};

//
// In gweyl, the class RootSpace, DynkinDiagram and the class Cartan are same
//
using RootSpace = Cartan;
using DynkinDiagram = Cartan;

//
// Vector in root space
//
class VectorRootSpace
{
public:
    // constructors
    explicit VectorRootSpace(Type X, NumberVector& v, Coordinate c);
    VectorRootSpace();
    virtual ~VectorRootSpace();
    VectorRootSpace(const VectorRootSpace& rhs);
    VectorRootSpace& operator=(const VectorRootSpace& rhs);

    // for debug
    void printf();

    // getters
    NumberVector simpleCoefficients();
    NumberVector simpleCoefficients() const;
    NumberVector fundamentalCoefficients();
    NumberVector fundamentalCoefficients() const;
    Type type();
    Type type() const;
    unsigned rank();
    unsigned rank() const;

    // eqaulity
    bool isInSameSpace(const VectorRootSpace& rhs);
    bool operator==(const VectorRootSpace& rhs);
    bool operator!=(const VectorRootSpace& rhs);

    // operators
    VectorRootSpace& operator+=(const VectorRootSpace& rhs);
    VectorRootSpace& operator-=(const VectorRootSpace& rhs);
    VectorRootSpace& operator*=(rational r);

    // dominant integral
    bool dominant();
    bool integral();
    bool isDominantIntegral();
private:
    struct Impl;
    std::unique_ptr<Impl> pImpl;
};

//
// operators of the class VectorRootSpace
//
rational InnerProduct(VectorRootSpace& v, VectorRootSpace& w);
VectorRootSpace operator+(const VectorRootSpace& v1, const VectorRootSpace& v2);
VectorRootSpace operator-(const VectorRootSpace& v1, const VectorRootSpace& v2);
VectorRootSpace operator*(const VectorRootSpace& v1, const rational r);
VectorRootSpace operator*(const rational r, const VectorRootSpace& v1);

//
// class IrreducibleRepresentation
//
class IrreducibleRepresentation{
public:
    //constructors
    IrreducibleRepresentation();
    explicit IrreducibleRepresentation(VectorRootSpace& highestweight);
    virtual ~IrreducibleRepresentation();
    IrreducibleRepresentation(const IrreducibleRepresentation& rhs);
    IrreducibleRepresentation& operator=(const IrreducibleRepresentation& rhs);

    //getters
    Type type();
    Type type() const;
    unsigned rank();
    unsigned rank() const;
    VectorRootSpace highestweight();
    VectorRootSpace highestweight() const;

    //operators
    bool operator==(const IrreducibleRepresentation& rhs);
    bool operator!=(const IrreducibleRepresentation& rhs);

    int dimension();

private:
    struct Impl;
    std::unique_ptr<Impl> pImpl;
};

}


#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/MatrixFunctions>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/utility.hpp>

namespace common_robotics_utilities
{
namespace math
{
bool Equal3d(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
  if ((v1.x() == v2.x()) && (v1.y() == v2.y()) && (v1.z() == v2.z()))
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool Equal4d(const Eigen::Vector4d& v1, const Eigen::Vector4d& v2)
{
  if ((v1(0) == v2(0)) && (v1(1) == v2(1))
      && (v1(2) == v2(2)) && (v1(3) == v2(3)))
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool CloseEnough(const double p1, const double p2, const double threshold)
{
  const double real_threshold = std::abs(threshold);
  const double abs_delta = std::abs(p2 - p1);
  if (abs_delta <= real_threshold)
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool CloseEnough(const Eigen::Vector3d& v1,
                 const Eigen::Vector3d& v2,
                 const double threshold)
{
  const double real_threshold = std::abs(threshold);
  if (std::abs(v1.x() - v2.x()) > real_threshold)
  {
    return false;
  }
  if (std::abs(v1.y() - v2.y()) > real_threshold)
  {
    return false;
  }
  if (std::abs(v1.z() - v2.z()) > real_threshold)
  {
    return false;
  }
  return true;
}

Eigen::Vector3d RotateVector(const Eigen::Quaterniond& quat,
                             const Eigen::Vector3d& vec)
{
  const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
  const Eigen::Quaterniond res = quat * (temp * quat.inverse());
  return Eigen::Vector3d(res.x(), res.y(), res.z());
}

Eigen::Vector3d RotateVectorReverse(const Eigen::Quaterniond& quat,
                                    const Eigen::Vector3d& vec)
{
  const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
  const Eigen::Quaterniond res = quat.inverse() * (temp * quat);
  return Eigen::Vector3d(res.x(), res.y(), res.z());
}

double EnforceContinuousRevoluteBounds(const double value)
{
  if ((value <= -M_PI) || (value > M_PI))
  {
    const double remainder = std::fmod(value, 2.0 * M_PI);
    if (remainder <= -M_PI)
    {
      return (remainder + (2.0 * M_PI));
    }
    else if (remainder > M_PI)
    {
      return (remainder - (2.0 * M_PI));
    }
    else
    {
      return remainder;
    }
  }
  else
  {
    return value;
  }
}

Eigen::VectorXd SafeNormal(const Eigen::VectorXd& vec)
{
  if (vec.size() > 0)
  {
    const double norm = vec.norm();
    if (norm > std::numeric_limits<double>::epsilon())
    {
      return vec / norm;
    }
    else
    {
      return Eigen::VectorXd::Zero(vec.size());
    }
  }
  else
  {
    return Eigen::VectorXd::Zero(vec.size());
  }
}

double SquaredNorm(const std::vector<double>& vec)
{
  double squared_norm = 0.0;
  for (size_t idx = 0; idx < vec.size(); idx++)
  {
    const double element = vec[idx];
    squared_norm += (element * element);
  }
  return squared_norm;
}

double Norm(const std::vector<double>& vec)
{
  return std::sqrt(SquaredNorm(vec));
}

std::vector<double> Abs(const std::vector<double>& vec)
{
  std::vector<double> absed(vec.size(), 0.0);
  for (size_t idx = 0; idx < absed.size(); idx++)
  {
    absed[idx] = std::abs(vec[idx]);
  }
  return absed;
}

std::vector<double> Multiply(const std::vector<double>& vec,
                             const double scalar)
{
  std::vector<double> multiplied(vec.size(), 0.0);
  for (size_t idx = 0; idx < multiplied.size(); idx++)
  {
    const double element = vec[idx];
    multiplied[idx] = element * scalar;
  }
  return multiplied;
}

std::vector<double> Multiply(const std::vector<double>& vec1,
                             const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> multiplied(vec1.size(), 0.0);
    for (size_t idx = 0; idx < multiplied.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      multiplied[idx] = element1 * element2;
    }
    return multiplied;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

std::vector<double> Divide(const std::vector<double>& vec, const double scalar)
{
  const double inv_scalar = 1.0 / scalar;
  return Multiply(vec, inv_scalar);
}

std::vector<double> Divide(const std::vector<double>& vec1,
                           const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> divided(vec1.size(), 0.0);
    for (size_t idx = 0; idx < divided.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      divided[idx] = element1 / element2;
    }
    return divided;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

std::vector<double> Add(const std::vector<double>& vec, const double scalar)
{
  std::vector<double> added(vec.size(), 0.0);
  for (size_t idx = 0; idx < added.size(); idx++)
  {
    added[idx] = vec[idx] + scalar;
  }
  return added;
}

std::vector<double> Add(const std::vector<double>& vec1,
                        const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> added(vec1.size(), 0.0);
    for (size_t idx = 0; idx < added.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      added[idx] = element1 + element2;
    }
    return added;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

std::vector<double> Sub(const std::vector<double>& vec, const double scalar)
{
  std::vector<double> subed(vec.size(), 0.0);
  for (size_t idx = 0; idx < subed.size(); idx++)
  {
    subed[idx] = vec[idx] - scalar;
  }
  return subed;
}

std::vector<double> Sub(const std::vector<double>& vec1,
                        const std::vector<double>& vec2)
{
  if (vec1.size() == vec2.size())
  {
    std::vector<double> subed(vec1.size(), 0.0);
    for (size_t idx = 0; idx < subed.size(); idx++)
    {
      const double element1 = vec1[idx];
      const double element2 = vec2[idx];
      subed[idx] = element1 - element2;
    }
    return subed;
  }
  else
  {
    throw std::invalid_argument("vec1.size() != vec2.size()");
  }
}

double Sum(const std::vector<double>& vec)
{
  double sum = 0.0;
  for (size_t idx = 0; idx < vec.size(); idx++)
  {
    const double element = vec[idx];
    sum += element;
  }
  return sum;
}

Eigen::Matrix3d Skew(const Eigen::Vector3d& vector)
{
  Eigen::Matrix3d skewed;
  skewed << 0.0, -vector.z(), vector.y(),
        vector.z(), 0.0, -vector.x(),
        -vector.y(), vector.x(), 0.0;
  return skewed;
}

Eigen::Vector3d Unskew(const Eigen::Matrix3d& matrix)
{
  const Eigen::Matrix3d matrix_symetric = (matrix - matrix.transpose()) / 2.0;
  const Eigen::Vector3d unskewed(matrix_symetric(2, 1),
                                 matrix_symetric(0, 2),
                                 matrix_symetric(1, 0));
  return unskewed;
}

Eigen::Matrix4d TwistHat(const Eigen::Matrix<double, 6, 1>& twist)
{
  const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
  const Eigen::Matrix3d hatted_rot_velocity = Skew(twist.segment<3>(3));
  Eigen::Matrix4d hatted_twist = Eigen::Matrix4d::Zero();
  hatted_twist.block<3, 3>(0, 0) = hatted_rot_velocity;
  hatted_twist.block<3, 1>(0, 3) = trans_velocity;
  return hatted_twist;
}

Eigen::Matrix<double, 6, 1> TwistUnhat(const Eigen::Matrix4d& hatted_twist)
{
   const Eigen::Vector3d trans_velocity = hatted_twist.block<3, 1>(0, 3);
   const Eigen::Vector3d rot_velocity = Unskew(hatted_twist.block<3, 3>(0, 0));
   Eigen::Matrix<double, 6, 1> twist;
   twist.segment<3>(0) = trans_velocity;
   twist.segment<3>(3) = rot_velocity;
   return twist;
}

Eigen::Matrix<double, 6, 6> AdjointFromTransform(
    const Eigen::Isometry3d& transform)
{
  const Eigen::Matrix3d rotation = transform.matrix().block<3, 3>(0, 0);
  const Eigen::Vector3d translation = transform.matrix().block<3, 1>(0, 3);
  const Eigen::Matrix3d translation_hat = Skew(translation);
  // Assemble the adjoint matrix
  Eigen::Matrix<double, 6, 6> adjoint;
  adjoint.block<3, 3>(0, 0) = rotation;
  adjoint.block<3, 3>(0, 3) = translation_hat * rotation;
  adjoint.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
  adjoint.block<3, 3>(3, 3) = rotation;
  return adjoint;
}

Eigen::Matrix<double, 6, 1> TransformTwist(
    const Eigen::Isometry3d& transform,
    const Eigen::Matrix<double, 6, 1>& initial_twist)
{
  return (Eigen::Matrix<double, 6, 1>)(AdjointFromTransform(transform)
                                       * initial_twist);
}

Eigen::Matrix<double, 6, 1> TwistBetweenTransforms(
    const Eigen::Isometry3d& start,
    const Eigen::Isometry3d& end)
{
  const Eigen::Isometry3d t_diff = start.inverse() * end;
  return TwistUnhat(t_diff.matrix().log());
}

Eigen::Matrix3d ExpMatrixExact(const Eigen::Matrix3d& hatted_rot_velocity,
                               const double delta_t)
{
  if (std::abs(Unskew(hatted_rot_velocity).norm() - 1.0) < 1e-10)
  {
    const Eigen::Matrix3d exp_matrix = Eigen::Matrix3d::Identity()
      + (hatted_rot_velocity * sin(delta_t))
      + (hatted_rot_velocity * hatted_rot_velocity * (1.0 - cos(delta_t)));
    return exp_matrix;
  }
  else
  {
    throw std::invalid_argument("Invalid hatted_rot_velocity: "
                                "std::abs(Unskew(hatted_rot_velocity).norm()"
                                " - 1.0) >= 1e-10");
  }
}

Eigen::Isometry3d ExpTwist(const Eigen::Matrix<double, 6, 1>& twist,
                           const double delta_t)
{
  const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
  const Eigen::Vector3d rot_velocity = twist.segment<3>(3);
  const double trans_velocity_norm = trans_velocity.norm();
  const double rot_velocity_norm = rot_velocity.norm();
  Eigen::Matrix4d raw_transform = Eigen::Matrix4d::Identity();
  if (rot_velocity_norm >= 1e-100)
  {
    const double scaled_delta_t = delta_t * rot_velocity_norm;
    const Eigen::Vector3d scaled_trans_velocity
        = trans_velocity / rot_velocity_norm;
    const Eigen::Vector3d scaled_rot_velocity
        = rot_velocity / rot_velocity_norm;
    const Eigen::Matrix3d rotation_displacement
        = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
    const Eigen::Vector3d translation_displacement
        = ((Eigen::Matrix3d::Identity() - rotation_displacement)
           * scaled_rot_velocity.cross(scaled_trans_velocity))
          + (scaled_rot_velocity * scaled_rot_velocity.transpose()
             * scaled_trans_velocity * scaled_delta_t);
    raw_transform.block<3, 3>(0, 0) = rotation_displacement;
    raw_transform.block<3, 1>(0, 3) = translation_displacement;
  }
  else
  {
    if ((trans_velocity_norm >= 1e-100) || (rot_velocity_norm == 0.0))
    {
      raw_transform.block<3, 1>(0, 3) = trans_velocity * delta_t;
    }
    else
    {
      std::cerr << "YOU MAY ENCOUNTER NUMERICAL INSTABILITY IN EXPTWIST(...)"
                   " WITH TRANS & ROT VELOCITY NORM < 1e-100 ***" << std::endl;
      const double scaled_delta_t = delta_t * rot_velocity_norm;
      const Eigen::Vector3d scaled_trans_velocity
          = trans_velocity / rot_velocity_norm;
      const Eigen::Vector3d scaled_rot_velocity
          = rot_velocity / rot_velocity_norm;
      const Eigen::Matrix3d rotation_displacement
          = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
      const Eigen::Vector3d translation_displacement
          = ((Eigen::Matrix3d::Identity() - rotation_displacement)
             * scaled_rot_velocity.cross(scaled_trans_velocity))
            + (scaled_rot_velocity * scaled_rot_velocity.transpose()
               * scaled_trans_velocity * scaled_delta_t);
      raw_transform.block<3, 3>(0, 0) = rotation_displacement;
      raw_transform.block<3, 1>(0, 3) = translation_displacement;
    }
  }
  Eigen::Isometry3d transform;
  transform = raw_transform;
  return transform;
}

double Interpolate(const double p1, const double p2, const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((p1 * (1.0 - real_ratio)) + (p2 * real_ratio));
}

double InterpolateContinuousRevolute(const double p1,
                                     const double p2,
                                     const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Safety check args
  const double real_p1 = EnforceContinuousRevoluteBounds(p1);
  const double real_p2 = EnforceContinuousRevoluteBounds(p2);
  // Interpolate
  double interpolated = 0.0;
  double diff = real_p2 - real_p1;
  if (std::abs(diff) <= M_PI)
  {
    interpolated = real_p1 + diff * real_ratio;
  }
  else
  {
    if (diff > 0.0)
    {
      diff = 2.0 * M_PI - diff;
    }
    else
    {
      diff = -2.0 * M_PI - diff;
    }
    interpolated = real_p1 - diff * real_ratio;
    // Input states are within bounds, so the following check is sufficient
    if (interpolated > M_PI)
    {
      interpolated -= 2.0 * M_PI;
    }
    else
    {
      if (interpolated < -M_PI)
      {
        interpolated += 2.0 * M_PI;
      }
    }
  }
  return interpolated;
}

std::vector<double> Interpolate(const std::vector<double>& v1,
                                const std::vector<double>& v2,
                                const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Safety check inputs
  const size_t len = v1.size();
  if (len != v2.size())
  {
    throw std::invalid_argument("Vectors v1 and v2 must be the same size");
  }
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  std::vector<double> interped(len, 0);
  for (size_t idx = 0; idx < len; idx++)
  {
    interped[idx] = ((v1[idx] * (1.0 - real_ratio)) + (v2[idx] * real_ratio));
  }
  return interped;
}

Eigen::Quaterniond Interpolate(const Eigen::Quaterniond& q1,
                               const Eigen::Quaterniond& q2,
                               const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  return q1.slerp(real_ratio, q2);
}

Eigen::VectorXd InterpolateXd(const Eigen::VectorXd& v1,
                              const Eigen::VectorXd& v2,
                              const double ratio)
{
  // Safety check sizes
  if (v1.size() != v2.size())
  {
    throw std::invalid_argument("Vectors v1 and v2 must be the same size");
  }
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

Eigen::Vector3d Interpolate3d(const Eigen::Vector3d& v1,
                              const Eigen::Vector3d& v2,
                              const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

Eigen::Vector4d Interpolate4d(const Eigen::Vector4d& v1,
                              const Eigen::Vector4d& v2,
                              const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  // This is the numerically stable version,
  // rather than  (p1 + (p2 - p1) * real_ratio)
  return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
}

Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& t1,
                              const Eigen::Isometry3d& t2,
                              const double ratio)
{
  // Safety check ratio
  const double real_ratio = utility::ClampValue(ratio, 0.0, 1.0);
  // Interpolate
  const Eigen::Vector3d v1 = t1.translation();
  const Eigen::Quaterniond q1(t1.rotation());
  const Eigen::Vector3d v2 = t2.translation();
  const Eigen::Quaterniond q2(t2.rotation());
  const Eigen::Vector3d vint = Interpolate3d(v1, v2, real_ratio);
  const Eigen::Quaterniond qint = Interpolate(q1, q2, real_ratio);
  const Eigen::Isometry3d tint = ((Eigen::Translation3d)vint) * qint;
  return tint;
}

double SquaredDistance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
{
  const double xd = v2.x() - v1.x();
  const double yd = v2.y() - v1.y();
  return ((xd * xd) + (yd * yd));
}

double Distance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
{
  return std::sqrt(SquaredDistance(v1, v2));
}

double SquaredDistance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
  const double xd = v2.x() - v1.x();
  const double yd = v2.y() - v1.y();
  const double zd = v2.z() - v1.z();
  return ((xd * xd) + (yd * yd) + (zd * zd));
}

double Distance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
{
  return std::sqrt(SquaredDistance(v1, v2));
}

double SquaredDistance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
{
  if (v1.size() == v2.size())
  {
    return (v2 - v1).squaredNorm();
  }
  else
  {
    throw std::invalid_argument("v1.size() != v2.size()");
  }
}

double Distance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
{
  return std::sqrt(SquaredDistance(v1, v2));
}

double Distance(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2)
{
  const double dq = std::abs((q1.w() * q2.w())
                             + (q1.x() * q2.x())
                             + (q1.y() * q2.y())
                             + (q1.z() * q2.z()));
  if (dq < (1.0 - std::numeric_limits<double>::epsilon()))
  {
    return std::acos(2.0 * (dq * dq) - 1.0);
  }
  else
  {
    return 0.0;
  }
}

double Distance(const Eigen::Isometry3d& t1,
                const Eigen::Isometry3d& t2,
                const double alpha)
{
  const double real_alpha = utility::ClampValue(alpha, 0.0, 1.0);
  const Eigen::Vector3d v1 = t1.translation();
  const Eigen::Quaterniond q1(t1.rotation());
  const Eigen::Vector3d v2 = t2.translation();
  const Eigen::Quaterniond q2(t2.rotation());
  const double vdist = Distance(v1, v2) * (1.0 - real_alpha);
  const double qdist = Distance(q1, q2) * (real_alpha);
  return vdist + qdist;
}

double SquaredDistance(const std::vector<double>& p1,
                       const std::vector<double>& p2)
{
  if (p1.size() == p2.size())
  {
    double distance = 0.0;
    for (size_t idx = 0; idx < p1.size(); idx++)
    {
      distance += (p2[idx] - p1[idx]) * (p2[idx] - p1[idx]);
    }
    return distance;
  }
  else
  {
    throw std::invalid_argument("p1.size() != p2.size()");
  }
}

double Distance(const std::vector<double>& p1, const std::vector<double>& p2)
{
  if (p1.size() == p2.size())
  {
    return std::sqrt(SquaredDistance(p1, p2));
  }
  else
  {
    throw std::invalid_argument("p1.size() != p2.size()");
  }
}

double ContinuousRevoluteSignedDistance(const double p1, const double p2)
{
  // Safety check args
  const double real_p1 = EnforceContinuousRevoluteBounds(p1);
  const double real_p2 = EnforceContinuousRevoluteBounds(p2);
  const double raw_distance = real_p2 - real_p1;
  if ((raw_distance <= -M_PI) || (raw_distance > M_PI))
  {
    if (raw_distance <= -M_PI)
    {
      return (-(2.0 * M_PI) - raw_distance);
    }
    else if (raw_distance > M_PI)
    {
      return ((2.0 * M_PI) - raw_distance);
    }
    else
    {
      return raw_distance;
    }
  }
  else
  {
    return raw_distance;
  }
}

double ContinuousRevoluteDistance(const double p1, const double p2)
{
  return std::abs(ContinuousRevoluteSignedDistance(p1, p2));
}

double AddContinuousRevoluteValues(const double start, const double change)
{
  return EnforceContinuousRevoluteBounds(start + change);
}

double GetContinuousRevoluteRange(const double start, const double end)
{
  const double raw_range = ContinuousRevoluteSignedDistance(start, end);
  if (raw_range >= 0.0)
  {
    return raw_range;
  }
  else
  {
    return (2.0 * M_PI) + raw_range;
  }
}

bool CheckInContinuousRevoluteRange(const double start,
                                    const double range,
                                    const double val)
{
  const double real_val = EnforceContinuousRevoluteBounds(val);
  const double real_start = EnforceContinuousRevoluteBounds(start);
  const double delta = ContinuousRevoluteSignedDistance(real_start, real_val);
  if (delta >= 0.0)
  {
    if (delta <= range)
    {
      return true;
    }
    else
    {
      return false;
    }
  }
  else
  {
    const double real_delta = (2.0 * M_PI) + delta;
    if (real_delta <= range)
    {
      return true;
    }
    else
    {
      return false;
    }
  }
}

bool CheckInContinuousRevoluteBounds(const double start,
                                     const double end,
                                     const double val)
{
  const double range = GetContinuousRevoluteRange(start, end);
  return CheckInContinuousRevoluteRange(start, range, val);
}

double AverageStdVectorDouble(const std::vector<double>& values,
                              const std::vector<double>& weights)
{
  // Get the weights
  if (values.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  if ((weights.size() != values.size()) && (weights.size() != 0))
  {
    throw std::invalid_argument("Provided weights must be empty"
                                " or same size to provided vector");
  }
  const bool use_weights = (weights.size() != 0);
  // Find the first element with non-zero weight
  size_t starting_idx = 0;
  while (starting_idx < weights.size()
         && std::abs(weights[starting_idx]) == 0.0)
  {
    starting_idx++;
  }
  // If all weights are zero, result is undefined
  if (starting_idx >= values.size())
  {
    throw std::invalid_argument("Provided weights are all zero");
  }
  // Start the recursive definition with the base case
  double average = values[starting_idx];
  const double starting_weight = use_weights ? std::abs(weights[starting_idx])
                                             : 1.0;
  double weights_running_sum = starting_weight;
  // Do the weighted averaging on the rest of the vectors
  for (size_t idx = starting_idx + 1; idx < values.size(); idx++)
  {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    weights_running_sum += weight;
    const double effective_weight = weight / weights_running_sum;
    const double prev_average = average;
    const double current = values[idx];
    average = prev_average + (effective_weight * (current - prev_average));
  }
  return average;
}

double ComputeStdDevStdVectorDouble(const std::vector<double>& values,
                                    const double mean)
{
  if (values.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  else if (values.size() == 1)
  {
    return 0.0;
  }
  else
  {
    const double inv_n_minus_1 = 1.0 / (double)(values.size() - 1);
    double stddev_sum = 0.0;
    for (size_t idx = 0; idx < values.size(); idx++)
    {
      const double delta = values[idx] - mean;
      stddev_sum += (delta * delta);
    }
    return std::sqrt(stddev_sum * inv_n_minus_1);
  }
}

double ComputeStdDevStdVectorDouble(const std::vector<double>& values)
{
  const double mean = AverageStdVectorDouble(values);
  return ComputeStdDevStdVectorDouble(values, mean);
}

double AverageContinuousRevolute(const std::vector<double>& angles,
                                 const std::vector<double>& weights)
{
  return AverageStdVectorDouble(angles, weights);
}

Eigen::Vector3d AverageEigenVector3d(const VectorVector3d& vectors,
                                     const std::vector<double>& weights)
{
  return AverageEigenVector(vectors, weights);
}

Eigen::VectorXd AverageEigenVectorXd(
    const std::vector<Eigen::VectorXd>& vectors,
    const std::vector<double>& weights)
{
  return AverageEigenVector(vectors, weights);
}

// Implementation of method described in (http://stackoverflow.com/a/27410865)
// See paper at (http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf) for more
Eigen::Quaterniond AverageEigenQuaterniond(const VectorQuaterniond& quaternions,
                                           const std::vector<double>& weights)
{
  // Get the weights
  const bool use_weights = weights.size() == quaternions.size() ? true : false;
  if (quaternions.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  if ((weights.size() != quaternions.size()) && (weights.size() != 0))
  {
    throw std::invalid_argument("Provided weights must be empty"
                                " or same size to provided vector");
  }
  // Shortcut the process if there is only 1 quaternion
  if (quaternions.size() == 1)
  {
    if (weights.size() > 0)
    {
      if (std::abs(weights[0]) == 0.0)
      {
        throw std::invalid_argument("Single quaternion with zero weight");
      }
    }
    return quaternions[0];
  }
  // Build the averaging matrix
  Eigen::MatrixXd q_matrix(4, quaternions.size());
  for (size_t idx = 0; idx < quaternions.size(); idx++)
  {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    const Eigen::Quaterniond& q = quaternions[idx];
    q_matrix.col((ssize_t)idx) << weight * q.w(),
                                  weight * q.x(),
                                  weight * q.y(),
                                  weight * q.z();
  }
  // Make the matrix square
  const Eigen::Matrix<double, 4, 4> qqtranspose_matrix
      = q_matrix * q_matrix.transpose();
  // Compute the eigenvectors and eigenvalues of the qqtranspose matrix
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>
      solver(qqtranspose_matrix);
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType
      eigen_values = solver.eigenvalues();
  const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
      eigen_vectors = solver.eigenvectors();
  // Extract the eigenvector corresponding to the largest eigenvalue
  double max_eigenvalue = -std::numeric_limits<double>::infinity();
  int64_t max_eigenvector_index = -1;
  for (size_t idx = 0; idx < 4; idx++)
  {
    const double current_eigenvalue = eigen_values((long)idx).real();
    if (current_eigenvalue > max_eigenvalue)
    {
      max_eigenvalue = current_eigenvalue;
      max_eigenvector_index = (int64_t)idx;
    }
  }
  if (max_eigenvector_index < 0)
  {
    throw std::runtime_error("Failed to find max eigenvector");
  }
  // Note that these are already normalized!
  const Eigen::Vector4cd best_eigenvector
      = eigen_vectors.col((long)max_eigenvector_index);
  // Convert back into a quaternion
  const Eigen::Quaterniond average_q(best_eigenvector(0).real(),
                                     best_eigenvector(1).real(),
                                     best_eigenvector(2).real(),
                                     best_eigenvector(3).real());
  return average_q;
}

Eigen::Isometry3d AverageEigenIsometry3d(const VectorIsometry3d& transforms,
                                         const std::vector<double>& weights)
{
  if (transforms.empty())
  {
    throw std::invalid_argument("Provided vector is empty");
  }
  if ((weights.size() != transforms.size()) && (weights.size() != 0))
  {
    throw std::invalid_argument("Provided weights must be empty"
                                " or same size to provided vector");
  }
  // Shortcut the process if there is only 1 transform
  if (transforms.size() == 1)
  {
    if (weights.size() > 0)
    {
      if (std::abs(weights[0]) == 0.0)
      {
        throw std::invalid_argument("Single transform with zero weight");
      }
    }
    return transforms[0];
  }
  // Extract components
  VectorVector3d translations(transforms.size());
  VectorQuaterniond rotations(transforms.size());
  for (size_t idx = 0; idx < transforms.size(); idx++)
  {
    translations[idx] = transforms[idx].translation();
    rotations[idx] = Eigen::Quaterniond(transforms[idx].rotation());
  }
  // Average
  const Eigen::Vector3d average_translation
      = AverageEigenVector(translations, weights);
  const Eigen::Quaterniond average_rotation
      = AverageEigenQuaterniond(rotations, weights);
  // Make the average transform
  const Eigen::Isometry3d average_transform
      = (Eigen::Translation3d)average_translation * average_rotation;
  return average_transform;
}

double WeightedDotProduct(const Eigen::VectorXd& vec1,
                          const Eigen::VectorXd& vec2,
                          const Eigen::VectorXd& weights)
{
  return vec1.cwiseProduct(weights).dot(vec2);
}

double WeightedSquaredNorm(const Eigen::VectorXd& vec,
                           const Eigen::VectorXd weights)
{
  return WeightedDotProduct(vec, vec, weights);
}

double WeightedNorm(const Eigen::VectorXd& vec, const Eigen::VectorXd& weights)
{
  return std::sqrt(WeightedSquaredNorm(vec, weights));
}

double WeightedCosineAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                         const Eigen::VectorXd& vec2,
                                         const Eigen::VectorXd& weights)
{
  const double vec1_norm = WeightedNorm(vec1, weights);
  const double vec2_norm = WeightedNorm(vec2, weights);
  if (vec1_norm > 0 && vec2_norm > 0)
  {
    const double result
        = WeightedDotProduct(vec1, vec2, weights) / (vec1_norm * vec2_norm);
    return std::max(-1.0, std::min(result, 1.0));;
  }
  else
  {
    throw std::invalid_argument("One or more input vectors has zero norm");
  }
}

double WeightedAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                   const Eigen::VectorXd& vec2,
                                   const Eigen::VectorXd& weights)
{
  return std::acos(WeightedCosineAngleBetweenVectors(vec1, vec2, weights));
}
}  // namespace math
}  // namespace common_robotics_utilities


#include "simulate.h"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <random>
#include <string>
#include <vector>
#include <tuple>

#include <armadillo>
#include <boost/math/distributions/normal.hpp>

using namespace std;

// constants
const double ALPHA = 0.05;

int main(int argc, char *argv[]) {
  clock_t startTime = clock();
  ios::sync_with_stdio(false); cin.tie(NULL);
  // parse arguments
  int ITERATIONS = stoi(argv[1]); // simulation runs
  int N = stoi(argv[2]);          // 1/2 sample size
  double POWER = stod(argv[3]);    // power

  // calculate variance used for generators
  double variance = calculateVariance(N, ALPHA, POWER);
  double standardDeviation = sqrt(variance);

  // set up generators
  random_device randomDevice;
  mt19937_64 rng;
  rng.seed(randomDevice());
  normal_distribution<double> normalMean0(0, standardDeviation);
  normal_distribution<double> normalMean1(1, standardDeviation);

  // store and print results
  vector<tuple<double, double, int, int, double, double>> results(ITERATIONS); // initial p-value, "cheated" beta, set size, subset size, balance p-value, "cheated" p-value
  cout << setprecision(12) << fixed;
  cout << "2N\tpower\tp.value\tbeta\tset.size\tsubset.size\tbalance.p.value\tfake.p.value" << endl;
  // randomly assign treatment
  vector<int> X(2*N);
  for (int i = 0; i < N; ++i) X[i] = 1;
  shuffle(X.begin() , X.end(), rng);
  arma::mat Z(2*N, 2*N, arma::fill::zeros); // pre allocate Z matrix
  for (int i = 0; i < ITERATIONS; ++i) {
    // generate response
    arma::Col<double> Y(2*N);
    for (int j = 0; j < 2*N; ++j) {
      Y(j) = X[j] == 0 ? normalMean0(rng) : normalMean1(rng);
    }
    results[i] = simulate(Y, X, standardDeviation, true, Z, rng);
    cout << 2*N << '\t' << POWER << '\t'
         << get<0>(results[i]) << '\t'
         << get<1>(results[i]) << '\t'
         << get<2>(results[i]) << '\t'
         << get<3>(results[i]) << '\t'
         << get<4>(results[i]) << '\t'
         << get<5>(results[i]) << endl;
  }

  double duration = (clock() - startTime) / (double) CLOCKS_PER_SEC;
  cerr << "Time taken (seconds): " << duration << endl;
  return 0;
}


// requires LAPACK v. 3.0

#include <cstddef>
#include <iostream>
#include <algorithm>
#include <boost/numeric/bindings/lapack/driver/gesdd.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "utils.h"

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

using std::size_t;
using std::cout;
using std::endl;

typedef double real_t;
typedef ublas::matrix<real_t, ublas::column_major> m_t;
typedef ublas::vector<real_t> v_t;

int main() {

  cout << endl;

  size_t m = 3, n = 4;
  size_t minmn = m < n ? m : n;
  m_t a (m, n);
  a(0,0) = 2.;  a(0,1) = 2.;  a(0,2) = 2.;   a(0,3) = 2.;
  a(1,0) = 1.7; a(1,1) = 0.1; a(1,2) = -1.7; a(1,3) = -0.1;
  a(2,0) = 0.6; a(2,1) = 1.8; a(2,2) = -0.6; a(2,3) = -1.8;

  m_t a2 (a); // for parts 2 & 3

  print_m (a, "A");
  cout << endl;

  v_t s (minmn);
  m_t u (m, minmn);
  m_t vt (minmn, n);

//  lapack::gesdd (a, s, u, vt);
  lapack::gesdd ('S', a, s, u, vt);

  print_v (s, "s");
  cout << endl;
  print_m (u, "U");
  cout << endl;
  print_m (vt, "V^T");
  cout << endl << endl;

  // part 2

  // singular values and singular vectors satisfy  A v_i == s_i u_i
  for (int i = 0; i < minmn; ++i) {
    cout << "A v_" << i << " == s_" << i << " u_" << i << endl;
    v_t avi = ublas::prod (a2, ublas::row (vt, i));
    print_v (avi);
    v_t siui = s[i] * ublas::column (u, i);
    print_v (siui);
    cout << endl;
  }
  cout << endl;

  // singular values and singular vectors satisfy  A^T u_i == s_i v_i
  for (int i = 0; i < minmn; ++i) {
    cout << "A^T u_" << i << " == s_" << i << " v_" << i << endl;
    v_t atui = ublas::prod (trans (a2), ublas::column (u, i));
    print_v (atui);
    v_t sivi = s[i] * ublas::row (vt, i);
    print_v (sivi);
    cout << endl;
  }
  cout << endl;

  // part 3

//  lapack::gesdd (a2, s);
  lapack::gesdd ('N', a2, s, u, vt);

  print_v (s, "singular values only");
  cout << endl;
}


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/ModelCoefficients.h>
#include <vector>

#include <time.h>

#define PI 3.1415926

class Point
{
    public:
    double x;
    double y;
    double z;
    Point(double v[]):x(v[0]),y(v[1]),z(v[2]){}
};

//求法向量
double* get_normal(Point p1, Point p2, Point p3)
{
    double* norm_vec;
    norm_vec = new double[3];
    double a = ( (p2.y-p1.y)*(p3.z-p1.z)-(p2.z-p1.z)*(p3.y-p1.y));

    double b = ( (p2.z-p1.z)*(p3.x-p1.x)-(p2.x-p1.x)*(p3.z-p1.z));

    double c = ( (p2.x-p1.x)*(p3.y-p1.y)-(p2.y-p1.y)*(p3.x-p1.x));

    double norm = std::sqrt(a*a+b*b+c*c);

    norm_vec[0] = a/norm;
    norm_vec[1] = b/norm;
    norm_vec[2] = c/norm;
    // double v[3] = {a/norm, b/norm, c/norm};

    return norm_vec;
}

//求旋转矩阵4*4

double get_theta_from_2_vec(double*v1,double*v2)
{
    double costheta = (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) / (0.000001+std::sqrt((v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2])*(v2[0]*v2[0]+v2[1]*v2[1]+v2[2]*v2[2])));
    return std::acos(costheta);
}

Eigen::Matrix4d get_mat(Eigen::VectorXd coed)
{
    double z[3] = {0,0,1};
    // double a[3] = {-0.0554246, 0.0372348, 0.997768};
    // double a[3] = {-0.0842539, 0.882628, 0.462459};
    double a[3] = {coed[0], coed[1], coed[2]};

    std::cout<<"normal vector: "<<coed[0]<<", "<<coed[1]<<", "<<coed[2]<<std::endl;
    // double za = z[0]*a[0] + z[1]*a[1] + z[2]*a[2];

    double o[3] = {0,0,0};
    Point p1(z);
    Point p2(a);
    Point p3(o);
    double* axis_line = get_normal(p1, p2, p3);
    std::cout<<axis_line[1] << std::endl;

    // double a_xOy[3] = {a[0], a[1],0};
    // double x_vec[3] = {1,0,0};
    // double thetaxOz= get_theta_from_2_vec(a_xOy,x_vec);
    // //Eigen::Quaterniond q_rotate_to_xOz (std::cos(thetaxOz/2),std::sin(thetaxOz/2),0,0);
    // Eigen::Quaterniond q_rotate_to_xOz (std::cos(thetaxOz/2),0,0,std::sin(thetaxOz/2));

    // double a_xOy_to_z[3] = {a[0],0, a[2]};
    // double z_vec[3] = {0,0,1};
    // double theta_toz = 1.0*get_theta_from_2_vec(a_xOy_to_z, z_vec);
    // Eigen::Quaterniond q_rotate_to_z(std::cos(theta_toz/2),0,std::sin(theta_toz/2),0);

    // double a_z0y_to_x[3] = {0, a[1], a[2]};
    // double y_vec[3] = {0,1,0};
    // double theta_yox = get_theta_from_2_vec(a_z0y_to_x,y_vec);
    // Eigen::Quaterniond q_rotate_to_x(std::cos(theta_yox/2),std::sin(theta_yox/2),0,0);

    //四元数{cos(r/2), nx*sin(r/2), ny*sin(r/2), nz*sin(r/2)}
    //Eigen::Quaterniond q = q_rotate_to_xOz*q_rotate_to_z;
    //Eigen::Quaterniond q = q_rotate_to_xOz*q_rotate_to_z*q_rotate_to_x;

    double theta_all = -1.0*get_theta_from_2_vec(a, z);
    Eigen::Quaterniond q(std::cos(theta_all/2),axis_line[0]*std::sin(theta_all/2),axis_line[1]*std::sin(theta_all/2),axis_line[2]*std::sin(theta_all/2));
    // Eigen::Quaterniond q(std::cos(theta_all/2),-0.557652*std::sin(theta_all/2),-0.830075*std::sin(theta_all/2),0);

    // Eigen::Quaterniond q = q_rotate_to_xOz;
    Eigen::Matrix3d mat3 = q.toRotationMatrix();
    Eigen::Matrix4d mat4 = Eigen::Matrix4d::Identity();
    mat4.block(0,0,3,3) = mat3;
    mat4(2, 3) = coed[3];

    std::cout << mat3 << std::endl;

    return mat4;
}

void
showHelp ()
{
  std::cout << std::endl;
  std::cout << "Usage: ./rotate_pcd input.pcd [Options]"<< std::endl;
  std::cout << "Options:" << std::endl;
  std::cout << "     -h:                        Show this help." << std::endl;
  std::cout << "     -v:                        Show pcl viewer." << std::endl;
  std::cout << "     -o  output.pcd:            Save inlier points to pcd file." << std::endl;
  std::cout << "     -t  threshold:             Distance threshold (default 0.1)" << std::endl;
  std::cout << "     -cs coord_scale:           Coordinate Scale of pcl viewer (default 100.0)" << std::endl;
}

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

    std::clock_t t1 = std::clock();
    //creates a PointCloud<PointXYZ> boost shared pointer and initializes it.
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

    if (pcl::console::find_switch (argc, argv, "-h"))
    {
        showHelp ();
        exit (0);
    }
    std::string input_path;
    if (argc < 2)
    {
        showHelp ();
        exit (0);
    }
    else
    {
        input_path = argv[1];
        if (pcl::io::loadPCDFile<pcl::PointXYZ> (input_path, *source_cloud) == -1) //* load the file
        {
            std::cerr << "Input path: " << input_path << std::endl;
            PCL_ERROR ("Couldn't read file model, please try again. \n");
            showHelp();
            return (-1);
        }
    }

    std::size_t sz = source_cloud->points.size();
    std::cout<<"Points size: "<<sz<<std::endl;

    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (source_cloud));
    Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1);
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);

    double threshold;
    if (pcl::console::parse_argument (argc, argv, "-t", threshold) != -1)
    {
      std::cout<<"Distance threshold: "<<threshold<<std::endl;
    }
    else
    {
      threshold = 0.1;
      std::cout<<"Distance threshold: 0.1"<<std::endl;
    }

    ransac.setDistanceThreshold (threshold);
    ransac.computeModel();
    // ransac.getInliers(inliers);
    ransac.getModelCoefficients(coef);

    Eigen::VectorXd coed = coef.cast<double>();

    std::clock_t t2 = std::clock();

    Eigen::Matrix4d transform_1_d = get_mat(coed);

    // std::cout << transform_1_d << std::endl;

    // Executing the transformation
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    /*
    void pcl::transformPointCloud(const pcl::PointCloud< PointT > & cloud_in,
                                    pcl::PointCloud< PointT > &  cloud_out,
                                    const Eigen::Matrix4f &  transform  )
    */
    // Apply an affine transform defined by an Eigen Transform.

    Eigen::Matrix4f transform_1_f = transform_1_d.cast<float>();

    pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_1_f);

    std::clock_t t3 = std::clock();
    // pcl::io::savePCDFileASCII("model_1_transform.pcd", *transformed_cloud);
    std::string output_path;
    if (pcl::console::parse_argument (argc, argv, "-o", output_path) != -1)
    {
        std::cout << "Rotated model will saved at: "<<output_path<<std::endl;

    }
    else
    {
        output_path = "transform.pcd";
        std::cout << "Rotated model will saved at: transform.pcd" <<std::endl;
    }
    if (pcl::io::savePCDFileASCII(output_path, *transformed_cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't save file model, please try again. \n");
        return (-1);
    }

    std::clock_t t4 = std::clock();

    std::cout <<"Full cost: "<<(double)(t4-t1)/CLOCKS_PER_SEC<<std::endl;

    std::cout <<"RANSaC cost: "<<(double)(t2-t1)/CLOCKS_PER_SEC<<std::endl;
    std::cout <<"Transform cost: "<<(double)(t3-t1)/CLOCKS_PER_SEC<<std::endl;
    // Visualization
    if (pcl::console::find_switch (argc, argv, "-v"))
    {
        printf(  "\nPoint cloud colors :  white  = original point cloud\n"
        "                        red  = transformed point cloud\n");
        pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

        // Define R,G,B colors for the point cloud
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (source_cloud, 255, 255, 255);
        //We add the point cloud to the viewer and pass the color handler
        viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
        viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

        double coord_scale;
        if (pcl::console::parse_argument (argc, argv, "-cs", coord_scale) != -1)
        {
            std::cout<<"Display coordinate scale: "<<coord_scale<<std::endl;
        }
        else
        {
            coord_scale = 100.0;
            std::cout<<"Display coordinate scale: 100.0"<<std::endl;
        }
        viewer.addCoordinateSystem (coord_scale, 0);  //Adds 3D axes describing a coordinate system to screen at 0,0,0.
        viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
        viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");

        viewer.setPosition(800, 400); // Setting visualiser window position

        while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
        viewer.spinOnce ();
        }
    }

  return 0;
}