// Standard
#include <math.h>
#include <stdio.h>
#include <iostream>

// Timer
#include <chrono>

#include <ExternalSource/myOptimizer/Goldfarb/QuadProg++.hh>

#include <Eigen/Dense>
#include <eigen3/unsupported/Eigen/MatrixFunctions>
#include <vector>

// #define MPC_PRINT_ALL
// #define MPC_TIME_ALL

// We are following the MPC formulation from:
// Di Carlo, Jared, et al. "Dynamic locomotion in the mit cheetah 3 through
// convex model-predictive control." 2018 IEEE/RSJ International Conference on
// Intelligent Robots and Systems (IROS). IEEE, 2018.

inline Eigen::MatrixXd R_roll(const double& phi) {
    Eigen::MatrixXd Rx(3, 3);
    Rx.setZero();
    Rx << 1.0, 0.0, 0.0, 0.0, cos(phi), -sin(phi), 0.0, sin(phi), cos(phi);
    return Rx;
}

inline Eigen::MatrixXd R_pitch(const double& theta) {
    Eigen::MatrixXd Ry(3, 3);
    Ry.setZero();
    Ry << cos(theta), 0.0, sin(theta), 0.0, 1.0, 0.0, -sin(theta), 0.0,
        cos(theta);
    return Ry;
}

inline Eigen::MatrixXd R_yaw(const double& psi) {
    Eigen::MatrixXd Rz(3, 3);
    Rz.setZero();
    Rz << cos(psi), -sin(psi), 0.0, sin(psi), cos(psi), 0.0, 0.0, 0.0, 1.0;
    return Rz;
}

inline Eigen::MatrixXd skew_sym_mat(const Eigen::MatrixXd& r) {
    Eigen::MatrixXd cm(3, 3);
    cm << 0.f, -r(2), r(1), r(2), 0.f, -r(0), -r(1), r(0), 0.f;
    return cm;
}

// Transform input vector to nxm matrix. Vector must have dimension n*m.
void assemble_vec_to_matrix(const int& n, const int& m,
                            const Eigen::VectorXd& vec,
                            Eigen::MatrixXd& mat_out) {
    mat_out = Eigen::MatrixXd::Zero(n, m);
    for (int j = 0; j < m; j++) {
        mat_out.col(j) = vec.segment(j * n, n);
    }
    // std::cout << "mat_out:" << std::endl;
    // std::cout << mat_out << std::endl;
}

// Given dt, current x state, f_Mat, and r_feet. Find the next state, x1
void integrate_robot_dynamics(const double& dt,
                              const Eigen::VectorXd& x_current,
                              const Eigen::MatrixXd& f_Mat,
                              const Eigen::MatrixXd& r_feet, const double& mass,
                              const Eigen::MatrixXd& I_body,
                              Eigen::VectorXd& x_next) {
    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    double roll = x_current[0];
    double pitch = x_current[1];
    double yaw = x_current[2];
    // std::cout << "x_current:" << x_current.transpose() << std::endl;

    Eigen::VectorXd theta_prior = x_current.head(3);
    Eigen::VectorXd p_prior = x_current.segment(3, 3);
    Eigen::VectorXd omega_prior = x_current.segment(6, 3);
    Eigen::VectorXd pdot_prior = x_current.segment(9, 3);

    Eigen::MatrixXd Rx = R_roll(roll);
    Eigen::MatrixXd Ry = R_pitch(pitch);
    Eigen::MatrixXd Rz = R_yaw(yaw);

    Eigen::MatrixXd R_zyx = Rz * Ry * Rx;
    Eigen::MatrixXd I_world =
        R_zyx * I_body *
        R_zyx.transpose();  // Inertia matrix aligned to CoM frame.

    // CoM acceleration, velocity, and position
    Eigen::VectorXd pddot(3);  // pddot = 1/m sum(f_i) - g
    Eigen::VectorXd pdot(3);   // pdot = pddot*dt + pdot prior;
    Eigen::VectorXd p(3);      // p = 0.5*pddot*dt*dt + pdot_prior*dt + p_prior;

    Eigen::VectorXd grav_vec(3);
    grav_vec.setZero();
    grav_vec[2] = -9.81;
    pddot.setZero();
    pddot += grav_vec;
    for (int i = 0; i < f_Mat.cols(); i++) {
        pddot += ((1.0 / mass) * f_Mat.col(i));
    }
    pdot = pddot * dt + pdot_prior;                         // CoM velocity
    p = 0.5 * pddot * dt * dt + pdot_prior * dt + p_prior;  // CoM position

    // Angular Acceleration, velocity, and new orientation
    Eigen::VectorXd omega_dot(3);  // omega_dot = I_inv * (sum(skew(r_i)*f_i)
                                   // -skew(omega_prior)*I*omega_prior
    Eigen::VectorXd omega(3);      // omega = omega_dot*dt + omega_prior;
    Eigen::VectorXd theta;         // theta = Omega_Rate*(0.5*omega_dot*dt +
                                   // omega_prior)*dt + theta_prior

    // ZYX from ZYX rates integration
    Eigen::MatrixXd omega_to_zyx_rate(3, 3);
    omega_to_zyx_rate << cos(yaw) / cos(pitch), sin(yaw) / cos(pitch), 0,
        -sin(yaw), cos(yaw), 0, cos(yaw) * sin(pitch) / cos(pitch),
        sin(yaw) * sin(pitch) / cos(pitch), 1;

    Eigen::VectorXd moments(3);
    moments.setZero();
    for (int i = 0; i < f_Mat.cols(); i++) {
        moments += skew_sym_mat(r_feet.col(i)) * f_Mat.col(i);
    }
    omega_dot = I_world.inverse() *
                (moments - skew_sym_mat(omega_prior) * (I_world * omega_prior));
    omega = omega_dot * dt + omega_prior;
    theta = omega_to_zyx_rate * (0.5 * omega_dot * dt + omega_prior) * dt +
            theta_prior;

    // std::cout << "pddot:" << pddot.transpose() << std::endl;
    // std::cout << "pdot:" << pdot.transpose() << std::endl;
    // std::cout << "p_prior:" << p_prior.transpose() << std::endl;
    // std::cout << "p_post:" <<  p.transpose() << std::endl;

    // std::cout << std::endl;
    // std::cout << "omega_dot:" << omega_dot.transpose() << std::endl;
    // std::cout << "omega:" << omega.transpose() << std::endl;

    // std::cout << "omega_to_zyx_rate" << std::endl;
    // std::cout << omega_to_zyx_rate << std::endl;

    // std::cout << "theta_prior:" << theta_prior.transpose() << std::endl;
    // std::cout << "theta_post:" << theta.transpose() << std::endl;

    // Set new x_next;
    x_next.head(3) = theta;
    x_next.segment(3, 3) = p;
    x_next.segment(6, 3) = omega;
    x_next.segment(9, 3) = pdot;
    x_next[12] = -9.81;

    // std::cout << "x_next:" << x_next.transpose() << std::endl;
}

// Inputs: m, I, current yaw, and footstep locations.
// Output: A, B matrix to be used for zero-order hold MPC
void cont_time_state_space(const double& mass, const Eigen::MatrixXd& I_body,
                           const double& psi_in, const Eigen::MatrixXd& r_feet,
                           Eigen::MatrixXd& A, Eigen::MatrixXd& B) {
    // Get System State: Current Robot yaw
    Eigen::MatrixXd Rz = R_yaw(psi_in);

    Eigen::MatrixXd I_world =
        Rz * I_body * Rz.transpose();  // Inertia matrix aligned to CoM frame.
    Eigen::MatrixXd I_inv = I_world.inverse();  // Inverse

    // Number of reaction forces equal to number of feet contacts

    int n_Fr = r_feet.cols();

    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    // There is an additional gravity state.
    // Populate A matrix
    A = Eigen::MatrixXd::Zero(13, 13);
    A.block(0, 6, 3, 3) = Rz;
    A.block(3, 9, 3, 3) = Eigen::MatrixXd::Identity(3, 3);
    A(11, 12) = 1.0;  // Gravity vector

    // Populate B matrix
    B = Eigen::MatrixXd::Zero(13, 3 * n_Fr);
    Eigen::MatrixXd eye3 = Eigen::MatrixXd::Identity(3, 3);
    for (int i = 0; i < n_Fr; i++) {
        B.block(6, i * 3, 3, 3) = I_inv * skew_sym_mat(r_feet.col(i));
        B.block(9, i * 3, 3, 3) = eye3 * 1.0 / mass;
    }

#ifdef MPC_PRINT_ALL
    std::cout << "Rz:" << std::endl;
    std::cout << Rz << std::endl;

    std::cout << "I_body:" << std::endl;
    std::cout << I_body << std::endl;

    std::cout << "I_world:" << std::endl;
    std::cout << I_world << std::endl;

    std::cout << "I_inv:" << std::endl;
    std::cout << I_inv << std::endl;

    std::cout << "r_feet:" << std::endl;
    std::cout << r_feet << std::endl;

    std::cout << "A:" << std::endl;
    std::cout << A << std::endl;

    std::cout << "B:" << std::endl;
    std::cout << B << std::endl;

    std::cout << "A.exp()" << std::endl;
    std::cout << A.exp() << std::endl;
#endif
}

// Returns the discretized matrices for a given A and B matrix.
void discrete_time_state_space(const double& dt, const Eigen::MatrixXd& A,
                               const Eigen::MatrixXd& B, Eigen::MatrixXd& Adt,
                               Eigen::MatrixXd& Bdt) {
    // p.215 Raymond DeCarlo: Linear Systems: A State Variable Approach with
    // Numerical Implementation, Prentice Hall, NJ, 1989
    int n = A.cols() + B.cols();
    Eigen::MatrixXd AB(n, n);
    Eigen::MatrixXd expAB(n, n);
    AB.setZero();
    AB.block(0, 0, A.cols(), A.cols()) = A;
    AB.block(0, A.cols(), A.cols(), B.cols()) = B;

    expAB = (AB * dt).exp();

    Adt = expAB.block(0, 0, A.cols(), A.cols());
    Bdt = expAB.block(0, A.cols(), A.cols(), B.cols());

#ifdef MPC_PRINT_ALL
    std::cout << "Adt" << std::endl;
    std::cout << Adt << std::endl;

    std::cout << "Bdt" << std::endl;
    std::cout << Bdt * dt << std::endl;
#endif
}

// Zero order hold qp matrices
void qp_matrices(const int& horizon, const Eigen::MatrixXd& Adt,
                 const Eigen::MatrixXd& Bdt, Eigen::MatrixXd& Aqp,
                 Eigen::MatrixXd& Bqp) {
    if (horizon < 1) {
        std::cerr << "Error! MPC horizon must be at least 1 timestep long."
                  << std::endl;
    }

    int n = Adt.cols();
    int m = Bdt.cols();
    std::vector<Eigen::MatrixXd> powerMats;
    for (int i = 0; i < horizon + 1; i++) {
        powerMats.push_back(Eigen::MatrixXd::Identity(n, n));
    }

    // Construct power matrices foe Adt
    for (int i = 1; i < horizon + 1; i++) {
        powerMats[i] = powerMats[i - 1] * Adt;
    }

    // Construct Aqp and Bqp matrices
    Aqp.setZero();
    Bqp.setZero();

    for (int i = 0; i < horizon; i++) {
        Aqp.block(i * n, 0, n, n) = powerMats[i + 1];  // Adt.pow(i+1);
        for (int j = 0; j < horizon; j++) {
            if (i >= j) {
                int a_num = i - j;
                Bqp.block(i * n, j * m, n, m) =
                    powerMats[a_num] * Bdt;  // Adt.pow(a_num)*Bdt;
            }
        }
    }

    // Print power matrices for Adt
    // for(int i = 0; i < horizon; i++){
    // 	std::cout << "powerMats[" << i << "]" << std::endl;
    // 	std::cout << powerMats[i] << std::endl;
    // }

#ifdef MPC_PRINT_ALL
    std::cout << "Aqp" << std::endl;
    std::cout << Aqp << std::endl;

    std::cout << "Bqp" << std::endl;
    std::cout << Bqp << std::endl;
#endif
}

void get_desired_x(const int& horizon, Eigen::VectorXd& X_ref) {
    Eigen::VectorXd x_des(13);

    x_des.setZero();
    x_des[0] = 0.0;        // M_PI/8; //des roll orientation
    x_des[1] = 0.0;        //-M_PI/8; //des pitch orientation
    x_des[2] = M_PI / 12;  // Yaw orientation

    x_des[3] =
        -0.1;  //;0.75; // Set desired z height to be 0.75m from the ground
    x_des[5] =
        0.75;  //;0.75; // Set desired z height to be 0.75m from the ground

    X_ref = Eigen::VectorXd(horizon * 13);
    X_ref.setZero();
    for (int i = 0; i < horizon; i++) {
        X_ref.segment(13 * i, 13) = x_des;
        // X_ref[13*i + 5] = x_des[5]; // Set constant desired z-height
    }

#ifdef MPC_PRINT_ALL
    std::cout << "X_ref " << std::endl;
    std::cout << X_ref.transpose() << std::endl;
#endif
}

void get_force_constraints(const int& n_Fr, Eigen::MatrixXd& CMat,
                           Eigen::VectorXd& cvec) {
    // Unilateral constraints on the force vector
    // CMat * u + cvec >= 0
    double mu = 0.9;      // coefficient of friction
    double fz_max = 500;  // maximum z reaction force for one force vector.

    Eigen::MatrixXd cfmat(
        6, 3);  // the constraint matrix for a single force vector
    Eigen::VectorXd cfvec(
        6);  // the constraint vector for a single force vector

    cfmat.setZero();
    cfvec.setZero();

    // -mu*fz <= fx <= mu*fz
    cfmat(0, 0) = -1;
    cfmat(0, 2) = mu;
    cfmat(1, 0) = 1;
    cfmat(1, 2) = mu;

    // -mu*fz <= fy <= mu*fz
    cfmat(2, 1) = -1;
    cfmat(2, 2) = mu;
    cfmat(3, 1) = 1;
    cfmat(3, 2) = mu;

    // fz_min = 0.0 <= fz <= fz_max
    cfmat(4, 2) = -1.0;
    cfvec[4] = fz_max;  // -fz + fz_max >= 0
    cfmat(5, 2) = 1.0;

#ifdef MPC_PRINT_ALL

    std::cout << "cfmat " << std::endl;
    std::cout << cfmat << std::endl;

    std::cout << "cfvec " << std::endl;
    std::cout << cfvec << std::endl;
#endif

    // Populate constraint matrix
    CMat.setZero();
    cvec.setZero();
    for (int i = 0; i < n_Fr; i++) {
        CMat.block(6 * i, 3 * i, 6, 3) = cfmat;
        cvec.segment(6 * i, 6) = cfvec;
    }

#ifdef MPC_PRINT_ALL
    std::cout << "CMat " << std::endl;
    std::cout << CMat << std::endl;

    std::cout << "cvec " << std::endl;
    std::cout << cvec << std::endl;
#endif
}

void get_qp_constraints(const int& horizon, const Eigen::MatrixXd& CMat,
                        const Eigen::VectorXd& cvec, Eigen::MatrixXd& Cqp,
                        Eigen::VectorXd& cvec_qp) {
    Cqp.setZero();
    cvec_qp.setZero();

    int num_rows = CMat.rows();
    int num_cols = CMat.cols();

    // Set the force constraints over the horizon
    // To DO: add 0 upperbound when reaction force should be 0.0 depending on
    // the gate cycle.
    for (int i = 0; i < horizon; i++) {
        Cqp.block(i * num_rows, i * num_cols, num_rows, num_cols) = CMat;
        cvec_qp.segment(i * num_rows, num_rows) = cvec;
    }

#ifdef MPC_PRINT_ALL
    std::cout << "Cqp" << std::endl;
    std::cout << Cqp.rows() << "x" << Cqp.cols() << std::endl;
    std::cout << Cqp << std::endl;

    std::cout << "cvec_qp" << std::endl;
    std::cout << cvec_qp << std::endl;
#endif
}

void get_qp_costs(const int& n, const int& m, const int& horizon,
                  const Eigen::VectorXd& vecS_cost, const double& control_alpha,
                  Eigen::MatrixXd& Sqp, Eigen::MatrixXd& Kqp) {
    Eigen::MatrixXd S_cost = vecS_cost.asDiagonal();
    Sqp.setZero();
    for (int i = 0; i < horizon; i++) {
        Sqp.block(n * i, n * i, n, n) = S_cost;
    }

    Kqp = control_alpha * Eigen::MatrixXd::Identity(m * horizon, m * horizon);

#ifdef MPC_PRINT_ALL
    std::cout << "S_cost" << std::endl;
    std::cout << S_cost << std::endl;

    std::cout << "Sqp" << std::endl;
    std::cout << Sqp << std::endl;

    std::cout << "Kqp" << std::endl;
    std::cout << Kqp << std::endl;
#endif
}

void solve_mpc_qp(const Eigen::MatrixXd& Aqp, const Eigen::MatrixXd& Bqp,
                  const Eigen::VectorXd& X_ref, const Eigen::VectorXd& x0,
                  const Eigen::MatrixXd& Sqp, const Eigen::MatrixXd& Kqp,
                  const Eigen::MatrixXd& Cqp, const Eigen::VectorXd& cvec_qp,
                  Eigen::VectorXd& f_vec_out) {
#ifdef MPC_TIME_ALL
    std::chrono::high_resolution_clock::time_point t_qp_prep_start;
    std::chrono::high_resolution_clock::time_point t_qp_prep_end;
    double dur_qp_prep = 0.0;

    std::chrono::high_resolution_clock::time_point t_qp_mult_start;
    std::chrono::high_resolution_clock::time_point t_qp_mult_end;
    double dur_qp_mult = 0.0;

    t_qp_prep_start = std::chrono::high_resolution_clock::now();
    t_qp_mult_start = std::chrono::high_resolution_clock::now();
#endif

    Eigen::MatrixXd H(Bqp.cols(), Bqp.cols());
    Eigen::VectorXd g_qp(Bqp.cols());

    // For some reason g_qp evaluates slow unless we do these operations first
    // std::cout << "x0" << x0.transpose() << std::endl;
    Eigen::MatrixXd BqpT = Bqp.transpose();
    Eigen::MatrixXd m1;
    m1.noalias() = BqpT * Sqp;
    Eigen::MatrixXd m2;
    m2.noalias() = 2.0 * m1;
    Eigen::VectorXd v1 = (Aqp * x0).eval() - X_ref;

    H = 2.0 * (m1 * Bqp + Kqp);
    g_qp.noalias() = m1 * v1;

#ifdef MPC_TIME_ALL
    t_qp_mult_end = std::chrono::high_resolution_clock::now();
#endif

    // std::cout << "H" << std::endl;
    // std::cout << H << std::endl;

    // std::cout << "g" << std::endl;
    // std::cout << g.transpose() << std::endl;

    // Quadprog matrices vectors
    GolDIdnani::GMatr<double> G, CE, CI;
    GolDIdnani::GVect<double> g0, ce0, ci0, x;
    int n_quadprog_ = 1;
    int p_quadprog_ = 0;
    int m_quadprog_ = 0;

    // Quadprog size setup
    n_quadprog_ = Bqp.cols();      // number of decision variables
    m_quadprog_ = 0;               // number of equality constraints
    p_quadprog_ = cvec_qp.size();  // number of inequality constraints
                                   // (unilateral force constraints).

    // std::cout << "n_quadprog_" << n_quadprog_ << std::endl;
    // std::cout << "p_quadprog_" << p_quadprog_ << std::endl;

    f_vec_out.resize(n_quadprog_);

    x.resize(n_quadprog_);  // Decision Variables
    // Objective
    G.resize(n_quadprog_, n_quadprog_);
    g0.resize(n_quadprog_);
    // Equality Constraints
    CE.resize(n_quadprog_, m_quadprog_);
    ce0.resize(m_quadprog_);
    // Inequality Constraints
    CI.resize(n_quadprog_, p_quadprog_);
    ci0.resize(p_quadprog_);

    // Solve the following QP:
    // min (1/2)*U^T*H*U + U^T*g
    // st. C*U <= cvec_qp
    // where U is the decision variable (vector of forces)

    // Populate G Cost Matrix
    // G = P
    for (int i = 0; i < n_quadprog_; i++) {
        for (int j = 0; j < n_quadprog_; j++) {
            G[i][j] = H(i, j);
        }
    }

    // Populate g0 cost vector
    for (int i = 0; i < n_quadprog_; i++) {
        g0[i] = g_qp[i];
    }

    // No equality constraints. So CE and ce0 are untouched.

    // Populate Inequality Constraint Matrix
    for (int i = 0; i < p_quadprog_; i++) {
        for (int j = 0; j < n_quadprog_; j++) {
            CI[j][i] = Cqp(i, j);
        }
    }

    // Populate Inequality Constraint Vector
    // ci0 = h_
    for (int i = 0; i < p_quadprog_; i++) {
        ci0[i] = cvec_qp[i];
    }

#ifdef MPC_TIME_ALL

    t_qp_prep_end = std::chrono::high_resolution_clock::now();
    dur_qp_mult = std::chrono::duration_cast<std::chrono::duration<double> >(
                      t_qp_mult_end - t_qp_mult_start)
                      .count();
    dur_qp_prep = std::chrono::duration_cast<std::chrono::duration<double> >(
                      t_qp_prep_end - t_qp_prep_start)
                      .count();

    std::cout << "QP mult took " << dur_qp_mult << " seconds." << std::endl;
    std::cout << "QP mult rate " << (1.0 / dur_qp_mult) << " Hz" << std::endl;
    std::cout << "QP prep took " << dur_qp_prep << " seconds." << std::endl;
    std::cout << "QP prep rate " << (1.0 / dur_qp_prep) << " Hz" << std::endl;

    // Time the QP solve
    std::chrono::high_resolution_clock::time_point t_qp_solve_start;
    std::chrono::high_resolution_clock::time_point t_qp_solve_end;
    double dur_qp_solve = 0.0;

    // Start
    t_qp_solve_start = std::chrono::high_resolution_clock::now();
#endif

    double qp_result = solve_quadprog(G, g0, CE, ce0, CI, ci0, x);

#ifdef MPC_TIME_ALL
    // End
    t_qp_solve_end = std::chrono::high_resolution_clock::now();
    dur_qp_solve = std::chrono::duration_cast<std::chrono::duration<double> >(
                       t_qp_solve_end - t_qp_solve_start)
                       .count();
#endif

    // Populate qd result from QP answer
    for (int i = 0; i < n_quadprog_; i++) {
        f_vec_out[i] = x[i];
    }
    // std::cout << "qp_result = " << qp_result << std::endl;
    // std::cout << "f_vec_out = " << f_vec_out.transpose() << std::endl;

#ifdef MPC_TIME_ALL
    std::cout << "QP took " << dur_qp_solve << " seconds." << std::endl;
    std::cout << "QP rate " << (1.0 / dur_qp_solve) << " Hz" << std::endl;
#endif
}

void print_f_vec(int& horizon, int& n_Fr, const Eigen::VectorXd& f_vec_out) {
    for (int i = 0; i < horizon; i++) {
        std::cout << "horizon:" << i + 1 << std::endl;

        for (int j = 0; j < n_Fr; j++) {
            std::cout << "  f" << j << ":"
                      << f_vec_out.segment(3 * i * n_Fr + 3 * j, 3).transpose()
                      << std::endl;
        }
    }
}

void solve_mpc(const Eigen::VectorXd& x0, const Eigen::VectorXd& X_des,
               const Eigen::MatrixXd& r_feet, const double& robot_mass,
               const Eigen::MatrixXd& I_body, const int& horizon,
               const double& mpc_dt, Eigen::VectorXd& f_vec_out) {
#ifdef MPC_TIME_ALL
    // Time the QP solve
    std::chrono::high_resolution_clock::time_point t_mat_setup_start;
    std::chrono::high_resolution_clock::time_point t_mat_setup_end;
    double dur_mat_setup = 0.0;
    // Start
    t_mat_setup_start = std::chrono::high_resolution_clock::now();
#endif

    int n_Fr = r_feet.cols();

    // create continuous time state space matrices
    double psi_yaw = x0[2];
    Eigen::MatrixXd A(13, 13);
    A.setZero();
    Eigen::MatrixXd B(13, 3 * n_Fr);
    B.setZero();
    int n = A.cols();
    int m = B.cols();
    cont_time_state_space(robot_mass, I_body, psi_yaw, r_feet, A, B);

    // create discrete time state space matrices
    Eigen::MatrixXd Adt(n, n);
    Adt.setZero();
    Eigen::MatrixXd Bdt(n, m);
    Bdt.setZero();
    discrete_time_state_space(mpc_dt, A, B, Adt, Bdt);

    // create A and B QP matrices
    Eigen::MatrixXd Aqp(n * horizon, n);
    Aqp.setZero();
    Eigen::MatrixXd Bqp(n * horizon, m * horizon);
    Bqp.setZero();
    qp_matrices(horizon, Adt, Bdt, Aqp, Bqp);

    // create force constraint matrices and vectors
    Eigen::MatrixXd CMat(
        6 * n_Fr,
        3 * n_Fr);  // the QP constraint matrix for n_Fr reaction forces
    Eigen::VectorXd cvec(
        6 * n_Fr);  // the constraint matrix for n_Fr reaction forces
    get_force_constraints(n_Fr, CMat, cvec);

    Eigen::MatrixXd Cqp(horizon * 6 * n_Fr, horizon * 3 * n_Fr);
    Eigen::VectorXd cvec_qp(horizon * 6 * n_Fr);
    get_qp_constraints(horizon, CMat, cvec, Cqp, cvec_qp);

    // Cost matrices (Values similar to cheetah 3)
    Eigen::MatrixXd Sqp(n * horizon, n * horizon);
    Eigen::MatrixXd Kqp(m * horizon, m * horizon);

    Eigen::VectorXd vecS_cost(n);
    //        <<  th1,  th2,  th3,  px,  py,  pz,   w1,  w2,   w3,   dpx,  dpy,
    //        dpz,  g
    // vecS_cost << 0.25, 0.25, 10.0, 2.0, 2.0, 50.0, 0.05, 0.05, 0.30, 0.20,
    // 0.2, 0.10, 0.0;
    vecS_cost << 1.0, 1.0, 10.0, 2.0, 2.0, 50.0, 0.05, 0.05, 0.30, 0.20, 0.2,
        0.10, 0.0;
    Eigen::MatrixXd S_cost = vecS_cost.asDiagonal();
    double control_alpha = 4e-5;
    get_qp_costs(n, m, horizon, vecS_cost, control_alpha, Sqp, Kqp);

#ifdef MPC_TIME_ALL
    // End
    t_mat_setup_end = std::chrono::high_resolution_clock::now();
    dur_mat_setup = std::chrono::duration_cast<std::chrono::duration<double> >(
                        t_mat_setup_end - t_mat_setup_start)
                        .count();

    std::cout << "QP matrices setup took " << dur_mat_setup << " seconds."
              << std::endl;
    std::cout << "QP matrices setup rate " << (1.0 / dur_mat_setup) << " Hz"
              << std::endl;

    std::cout << "robot state x_cur:" << x0.transpose() << std::endl;
    std::cout << "            x_des:" << X_des.head(n).transpose() << std::endl;
    std::cout << "horizon steps = " << horizon
              << ", horizon_time = " << horizon * mpc_dt << std::endl;
#endif

    // Solve MPC
    solve_mpc_qp(Aqp, Bqp, X_des, x0, Sqp, Kqp, Cqp, cvec_qp, f_vec_out);
}

void simulate_mpc() {
    // System Params
    double robot_mass = 10;  // kg mass of the robot
    Eigen::MatrixXd I_body =
        Eigen::MatrixXd::Identity(3, 3);  // Body Inertia matrix
    I_body(0, 0) = 5;
    I_body(0, 1) = 2;
    I_body(1, 0) = 2;
    I_body(1, 1) = 5;
    I_body(2, 2) = 2.5;

    // starting robot state
    // Current reduced state of the robot
    // x = [Theta, p, omega, pdot, g] \in \mathbf{R}^13
    Eigen::VectorXd x0(13);
    x0.setZero();
    x0[2] = 0.0;   // M_PI/24.0;; // Yaw orientation
    x0[3] = 0.0;   // x translation error
    x0[4] = 0.0;   // y translation error
    x0[5] = 0.75;  // Height
    x0[12] = -9.81;

    // Feet Configuration
    // Set Foot contact locations
    Eigen::MatrixXd r_feet(3, 4);  // Each column is a reaction force in x,y,z
    r_feet.setZero();
    double foot_length = 0.05;   // 5cm distance between toe and heel
    double nominal_width = 0.1;  // 10cm distance between left and right feet

    // Flat ground contact, z = 0.0
    // 2 Contact Configuration
    // // Right Foot
    // r_feet(0, 0) = 0.0; // x
    // r_feet(1, 0) = -nominal_width/2.0; //y
    // // Left Foot
    // r_feet(0, 1) = 0.0;  // x
    // r_feet(1, 1) = nominal_width/2.0; //y

    // 4 Contact Configuration
    // Right Foot Front
    r_feet(0, 0) = foot_length / 2.0;     // x
    r_feet(1, 0) = -nominal_width / 2.0;  // y
    // Right Foot Back
    r_feet(0, 1) = -foot_length / 2.0;    // x
    r_feet(1, 1) = -nominal_width / 2.0;  // y
    // Left Foot Front
    r_feet(0, 2) = foot_length / 2.0;    // x
    r_feet(1, 2) = nominal_width / 2.0;  // y
    // Left Foot Back
    r_feet(0, 3) = -foot_length / 2.0;   // x
    r_feet(1, 3) = nominal_width / 2.0;  // y

    int horizon = 10;
    double mpc_dt = 0.025;

    int n_Fr = r_feet.cols();  // Number of contacts
    int n = 13;
    int m = 3 * n_Fr;

    Eigen::VectorXd f_vec_out(m * horizon);
    Eigen::MatrixXd f_Mat(3, n_Fr);
    f_Mat.setZero();

    // Get desired reference
    Eigen::VectorXd X_des(n * horizon);
    get_desired_x(horizon, X_des);

    // Solve the MPC
    solve_mpc(x0, X_des, r_feet, robot_mass, I_body, horizon, mpc_dt,
              f_vec_out);
    print_f_vec(horizon, n_Fr, f_vec_out);
    // Populate force output from 1 horizon.
    assemble_vec_to_matrix(3, n_Fr, f_vec_out.head(3 * n_Fr), f_Mat);

    // Simulate MPC for one time step
    double sim_dt = 1e-3;
    Eigen::VectorXd x_prev(n);
    x_prev = x0;
    Eigen::VectorXd x_next(n);
    x_next.setZero();
    integrate_robot_dynamics(sim_dt, x_prev, f_Mat, r_feet, robot_mass, I_body,
                             x_next);

    // Simulate MPC for x seconds
    double total_sim_time = 7.0;
    int sim_steps = static_cast<int>(total_sim_time / sim_dt);
    std::cout << "sim_steps:" << sim_steps << std::endl;

    double last_control_time = 0.0;
    double cur_time = 0.0;

    std::cout << "x_start:" << x0.transpose() << std::endl;
    Eigen::VectorXd f_cmd(12);
    f_cmd.setZero();

    bool print_for_plots = false;

    if (print_for_plots) {
        printf(
            "t, r, p, y, x, y, z, wx, wy, wz, dx, dy, dz, f0x, f0y, f0z, f1x, "
            "f1y, f1z, f2x, f2y, f2z, f3x, f3y, f3z\n");
    }
    for (int i = 0; i < sim_steps; i++) {
        // Solve new MPC every mpc control tick
        if ((cur_time - last_control_time) > mpc_dt) {
            solve_mpc(x_prev, X_des, r_feet, robot_mass, I_body, horizon,
                      mpc_dt, f_vec_out);
            assemble_vec_to_matrix(3, n_Fr, f_vec_out.head(3 * n_Fr), f_Mat);
            last_control_time = cur_time;
            f_cmd.head(m) = f_vec_out.head(m);

            if (!print_for_plots) {
                std::cout << "  Computed MPC Forces" << std::endl;
                printf("  f0:(%0.3f,%0.3f,%0.3f)\n", f_cmd[0], f_cmd[1],
                       f_cmd[2]);
                printf("  f1:(%0.3f,%0.3f,%0.3f)\n", f_cmd[3], f_cmd[4],
                       f_cmd[5]);
                if (n_Fr > 2) {
                    printf("  f2:(%0.3f,%0.3f,%0.3f)\n", f_cmd[6], f_cmd[7],
                           f_cmd[8]);
                    printf("  f3:(%0.3f,%0.3f,%0.3f)\n", f_cmd[9], f_cmd[10],
                           f_cmd[11]);
                }
            }
        }

        // Integrate the robot dynamics
        integrate_robot_dynamics(sim_dt, x_prev, f_Mat, r_feet, robot_mass,
                                 I_body, x_next);
        // Update x
        x_prev = x_next;
        // Increment time
        cur_time += sim_dt;

        if (print_for_plots) {
            printf(
                "%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f, "
                "%0.3f, %0.3f, %0.3f, %0.3f, %0.3f, %0.3f,%0.3f,%0.3f, "
                "%0.3f,%0.3f,%0.3f, %0.3f,%0.3f,%0.3f, %0.3f,%0.3f,%0.3f \n",
                cur_time, x_next[0], x_next[1], x_next[2], x_next[3], x_next[4],
                x_next[5], x_next[6], x_next[7], x_next[8], x_next[9],
                x_next[10], x_next[11], f_cmd[0], f_cmd[1], f_cmd[2], f_cmd[3],
                f_cmd[4], f_cmd[5], f_cmd[6], f_cmd[7], f_cmd[8], f_cmd[9],
                f_cmd[10], f_cmd[11]);
        } else {
            printf(
                "t:%0.3f, (R,P,Y):(%0.3f, %0.3f, %0.3f), (x,y,z):(%0.3f, "
                "%0.3f, %0.3f), (wx, wy, wz):(%0.3f, %0.3f, %0.3f), (dx, dy, "
                "dz):(%0.3f, %0.3f, %0.3f) \n",
                cur_time, x_next[0], x_next[1], x_next[2], x_next[3], x_next[4],
                x_next[5], x_next[6], x_next[7], x_next[8], x_next[9],
                x_next[10], x_next[11]);
        }

        // std::cout << "    f:" << f_vec_out.head(m).transpose() << std::endl;
        // for(int j = 0; j < n_Fr; j++){
        //   std::cout << "    f" << j << ":" << f_vec_out.segment(3*j,
        //   3).transpose() << std::endl;
        // }
        // std::cout << "cur_time:" << cur_time << "  last_control_time:" <<
        // last_control_time << std::endl;
    }
    std::cout << "x_des" << X_des.tail(n).transpose() << std::endl;
}

int main(int argc, char** argv) {
    simulate_mpc();
    return 0;
}


#ifndef HH_PRE_HPP_INCLUDED
#define HH_PRE_HPP_INCLUDED

#include <vector>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>

//UNITS: sec, V, A, M

//DECLARATIONS FOR HH_PRE_MODEL

class HH_PRE
{
   private:
      const double pi = boost::math::constants::pi<double>();
      double Cm = 1.0;   // F/m^2 Specific capacitance
      //----Reversal potentials for Na, K, channels in mV
      double ENa = 50.0;   // Reversal potential for Sodium currents in Volts
      double EK = -85.0;    // Reversal potential for Potassium currents in Volts
      double ELeak = -81; // Reversal potential for Leak currents in Volts

      double GNa = 100;   // Maximum conductance for Sodium channels S/m^2
      double GK = 36;    // Maximum conductance for Potassium channels S/m^2
      double GLeak = 0.5;  // Maximum conductance for Leak channels S/m^2

   public:
      double IExt = 0.0; //extern current in Amps
      double Area = 0.0;
      double Cap = 0.0;
      double V = 0.0;      // Membrane voltage V
      /* NEURON class explicit constructor */
      HH_PRE( double Area_ = 500E-08): Area(Area_)
      {
         Cap = Cm * Area;
      };
      //---Equations for bouton Na Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
      double alpha_Na_act(const double V);
      double beta_Na_act(const double V);
      double alpha_Na_inact(const double V);
      double beta_Na_inact(const double V);
      //---Equations for bouton K Channel (Ref: Dominique Engel & Peter Jonas, 2005, Neuron)
      double alpha_K_act(const double V);
      double beta_K_act(const double V);
      //---Declaration of the ODE solver
      template <class State, class Deriv >
      void operator() ( const State &x, Deriv &dxdt , const double  t );
};
//Function declarations
//------------- Na+ channels --- implimented from classical HH (1952)
double HH_PRE::alpha_Na_act(const double V)
{
   double a = 0.1;
   double b = 45;
   double c = 10;
   double val = (- a * ( (V + b) / (exp( -(V + b) / c ) - 1 ) ) );

   double A = 93.82;
   double B = -105;
   double C = 17;
   double VAL = (- A * ( (V + B) / (exp( -(V + B) / C ) - 1 ) ) );
   return (VAL);
}
double HH_PRE::beta_Na_act(const double V)
{
   double a = 4;
   double b = 70;
   double c = 18;
   double val = ( a * ( exp( - (V + b) / c )) );

   double A = 0.168;
   double C = 23.27;
   double VAL = ( A *  exp( - V / C ) );

   return (VAL);
}
double HH_PRE::alpha_Na_inact(const double V)
{
   double val = 0.07 * exp( -( V + 70) /20 );

   double A = 0.0001;
   double C = 18.706;
   double VAL = ( A *  exp( - (V + 0) / C ) );

   return (VAL);
}
double HH_PRE::beta_Na_inact(const double V)
{
   double val = ( 1 / ( exp( - (V + 40) / 10) + 1) );

   double A = 6.6;
   double B = 17.6;
   double C = 13.3;
   double VAL = ( A / ( (exp( -(V + B) / C ) + 1 ) ) );

   return (VAL);
}
//--------K+ channel alpha & beta
double HH_PRE::alpha_K_act(const double V)
{
   double val = -0.01 * (V + 60) / ( exp( -(V +  60) / 10) - 1 );
   double VAL = -0.01 * (V + 55) / ( exp( -(V +  55) / 10) - 1 );
   return (VAL);
};
double HH_PRE::beta_K_act(const double V)
{
   double val = 0.125 * exp (- (V + 70) / 80);
   double VAL = 0.125 * exp (- (V + 65) / 80);
   return (VAL);
};
//-------HH_PRE class ODE Function

template <class State, class Deriv >
void HH_PRE::operator() ( const State &x, Deriv &dxdt , const double t )
{
   //typename boost::range_iterator< const State >::type x_it = boost::begin( x_ );
   //typename boost::range_iterator< Deriv >::type dxdt_it = boost::begin( dxdt_ );

   dxdt[0] = ( alpha_Na_act(x[3]) * (1 - x[0]) ) - (beta_Na_act(x[3]) * x[0]); // Na activation
   dxdt[1] = ( alpha_Na_inact(x[3]) * (1 - x[1]) ) - (beta_Na_inact(x[3]) * x[1]); // Na inactivation
   dxdt[2] = ( alpha_K_act(x[3]) * (1 - x[2]) ) - (beta_K_act(x[3]) * x[2]); // K activation
   double INa =   -(Area * GNa * pow(x[0],3) * x[1] * (x[3] - ENa) );
   double IK =   -(Area * GK * pow(x[2],4) * (x[3] - EK) );
   double ILeak = -(Area * GLeak *  (x[3] - ELeak) );
   dxdt[3] = (1/Cap) * (INa + IK + ILeak  + (IExt * Area)  );
};

#endif // HH_PRE_HPP_INCLUDED


#include "icp.h"

#include <iostream>

#include <Eigen/Dense>

using FloatType = double;
using IndexType = int64_t;
using EigenMatrix = Eigen::MatrixXd;
using EigenVector = Eigen::VectorXd;

#include "common/io.h"
#include "common/kdtree.h"
#include "common/debug.h"
#include "svd.h"

// Custom struct for KD tree
struct Point : public Vec3 {
    Point() {}
    Point(const Vec3 &v, int index = -1) : Vec3(v), i(index) {}
    int i;
};

// Rodrigues rotation matrix
EigenMatrix rodrigues(const EigenVector &w, const double theta) {
    const double c = std::cos(theta);
    const double s = std::sin(theta);

    const double wx = w(0);
    const double wy = w(1);
    const double wz = w(2);
    EigenMatrix K(3, 3);
    K << 0.0, -wz, wy, wz, 0.0, -wx, -wy, wx, 0.0;

    EigenMatrix I = EigenMatrix::Identity(3, 3);
    return I + K * s + (K * K) * (1.0 - c);
}

//! single point to point ICP step
void point2pointICP_step(const std::vector<Vec3> &target, const std::vector<Vec3> &source, EigenMatrix *rotMat,
                         EigenVector *trans) {
    // Construct Kd-tree to find closest points
    KDTree<Vec3> tree;
    tree.construct(target);

    // {{ NOT_IMPL_ERROR();

    // Match closest point pairs
    // Hint:
    // Construct here that two matrices X and P,
    // whose elements are positions of source vertices (X),
    // and those for closest points of the sources (P)
    const int nPoints = (int)source.size();
    EigenMatrix X(nPoints, 3);
    EigenMatrix P(nPoints, 3);
    for (int i = 0; i < nPoints; i++) {
        const Vec3 &v = source[i];
        const Vec3 u = tree.nearest(v);
        X.row(i) << v.x, v.y, v.z;
        P.row(i) << u.x, u.y, u.z;
    }

    // Subtract xMean, pMean from X, P
    // Hint:
    // Row-wise or column-wise mean (also sum, max, min etc.) of Eigen matrices
    // can be easily calculate using "rowwise()" or "colwise()".
    const EigenVector xMean = X.colwise().mean();
    const EigenVector pMean = P.colwise().mean();
    X.rowwise() -= xMean.transpose();
    P.rowwise() -= pMean.transpose();

    // Solve with SVD to obtain R
    // Hint:
    // An SVD method "eigenSVD" is already given in "svd.h".
    // See its arguments carefully when using it.
    const EigenMatrix M = P.transpose() * X;
    EigenMatrix U, V;
    EigenVector sigma;
    eigenSVD(M, U, sigma, V);
    const double detVU = (U * V.transpose()).determinant();

    // Revise sign of determinant
    EigenVector diagH(3);
    diagH << 1.0, 1.0, detVU;

    // Output
    // Hint:
    // Be careful that arguments for output matrices and vectors
    // are represented as "pointers". Therefore, you should insert
    // values for them with "asterisk" like "*lhs = rhs".
    *rotMat = U * diagH.asDiagonal() * V.transpose();
    *trans = pMean - (*rotMat) * xMean;

    // }}
}

//! single point to plane ICP step
void point2planeICP_step(const std::vector<Vec3> &target, const std::vector<Vec3> &targetNorm,
                         const std::vector<Vec3> &source, Eigen::MatrixXd *rotMat, Eigen::VectorXd *trans) {
    // Construct Kd-tree to find closest points
    std::vector<Point> points;
    for (int i = 0; i < (int)target.size(); i++) {
        points.emplace_back(target[i], i);
    }

    KDTree<Point> tree;
    tree.construct(points);

    // {{ NOT_IMPL_ERROR();

    // Construct linear system
    // Hint:
    // prepare matrix A and vector b
    // A is 6x6 matrix, and b is 6-D vector
    const int nPoints = (int)source.size();
    EigenMatrix A(6, 6);
    EigenVector b(6);
    A.setZero();
    b.setZero();
    for (int i = 0; i < nPoints; i++) {
        const Vec3 &x = source[i];
        const Point pt = tree.nearest(x);
        const Vec3 &p = Vec3(pt.x, pt.y, pt.z);
        const Vec3 &n = targetNorm[pt.i];
        const Vec3 x_cross_n = cross(x, n);
        EigenVector v(6);
        v << x_cross_n.x, x_cross_n.y, x_cross_n.z, n.x, n.y, n.z;
        A += v * v.transpose();
        b += v * (dot(n, p - x));
    }

    // Solve
    // Hint:
    // Use "Eigen::PartialPivLU" to solve the system
    Eigen::PartialPivLU<EigenMatrix> lu(A);
    EigenVector u = lu.solve(b);

    // Substitute to instances a, t
    // Hint:
    // Eigen matrices and vectors can be initialized
    // with its elements by "<<" operator.
    EigenVector a(3);
    a << u(0), u(1), u(2);
    EigenVector t(3);
    t << u(3), u(4), u(5);

    // Reproduce rotation matrix
    // Hint:
    // Remember that vector "a" is the multiple of
    // the direction of rotation axis and
    // the degree of rotation angle.
    const double theta = a.norm();
    const EigenVector w = a / theta;
    *rotMat = rodrigues(w, theta);
    *trans = t;

    // }}
}

void rigidICP(const std::vector<Vec3> &target, const std::vector<Vec3> &targetNorm, std::vector<Vec3> &source,
              std::vector<Vec3> &sourceNorm, ICPMetric metric, int maxIters, double tolerance, bool verbose) {
    // Point-to-point ICP
    for (int it = 0; it < maxIters; it++) {
        Eigen::MatrixXd R(3, 3);
        Eigen::VectorXd t(3);
        switch (metric) {
        case ICPMetric::Point2Point:
            point2pointICP_step(target, source, &R, &t);
            break;

        case ICPMetric::Point2Plane:
            point2planeICP_step(target, targetNorm, source, &R, &t);
            break;

        default:
            throw std::runtime_error("Unknown ICP metric type!");
        }

        // Apply rigid transformation
        for (int i = 0; i < (int)source.size(); i++) {
            Eigen::VectorXd v(3);
            v << source[i].x, source[i].y, source[i].z;
            Eigen::VectorXd u = R * v + t;
            source[i] = Vec3(u(0), u(1), u(2));

            Eigen::VectorXd n(3);
            n << sourceNorm[i].x, sourceNorm[i].y, sourceNorm[i].z;
            Eigen::VectorXd m = R * n;
            sourceNorm[i] = Vec3(m(0), m(1), m(2));
        }

        // Check current error
        const double error = t.norm() + (R - Eigen::MatrixXd::Identity(3, 3)).norm();

        // Report
        if (verbose) {
            printf("*** %d iteration ***\n", it + 1);
            printf("R = \n");
            std::cout << R << std::endl;
            printf("t = \n");
            std::cout << t << std::endl;
            printf("error = %f\n", error);
            printf("\n");
        }

        char outfile[256];
        sprintf(outfile, "intermediate_%03d.off", it);
        write_off(std::string(outfile), source, sourceNorm);

        if (error < tolerance) {
            break;
        }
    }
}


/*
 utils.cxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <boost/range/numeric.hpp>
#include <cmath>

#include "utils.hxx"

double det3(ublas::matrix<double> a) {
  /* Fast Determinant of 3x3 Matrix */
  double det = a(0,0)*(a(1,1)*a(2,2) - a(1,2)*a(2,1))
             - a(0,1)*(a(1,0)*a(2,2) - a(1,2)*a(2,0))
             + a(0,2)*(a(1,0)*a(2,1) - a(1,1)*a(2,0));
  return det;
}

ublas::vector<double> cross3(ublas::vector<double> a,
                             ublas::vector<double> b) {

  /* Cross Product for Three-Dimensional Vectors */
  if (a.size() != 3 or b.size() != 3) {
    std::cout << "Exit(1): Vector dimensions not compatible for cross product\n";
    exit(1);
  }
  ublas::vector<double> c(3);
  c(0) = a(1)*b(2) - a(2)*b(1);
  c(1) = a(2)*b(0) - a(0)*b(2);
  c(2) = a(0)*b(1) - a(1)*b(0);
  return c;
}

ublas::matrix<double> inv3(ublas::matrix<double> a) {

  /* Matrix Inversion for Three-Dimensional Vectors */
  ublas::matrix<double> ia(a.size1(),a.size2());
  double det = det3(a);

  ia(0,0) = a(1,1)*a(2,2) - a(1,2)*a(2,1);
  ia(0,1) = a(0,2)*a(2,1) - a(0,1)*a(2,2);
  ia(0,2) = a(0,1)*a(1,2) - a(0,2)*a(1,1);

  ia(1,0) = a(1,2)*a(2,0) - a(1,0)*a(2,2);
  ia(1,1) = a(0,0)*a(2,2) - a(0,2)*a(2,0);
  ia(1,2) = a(0,2)*a(1,0) - a(0,0)*a(1,2);

  ia(2,0) = a(1,0)*a(2,1) - a(1,1)*a(2,0);
  ia(2,1) = a(0,1)*a(2,0) - a(0,0)*a(2,1);
  ia(2,2) = a(0,0)*a(1,1) - a(1,0)*a(0,1);

  return ia/det;
}

long linear_search(const std::vector<double>& vec,
		   double a,
		   double tol) {

  for (long i = 0; i < vec.size(); i++) {
    double value = fabs(vec[i] - a);
    if (value < tol) {
      return i;
    }
  }

  return vec.size();
}

//inline long product(ublas::vector<long> vec) {
//  return boost::accumulate(vec,1,std::multiplies<long>());
//}


#include "openbr/plugins/openbr_internal.h"
#include "openbr/core/opencvutils.h"
#include "openbr/core/eigenutils.h"

#include <opencv2/contrib/contrib.hpp>

#include <Eigen/Dense>

using namespace Eigen;
using namespace cv;

namespace br
{

class ShapeAxisRatioTransform : public UntrainableTransform
{
    Q_OBJECT

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        Mat indices;
        findNonZero(src,indices);

        dst.m() = Mat(1,1,CV_32FC1);

        if (indices.total() > 0) {
            MatrixXd data(indices.total(),2);

            for (size_t i=0; i<indices.total(); i++) {
                data(i,0) = indices.at<Point>(i).y;
                data(i,1) = indices.at<Point>(i).x;
            }

            MatrixXd centered = data.rowwise() - data.colwise().mean();
            MatrixXd cov = (centered.adjoint() * centered) / double(data.rows() - 1);

            SelfAdjointEigenSolver<Eigen::MatrixXd> eSolver(cov);
            MatrixXd D = eSolver.eigenvalues();

            if (eSolver.info() == Success)
                dst.m().at<float>(0,0) = D(0)/D(1);
            else
                dst.file.fte = true;
        } else {
            dst.file.fte = true;
            qWarning("No mask content for %s.",qPrintable(src.file.baseName()));
        }
    }
};

BR_REGISTER(Transform, ShapeAxisRatioTransform)

} // namespace br

#include "imgproc/shapeaxisratio.moc"


/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

#include "packages/pnp/gems/epnp/epnp.hpp"

#include <Eigen/SVD>

#include <cmath>
#include <utility>

namespace isaac {
namespace pnp {
namespace epnp {

// High-level function that implements the full EPnP pipeline
pnp::Status ComputeCameraPose(double focal_u, double focal_v, double principal_u,
                              double principal_v, const Matrix3Xd& points3,
                              const Matrix2Xd& points2, epnp::Result* result) {
  if (result == nullptr) {
    return pnp::Status::kErrorBadInputParams;
  }

  // Make sure focal lengths in pixels are non-zero.
  // There is no restriction on the principal point location.
  if (focal_u == 0 || focal_v == 0) {
    return pnp::Status::kErrorBadInputParams;
  }

  // At least 6 2D-3D point correspondences are required currently.
  // TODO: extend to 5 and 4 (requires complicated relinearization)
  if (points2.cols() != points3.cols() || points3.cols() < 6) {
    return pnp::Status::kErrorBadNumInputPoints;
  }

  // Compute control points in world coordinates. They represent a chosen 3D basis.
  constexpr double tol = 1e-3;
  result->ctl_points_world = ChooseBasis(points3, tol);

  // The dimensionality of the point cloud is the number of control points minus one because
  // the first control point is always the centroid.
  // Except when zero input points passed to ChooseBasis().
  result->input_dims = result->ctl_points_world.cols();
  if (result->input_dims) {
    result->input_dims--;
  }

  // Only planar and 3D input point cloud shapes are useful for camera pose estimation.
  if (result->input_dims < 2 || result->input_dims > 3) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  // Compute barycentric coordinates of all input 3D points with respect to chosen basis.
  result->bary_coeffs = ComputeBaryCoords(points3, result->ctl_points_world);
  if (result->bary_coeffs.cols() == 0) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  // Calculate space of the control point camera coordinates such that all
  // projection constraints are approximately satisified.
  if (!SolveProjConstraints(focal_u, focal_v, principal_u, principal_v, result->bary_coeffs,
                            points2, &result->proj_coeffs, &result->solution_basis,
                            &result->singular_values)) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  // Precompute distances between control points in the world frame.
  VectorXd ctl_distances = ComputeDistances(result->ctl_points_world);

  // Construct calibration matrix needed for reprojections at residual calculation.
  Matrix3d calib_matrix;
  calib_matrix << focal_u, 0, principal_u, 0, focal_v, principal_v, 0, 0, 1;

  // Given the solution basis, solve for control point camera coordinates.
  // Solve for various possible dimensions of the solution space and retain
  // the resulting pose that has the least reprojection error.
  const int max_solution_dims = 3;  // Only 1,2,3 are supported
  double min_rss_error = Inf;
  result->rss_repr_errors = VectorXd::Constant(max_solution_dims, Inf);
  for (int k = 0; k < max_solution_dims; k++) {
    const int sol_dims = k + 1;

    // Compute weights that describe the concrete solution in the solution space.
    Vector4d weights = SolveControlPoints(result->solution_basis, sol_dims, ctl_distances);
    if (weights == Vector4d::Zero()) {
      continue;
    }

    // Calculate control points in camera frame given the weights and the base vectors.
    Matrix3Xd ctl_points_cam = ReshapeToMatrix3xN(result->solution_basis * weights);

    // Compute rigid-body transform between control points in world and in camera frame.
    // The resulting transformation is the sought camera pose.
    Matrix3d rotation;
    Vector3d translation;
    if (!pnp::RegisterPointsEucl(result->ctl_points_world, ctl_points_cam, &rotation,
                                 &translation)) {
      continue;
    }

    // Compute squared reprojection errors of the input points, given the computed camera pose.
    VectorXd repr_errors = pnp::ColwiseSquaredNorms(
        points2 - pnp::EuclideanFromHomogeneous(calib_matrix *
                                                ((rotation * points3).colwise() + translation)));

    // Calculate reprojection Residual Sum of Squares (RSS).
    double rss_error = repr_errors.sum();
    result->rss_repr_errors(k) = rss_error;

    // Keep the best solution in terms of reprojection RSS.
    if (rss_error < min_rss_error) {
      min_rss_error = rss_error;
      result->ctl_points_cam = ctl_points_cam;
      result->rotation = rotation;
      result->translation = translation;
      result->solution_dims = sol_dims;
    }
  }

  // Solution could not be found with any dimensionality of the solution space.
  if (min_rss_error == Inf) {
    return pnp::Status::kErrorDegenerateGeom;
  }

  return pnp::Status::kSuccess;
}

// Choose a suitable 3D basis to represent the input 3D points in (via barycentric coordinates).
// Principal Component Analysis of the 3D point cloud.
Matrix3Xd ChooseBasis(const Matrix3Xd& points3, double tol) {
  // The output basis will be represented by a small set of 3D control points (up to 4).
  Matrix3Xd ctl_points;
  ctl_points.resize(3, 0);

  // At least 1 input point is required for further calculations to make sense.
  const int num_points = points3.cols();
  if (num_points < 1) {
    return ctl_points;
  }

  // Calculate centroid of the point cloud - requires at least 1 point.
  Vector3d centroid = points3.rowwise().sum() / num_points;

  // Center the point cloud - requires at least 1 point
  Matrix3Xd centered_points3 = points3;
  for (int i = 0; i < num_points; i++) {
    centered_points3.col(i) -= centroid;
  }

  // Compute principal components.
  // Singular values are sorted in decreasing order.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd;
  svd.compute(centered_points3, Eigen::ComputeFullU | Eigen::ComputeThinV);
  const VectorXd& singular_values = svd.singularValues();
  const MatrixXd& principal_directions = svd.matrixU();

  // Determine the number of non-vanishing principal components given a tolerance.
  int dims = 0;
  for (int i = 0; i < singular_values.rows(); i++) {
    if (singular_values(i) > tol) {
      dims++;
    }
  }

  // The output matrix is always of size 3-by-(dims+1) with the first column being the centroid
  // of the input point cloud.
  ctl_points = Matrix3Xd::Zero(3, dims + 1);
  ctl_points.col(0) = centroid;

  // Scale the other control points with respect to the centroid along the principal directions
  // such that their distance from the centroid equals the square root of the point variance.
  // This is only done for non-vanishing principal directions.
  const double factor = 1.0 / std::sqrt(static_cast<double>(num_points));
  for (int i = 0; i < dims; i++) {
    ctl_points.col(i + 1) = principal_directions.col(i) * factor * singular_values(i) + centroid;
  }

  return ctl_points;
}

// Compute barycentric coords of points with respect to a basis defined by 3 or 4 control points.
// Solves the linear problem A*X = B for X where:
// B is the 4xN matrix of homogeneous 3D points (3xN matrix points3 extended by a row of 1's)
// A is the 4x3 or 4x4 homogenenous matrix of control point coordinates,
// X is the 3xN or 4xN matrix of barycentric coordinates.
// Since the last columns of A and B are all 1's, the column sums of X are 1.
MatrixXd ComputeBaryCoords(const Matrix3Xd& points3, const Matrix3Xd& ctl_points) {
  MatrixXd bary_coords;
  bary_coords.resize(ctl_points.cols(), 0);
  if (points3.cols() && (ctl_points.cols() == 3 || ctl_points.cols() == 4)) {
    Matrix4Xd basis = pnp::HomogeneousFromEuclidean(ctl_points);

    // Solve the problem A*X = B
    bary_coords = basis.householderQr().solve(pnp::HomogeneousFromEuclidean(points3));
  }
  return bary_coords;
}

// Solve the projection equations via Singular-Value Decomposition and
// find the solution space of the control point camera coordinates.
bool SolveProjConstraints(double focal_u, double focal_v, double principal_u, double principal_v,
                          const MatrixXd& bary_coords, const Matrix2Xd points2,
                          MatrixXd* proj_coeffs, MatrixXd* sol_basis, VectorXd* singular_values) {
  if (proj_coeffs == nullptr || sol_basis == nullptr || singular_values == nullptr) {
    return false;
  }

  proj_coeffs->resize(0, 0);

  // At least 6 input points are required, each generating 2 equations.
  int num_points = points2.cols();
  if (num_points < 6) {
    return false;
  }
  if (bary_coords.cols() != num_points) {
    return false;
  }

  // Focal lengths should be non-zero.
  if (focal_u == 0 || focal_v == 0) {
    return false;
  }

  // Shorthands to make it easier to visually verify projection coefficients below.
  const double fu = focal_u;
  const double fv = focal_v;
  const double uc = principal_u;
  const double vc = principal_v;

  // Non-planar case with 4 control points (12 unknowns)
  if (bary_coords.rows() == 4) {
    // Construct the 2Nx12 coefficient matrix
    proj_coeffs->resize(2 * num_points, 12);
    for (int i = 0; i < num_points; i++) {
      // Shorthands to make it easier to check coefficient matrix
      const double u = points2(0, i);
      const double v = points2(1, i);
      const double b1 = bary_coords(0, i);
      const double b2 = bary_coords(1, i);
      const double b3 = bary_coords(2, i);
      const double b4 = bary_coords(3, i);
      proj_coeffs->row(2 * i + 0) << b1 * fu, 0, b1 * (uc - u), b2 * fu, 0, b2 * (uc - u), b3 * fu,
          0, b3 * (uc - u), b4 * fu, 0, b4 * (uc - u);
      proj_coeffs->row(2 * i + 1) << 0, b1 * fv, b1 * (vc - v), 0, b2 * fv, b2 * (vc - v), 0,
          b3 * fv, b3 * (vc - v), 0, b4 * fv, b4 * (vc - v);
    }

    // Planar case with 3 control points (9 unknowns)
  } else if (bary_coords.rows() == 3) {
    // Construct the 2Nx9 coefficient matrix
    proj_coeffs->resize(2 * num_points, 9);
    for (int i = 0; i < num_points; i++) {
      // Shorthands to make it easier to check coefficient matrix
      const double u = points2(0, i);
      const double v = points2(1, i);
      const double b1 = bary_coords(0, i);
      const double b2 = bary_coords(1, i);
      const double b3 = bary_coords(2, i);
      proj_coeffs->row(2 * i + 0) << b1 * fu, 0, b1 * (uc - u), b2 * fu, 0, b2 * (uc - u), b3 * fu,
          0, b3 * (uc - u);
      proj_coeffs->row(2 * i + 1) << 0, b1 * fv, b1 * (vc - v), 0, b2 * fv, b2 * (vc - v), 0,
          b3 * fv, b3 * (vc - v);
    }

    // Cases that are not solved with the current implementation.
  } else {
    return false;
  }

  // Solve the homogeneous linear problem by SVD
  // Matrix V is 9x9 (planar case) or 12x12 (non-planar case)
  // There are 9 (planar case) or 12 singular values  (non-planar case)
  Eigen::JacobiSVD<Eigen::MatrixXd> svd;
  svd.compute(*proj_coeffs, Eigen::ComputeThinU | Eigen::ComputeFullV);
  *singular_values = svd.singularValues();
  const MatrixXd& singular_vectors = svd.matrixV();

  // Output the last 4 columns of V in reverse order
  sol_basis->resize(singular_vectors.rows(), 4);
  for (int i = 0; i < 4; i++) {
    sol_basis->col(i) = singular_vectors.col(singular_vectors.cols() - i - 1);
  }

  // Put singular values in increasing order
  for (int i = 0; i < singular_values->size() / 2; i++) {
    std::swap((*singular_values)(i), (*singular_values)(singular_values->size() - 1 - i));
  }

  return true;
}

// Convert a solution vector of control point coordinates of length 3*C to a 3xC matrix for C=3,4
// such that the input vector contains the elements of the output matrix column-wise.
Matrix3Xd ReshapeToMatrix3xN(const VectorXd& vec) {
  Matrix3Xd matrix;
  if (vec.size() % 3) {
    return matrix;  // return a 3x0 matrix
  }
  matrix.resize(3, vec.size() / 3);
  for (int i = 0; i < matrix.cols(); i++) {
    matrix.col(i) = vec.block(3 * i, 0, 3, 1);
  }

  return matrix;
}

// Project points directly given in camera coordinates to the image
Matrix2Xd ProjectPoints(const Matrix3d& calib_matrix, const Matrix3Xd& points3) {
  Matrix2Xd points2;
  points2.resize(2, points3.cols());
  for (int i = 0; i < points3.cols(); i++) {
    Vector3d proj = calib_matrix * points3.col(i);
    if (proj(2) == 0) {
      // Avoids NaNs due to 0/0.
      points2(0, i) = Inf;
      points2(1, i) = Inf;
    } else {
      // Division by homogeneous coordinate.
      points2(0, i) = proj(0) / proj(2);
      points2(1, i) = proj(1) / proj(2);
    }
  }
  return points2;
}

// Compute distances between all possible pairs (i,j) of input points (in matrix columns).
VectorXd ComputeDistances(const MatrixXd& points) {
  int num_points = points.cols();
  if (num_points < 2) {
    return VectorXd(0);
  }

  VectorXd distances(num_points * (num_points - 1) / 2);
  int k = 0;
  for (int i = 0; i < points.cols(); i++) {
    for (int j = i + 1; j < points.cols(); j++) {
      distances[k++] = (points.col(i) - points.col(j)).norm();
    }
  }

  return distances;
}

// Compute the weights that correspond to a concrete solution for the control point camera
// coordinates in the basis given as input.
Vector4d SolveControlPoints(const MatrixXd& sol_basis, int sol_dims, const VectorXd& distances) {
  Vector4d weights = Vector4d::Zero();
  if (sol_basis.cols() != 4) {
    return weights;
  }
  if (sol_basis.rows() != 12 && sol_basis.rows() != 9) {
    return weights;
  }

  // non-planar case
  if (sol_basis.rows() == 12 && distances.size() != 6) {
    return weights;
  }

  // planar case
  if (sol_basis.rows() == 9 && distances.size() != 3) {
    return weights;
  }

  // Planar and non-planar case for 1-dimensional solution space
  if (sol_dims == 1) {
    // Only use singular vector corresponding to the least singular value.
    // Solution space is 1-dimensional -> only need to find a single global scale.

    // Calculate the unscaled camera coordinates of the control points.
    Matrix3Xd v = ReshapeToMatrix3xN(sol_basis.col(0));

    // Calculate distances between the unscaled control points.
    VectorXd distances_cam = ComputeDistances(v);

    // Calculate the unknown scale from the distances between control points in the world frame
    // and the corresponding distances in the camera frame.
    double scale = distances.dot(distances_cam) / distances_cam.dot(distances_cam);

    // Find sign of the scale such that a majority of control points are in front of the camera.
    int num_points_behind = (v.row(2).array() < 0).count();
    if (num_points_behind > 1) {
      scale = -scale;
    }

    weights(0) = scale;

    // Planar and non-planar for 2-dimensional solution space
  } else if (sol_dims == 2) {
    // Reshape the two basis vectors of the solution space into two 3xN matrices.
    // The sought matrix ctl_points_cam is a linear combination of these matrices.
    // ctl_points_cam = w1*v1 + w2*v2, where the weights w1 and w2 are still unknown.
    Matrix3Xd v1 = ReshapeToMatrix3xN(sol_basis.col(0));
    Matrix3Xd v2 = ReshapeToMatrix3xN(sol_basis.col(1));

    // Looking for the linear combination preserving the distances between control points.
    // This is a non-linear problem in the linear combination weights (w1,w2).
    // Strategy:
    //   (Step1) Solve for (w11,w22,w12) = (w1*w1, w2*w2, w1*w2) (linearized problem)
    //   (Step2) Extract (w1,w2) from (w11,w22,w12).
    // Both steps are over-determined problems.

    // Construct coefficient matrix of linearized problem (see Eq.13 in the paper).
    // This matrix is 6x4 in the non-planar case.
    const int num_points = v1.cols();
    const int num_equations = num_points * (num_points - 1) / 2;
    int k = 0;
    MatrixXd coeff_matrix(num_equations, 3);
    for (int i = 0; i < num_points; i++) {
      for (int j = i + 1; j < num_points; j++) {
        Vector3d d1 = v1.col(i) - v1.col(j);
        Vector3d d2 = v2.col(i) - v2.col(j);
        coeff_matrix.row(k++) << d1.dot(d1), d2.dot(d2), 2 * d1.dot(d2);
      }
    }

    // Step1: Compute least squares solution to the linearized problem.
    VectorXd squared_distances = distances.cwiseProduct(distances);
    VectorXd lin_sol =
        coeff_matrix.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(squared_distances);

    // Solution of the linearized problem.
    const double w11 = lin_sol(0);
    const double w22 = lin_sol(1);
    const double w12 = lin_sol(2);

    // Step2: Calculate weights w1 and w2 (denoted beta_i in the EPnP paper)
    // from the non-linear constraints (w11,w22,w12) = (w1*w1, w2*w2, w1*w2).
    // There are always two solutions, (w1,w2) and (-w1,-w2) -> sign is solved further below.
    // Over-constrained quadratic problem - least-squares solution has no simple closed form.
    // Empirical tests show that solution order largely affects pose accuracy.
    // Strategy: we exactly match the larger magnitude wij and hope for less overall alg. error.
    double w1, w2;
    w1 = w2 = 0;
    if (w11 >= w22 && w11 > 0) {
      w1 = std::sqrt(w11);
      if (w22 > std::abs(w12)) {
        w2 = std::sqrt(w22);
      } else {
        w2 = w12 / w1;
      }
    } else if (w11 <= w22 && w22 > 0) {
      w2 = std::sqrt(w22);
      if (w11 > std::abs(w12)) {
        w1 = std::sqrt(w11);
      } else {
        w1 = w12 / w2;
      }
    }

    // There are always two solutions: (w1,w2) and (-w1,-w2) but
    // one corresponds to control points behind the camera.
    // Find sign such that a majority all control points are in front of the camera.
    VectorXd ctl_point_depths = w1 * v1.row(2) + w2 * v2.row(2);
    int num_points_behind = (ctl_point_depths.array() < 0).count();
    if (num_points_behind > int{num_points / 2}) {
      weights << -w1, -w2, 0, 0;
    } else {
      weights << w1, w2, 0, 0;
    }

    // Non-planar case for 2-dimensional solution space
  } else if (sol_dims == 3 && sol_basis.rows() == 12) {
    // Reshape the 3 basis vectors of the solution space into three 3xN matrices.
    // The sought matrix ctl_points_cam is a linear combination of these matrices.
    // ctl_points_cam = w1*v1 + w2*v2 + w3*v3, where the weights (w1,w2,w3) are still unknown.
    Matrix3Xd v1 = ReshapeToMatrix3xN(sol_basis.col(0));
    Matrix3Xd v2 = ReshapeToMatrix3xN(sol_basis.col(1));
    Matrix3Xd v3 = ReshapeToMatrix3xN(sol_basis.col(2));

    // Looking for the linear combination preserving the distances between control points.
    // This is a non-linear problem in the linear combination weights (w1,w2,w3).
    // Strategy:
    //   (Step1) Solve for (w11,w22,w33,w12,w13,w23) = (w1*w1, w2*w2, w3*w3, w1*w2, w1*w3, w2*w3)
    //           (linearized problem).
    //   (Step2) Extract (w1,w2,w3) from (w11,w22,w33,w12,w13,w23).

    // Step1: Construct coefficient matrix of linearized problem (Case N=3 in the paper).
    // This matrix is 6x6 in the non-planar case.
    const int num_points = v1.cols();
    const int num_equations = num_points * (num_points - 1) / 2;
    int k = 0;
    MatrixXd coeff_matrix(num_equations, 6);
    for (int i = 0; i < num_points; i++) {
      for (int j = i + 1; j < num_points; j++) {
        Vector3d d1 = v1.col(i) - v1.col(j);
        Vector3d d2 = v2.col(i) - v2.col(j);
        Vector3d d3 = v3.col(i) - v3.col(j);
        coeff_matrix.row(k++) << d1.dot(d1), d2.dot(d2), d3.dot(d3), 2 * d1.dot(d2), 2 * d1.dot(d3),
            2 * d2.dot(d3);
      }
    }

    // Step1: Compute least squares solution to the linearized problem.
    VectorXd squared_distances = distances.cwiseProduct(distances);
    VectorXd lin_sol =
        coeff_matrix.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(squared_distances);

    // Solution of the linearized problem.
    const double w11 = lin_sol(0);
    const double w22 = lin_sol(1);
    const double w33 = lin_sol(2);
    const double w12 = lin_sol(3);
    const double w13 = lin_sol(4);
    const double w23 = lin_sol(5);

    // Calculate weights w1, w2, w3 from the non-linear constraints
    // (w11, w22, w33, w12, w13, w23) = (w1*w1, w2*w2, w3*w3, w1*w2, w1*w3, w2*w3).
    // There are always two solutions (w1,w2,w3) and (-w1,-w2,-w3) -> sign solved further below.
    // Over-constrained quadratic problem - leats-squares solution has no simple closed form.
    // Empirical tests show that solution order largely affects pose accuracy.
    // Strategy: we exactly match the larger magnitude wij and hope for less overall alg. error.
    double w1 = 0, w2 = 0, w3 = 0;
    if (w11 > w22 && w11 > w33 && w11 > 0) {
      w1 = std::sqrt(w11);
      if (w22 > std::abs(w12)) {
        w2 = std::sqrt(w22);
      } else {
        w2 = w12 / w1;
      }
      if (w33 > std::abs(w13)) {
        w3 = std::sqrt(w33);
      } else {
        w3 = w13 / w1;
      }
    } else if (w22 > w11 && w22 > w33 && w22 > 0) {
      w2 = std::sqrt(w22);
      if (w11 > std::abs(w12)) {
        w1 = std::sqrt(w11);
      } else {
        w1 = w12 / w2;
      }
      if (w33 > std::abs(w23)) {
        w3 = std::sqrt(w33);
      } else {
        w3 = w23 / w2;
      }
    } else if (w33 > w11 && w33 > w22 && w33 > 0) {
      w3 = std::sqrt(w33);
      if (w11 > std::abs(w13)) {
        w1 = std::sqrt(w11);
      } else {
        w1 = w13 / w3;
      }
      if (w22 > std::abs(w23)) {
        w2 = std::sqrt(w22);
      } else {
        w2 = w23 / w3;
      }
    }

    // There are always two solutions: (w1,w2,w3) and (-w1,-w2,-w3) but
    // one corresponds to control points behind the camera.
    // Find sign such that a majority all control points are in front of the camera.
    VectorXd ctl_point_depths = w1 * v1.row(2) + w2 * v2.row(2) + w3 * v3.row(2);
    int num_points_behind = (ctl_point_depths.array() < 0).count();
    if (num_points_behind > int{num_points / 2}) {
      weights << -w1, -w2, -w3, 0;
    } else {
      weights << w1, w2, w3, 0;
    }
  }

  // TODO: planar N=3 case using relinearization
  // TODO: non-planar N=4 (affine camera) case using relinearization
  // planar N=4 case is underconstrained = ambiguous (3 equations but 4 unknowns)

  return weights;
}

}  // namespace epnp
}  // namespace pnp
}  // namespace isaac


/**
 * TSAI 1999 Hand-eye calibration
 * Emanuele Ruffaldi 2016
 *
 * Copyright Scuola Superiore Sant'Anna (2016)
 * Emanuele Ruffaldi, e.ruffaldi@sssup.it
 */
#pragma once

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

template <class T>
 using Vector3 = Eigen::Matrix<T,3,1>;

template <class T>
 using Matrix3 = Eigen::Matrix<T,3,3>;

template <class T>
 using Matrix4 = Eigen::Matrix<T,4,4>;

/**
 * Computes Tsai calibration: four frames
 *	 R = robot root
 * 	 E = robot end-effector
 *   C = camera
 *   M = marker
 *
 * The objective is EC
 * We move E wrt R and keep R and M fixed each other
 */

float calibrationTsai(const std::vector<Eigen::Matrix4f> & cMm,
                     const std::vector<Eigen::Matrix4f> & rMe,
                     Eigen::Matrix4f &eMc, bool computeResidual = false);

double calibrationTsai(const std::vector<Eigen::Matrix4d> & cMm,
                     const std::vector<Eigen::Matrix4d> & rMe,
                     Eigen::Matrix4d &eMc, bool computeResidual = false);

/*
 * These are internal functions used for testing the Tsai
 * They mainly deal with the specific Tsai parametrization of the vector that is more robust to small values than the regular one
 */
template <class T>
Eigen::Matrix<T,3,1> paratsaiprime2paratsai(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Matrix<T,3,1> paratsai2paratsaiprime(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Matrix<T,3,3> paratsai2rot(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Quaternion<T> paratsai2quat(const Eigen::Matrix<T,3,1> & a);
template <class T>
Eigen::Matrix<T,3,1> quat2paratsai(const Eigen::Quaternion<T> & q);
template <class T>
T paratsaiprime2theta(const Eigen::Matrix<T,3,1> & x);
template <class T>
T paratsai2theta(const Eigen::Matrix<T,3,1> & x);

/// returns the skew of the vector
template <class T>
inline Matrix3<T> skewtsai(const Vector3<T> & a)
{
  Matrix3<T> r;
  r  << 0, -a(2), a(1),  a(2),0,-a(0),-a(1),a(0),0;
  return r;
}

template <class T>
Vector3<T> paratsaiprime2paratsai(const Vector3<T> & a)
{
    return 2*a/sqrt(1+a.squaredNorm()); // equation 14
}

template <class T>
Vector3<T> paratsai2paratsaiprime(const Vector3<T> & a)
{
  return a/sqrt(4-a.squaredNorm());
}

/// converts the parametric in tsai form to rotation
/// NO NEED OF TRIGONOMETRICS
/// equation 10
template <class T>
Matrix3<T> paratsai2rot(const Vector3<T> & a)
{
  T sn = a.squaredNorm();
  T alpha = sqrt(4-sn);
  return (1-sn/2)*Matrix3<T>::Identity() + 0.5*(a*a.transpose() + alpha * skewtsai(a));
}

/// converts the parametric in tsai form to rotation
/// NO NEED OF TRIGONOMETRICS
/// equation 9 modified NOT USED
template <class T>
Eigen::Quaternion<T> paratsai2quat(const Vector3<T> & a)
{
  double sinthetahalf = a.norm()/2; // 2 * sin(theta)/2 => sin(theta)/2
  return Eigen::Quaternion<T>(sqrt(1-sinthetahalf*sinthetahalf),a.x()/2,a.y()/2,a.z()/2); // cos(theta/2)
}

template <class T>
T paratsai2theta(const Vector3<T> & x)
{
  return 2*asin(x.norm()/2); // eq. 9
}


#include<iostream>
#include <Eigen/Dense>
#include <math.h>
#include <vector>
#include <Eigen/QR>

using namespace std;
Eigen::MatrixXf pseudoinverse(Eigen::MatrixXf m)
{
    //Eigen::Matrix<float,2,3> m;
  //  m<<0.68,0.597,-0.211, 0.823,0.566,-0.605;
   // m<<1,2,3,4,5,6;
    cout<<m<<endl;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd =m.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
   // Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
    const Eigen::MatrixXf singularValues = svd.singularValues();
	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> singularValuesInv(m.cols(), m.rows());
	singularValuesInv.setZero();
	double  pinvtoler = 1.e-6; // choose your tolerance wisely
	for (unsigned int i = 0; i < singularValues.size(); ++i) {
	 	if (singularValues(i) > pinvtoler)
	 		singularValuesInv(i, i) = 1.0f / singularValues(i);
	 	else
	 		singularValuesInv(i, i) = 0.f;
	 }
    Eigen::MatrixXf pinvmat = svd.matrixV() * singularValuesInv * svd.matrixU().transpose();
    std::cout << pinvmat << std::endl;
	return pinvmat;
}
int main(int argc, char **argv){
      Eigen::Matrix<double, 6, 1> error;
      for(int i=0; i<6;i++)
      for (int j = 0; j < 1; j++)
      {
          error(i,j)=1;
          std::cout<<error(i,j);
      }
    cout<<endl;
    std::vector<int>ve;
    ve.push_back(1);
    ve.push_back(2);
    cout<<ve.data()<<endl;
    Eigen::Vector4f ve4(1,2,3,4);
    std::cout<<error.cols()<<error.rows()<<endl;
    cout<<ve4<<endl;

    Eigen::MatrixXf m(2,2);
    m(0,0) = 3;
    m(1,0) = 2.5;
    m(0,1) = -1;
    m(1,1) = m(1,0) + m(0,1);

    array<double,4>arr={2,6,7.11,8.23};

    Eigen::Map<const Eigen::Matrix<double, 2, 2>>m2(arr.data());
    cout<<m2<<endl;
    // m2<<1,2,3,4;
    // cout<<m2<<endl;
    //pseudoinverse();
    // Eigen::Matrix<float, 6, 7> name=Eigen::Matrix<float,6,7>::Random();
    // Eigen::Matrix<float,3,7> res;
    // for (int i = 0; i < res.rows(); i++)
    // {
    //     for (int j = 0; j < res.cols(); j++)
    //     {
    //             res(i,j)=name(i,j);
    //     }
    // }
    // cout<<res;

//    Eigen::Matrix<float,2,3> m2;
//    m2<<0.68,0.597,-0.211, 0.823,0.566,-0.605;
//    m2<<1,2,3,4,5,6;
//    cout<<m2;

  //cout<<(pseudoinverse(res));

}


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

#ifndef QR_HPP
#define QR_HPP

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

// QR decomposition using Gram-Schmit orthogonalization

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> bool qr(const ub::matrix<T>& in, ub::matrix<T>& q, ub::matrix<T>& r)
{
	int i, j, k;
	int n;
	ub::vector<T> v;
	T tmp;

	n = in.size1();
	if (n != in.size2()) return false;

	q.resize(n, n);
	r.resize(n, n);
	v.resize(n);

	for (i=0; i<n; i++) {
		for (k=0; k<n; k++) v(k) = in(k, i);
		for (j=0; j<i; j++) {
			tmp = 0.;
			for (k=0; k<n; k++) {
				tmp += in(k, i) * q(k, j);
			}
			for (k=0; k<n; k++) {
				v(k) -= tmp * q(k, j);
			}
			r(j, i) = tmp;
		}
		tmp = norm_2(v);
		if (tmp == 0.) return false;
		for (k=0; k<n; k++) q(k, i) = v(k) / tmp;
		r(i, i) = tmp;
		for (j=i+1; j<n; j++) r(j, i) = 0.;
	}

	return true;
}

} // namespace kv

#endif // QR_HPP


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core_math

#ifndef openOR_core_Math_detail_inverse_hpp
#define openOR_core_Math_detail_inverse_hpp

#include <boost/numeric/ublas/lu.hpp>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/void.hpp>
#include <boost/type_traits/is_same.hpp>

#include <openOR/Log/Logger.hpp>
#include <openOR/Math/matrix.hpp>

#include <openOR/Math/determinant.hpp>

namespace openOR {
	namespace Math {

		struct no_inverse_matrix_error : public std::runtime_error {
			no_inverse_matrix_error() : std::runtime_error("Could not get matrix inverse") {
				LOG(Log::Level::Debug, Log::noAttribs, Log::msg("Exception thrown: Could not get matrix inverse") );
			}
		};

		namespace Impl {

			template <class Type, int Dim>
			struct Inverse {
				static Type get(const Type& mat) {
					// function has to be specialized
					BOOST_MPL_ASSERT((boost::is_same<Type, boost::mpl::void_>));
				}
			};

			template <class Type>
			struct Inverse<Type, 2> {
				static Type get(const Type& mat) {
					Type matResult;
					Math::get<0, 0>(matResult) = Math::get<1, 1>(mat);
					Math::get<0, 1>(matResult) = -Math::get<0, 1>(mat);
					Math::get<1, 0>(matResult) = -Math::get<1, 0>(mat);
					Math::get<1, 1>(matResult) = Math::get<0, 0>(mat);
					matResult /= Math::determinant(mat);
					return matResult;
				}
			};

			template <class Type>
			struct Inverse<Type, 3> {
				static Type get(const Type& mat) {
					Type matResult;
					Math::get<0, 0>(matResult) = Math::get<1, 1>(mat) * Math::get<2, 2>(mat) - Math::get<1, 2>(mat) * Math::get<2, 1>(mat);
					Math::get<0, 1>(matResult) = Math::get<0, 2>(mat) * Math::get<2, 1>(mat) - Math::get<0, 1>(mat) * Math::get<2, 2>(mat);
					Math::get<0, 2>(matResult) = Math::get<0, 1>(mat) * Math::get<1, 2>(mat) - Math::get<0, 2>(mat) * Math::get<1, 1>(mat);

					Math::get<1, 0>(matResult) = Math::get<1, 2>(mat) * Math::get<2, 0>(mat) - Math::get<1, 0>(mat) * Math::get<2, 2>(mat);
					Math::get<1, 1>(matResult) = Math::get<0, 0>(mat) * Math::get<2, 2>(mat) - Math::get<0, 2>(mat) * Math::get<2, 0>(mat);
					Math::get<1, 2>(matResult) = Math::get<0, 2>(mat) * Math::get<1, 0>(mat) - Math::get<0, 0>(mat) * Math::get<1, 2>(mat);

					Math::get<2, 0>(matResult) = Math::get<1, 0>(mat) * Math::get<2, 1>(mat) - Math::get<1, 1>(mat) * Math::get<2, 0>(mat);
					Math::get<2, 1>(matResult) = Math::get<0, 1>(mat) * Math::get<2, 0>(mat) - Math::get<0, 0>(mat) * Math::get<2, 1>(mat);
					Math::get<2, 2>(matResult) = Math::get<0, 0>(mat) * Math::get<1, 1>(mat) - Math::get<0, 1>(mat) * Math::get<1, 0>(mat);

					matResult /= Math::determinant(mat);
					return matResult;
				}
			};

			template <class Type>
			struct Inverse<Type, 4> {
				static Type get(const Type& mat) {
					Type inverse;

					using namespace boost::numeric::ublas;
					typedef permutation_matrix<std::size_t> pmatrix;

					// create a working copy of the input
					Type A(mat);

					// create a permutation matrix for the LU-factorization
					pmatrix pm(A.size1());

					// perform LU-factorization
					int res = lu_factorize(A,pm);
					if( res != 0 ) { throw no_inverse_matrix_error(); }

					// create identity matrix of "inverse"
					inverse.assign( Math::MatrixTraits<Type>::IDENTITY );

					// backsubstitute to get the inverse
					lu_substitute(A, pm, inverse);

					return inverse;
				}
			};

		}
	}
}

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Regular_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Regular_triangulation_2.h>
#include <CGAL/internal/boost/function_property_map.hpp>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <fstream>
#include <iostream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel     K;
typedef K::FT                                                   FT;
typedef K::Weighted_point_2                                     Weighted_point;

typedef CGAL::Regular_triangulation_2<K>                        Triangulation;

typedef boost::graph_traits<Triangulation>::vertex_descriptor   vertex_descriptor;
typedef boost::graph_traits<Triangulation>::vertex_iterator     vertex_iterator;
typedef boost::graph_traits<Triangulation>::edge_descriptor     edge_descriptor;

template <typename T>
struct Compute_edge_weight
{
  const T& t_;

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

  typedef typename boost::graph_traits<T>::vertex_descriptor vertex_descriptor;
  typedef typename boost::graph_traits<T>::edge_descriptor   edge_descriptor;

  FT operator()(const edge_descriptor ed) const {
    vertex_descriptor svd = source(ed, t_);
    vertex_descriptor tvd = target(ed, t_);
    typename T::Vertex_handle sv = svd;
    typename T::Vertex_handle tv = tvd;
    return CGAL::power_product(sv->point(), tv->point());
  }
};

// The BGL makes use of indices associated to the vertices
// We use a std::map to store the index
typedef std::map<vertex_descriptor,int> VertexIndexMap;

// A std::map is not a property map, because it is not lightweight
typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;

int main(int argc,char* argv[])
{
  const char* filename = (argc > 1) ? argv[1] : "data/weighted_points.xyw";
  std::ifstream input(filename);
  Triangulation tr;

  Weighted_point wp;
  while(input >> wp)
    tr.insert(wp);

  // Note that with the input "data/weighted_points.xyw", there is one hidden vertex
  std::cout << "number of hidden vertices: " << tr.number_of_hidden_vertices() << std::endl;

  // Associate indices to the vertices
  VertexIndexMap vertex_id_map;
  VertexIdPropertyMap vertex_index_pmap(vertex_id_map);
  int index = 0;

  for(vertex_descriptor vd : vertices(tr))
    vertex_id_map[vd]= index++;

  // We use a custom edge length property map that computes the power distance
  // between the extremities of an edge of the regular triangulation
  typedef Compute_edge_weight<Triangulation> Edge_weight_functor;

  // In the function call you can see a named parameter: vertex_index_map
  std::list<edge_descriptor> mst;
  boost::kruskal_minimum_spanning_tree(tr, std::back_inserter(mst),
                                       vertex_index_map(vertex_index_pmap)
                                       .weight_map(CGAL::internal::boost_::make_function_property_map<
                                          edge_descriptor, FT, Edge_weight_functor>(Edge_weight_functor(tr))));

  std::cout << "The edges of the Euclidean mimimum spanning tree:" << std::endl;
  for(edge_descriptor ed : mst)
  {
    vertex_descriptor svd = source(ed, tr);
    vertex_descriptor tvd = target(ed, tr);
    Triangulation::Vertex_handle sv = svd;
    Triangulation::Vertex_handle tv = tvd;
    std::cout << "[ " << sv->point() << "  |  " << tv->point() << " ] " << std::endl;
  }

  return EXIT_SUCCESS;
}


/**
 * \file Chebyshev1Filter.hxx
 */

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/asinh.hpp>

#include <ATK/EQ/Chebyshev1Filter.h>
#include <ATK/EQ/helpers.h>

namespace Chebyshev1Utilities
{
  template<typename DataType>
  void create_chebyshev1_analog_coefficients(int order, DataType ripple, EQUtilities::ZPK<DataType>& zpk)
  {
    zpk.z.clear(); // no zeros for this filter type
    zpk.p.clear();
    if(ripple == 0)
    {
      return;
    }
    if(order == 0)
    {
      zpk.k = static_cast<DataType>(std::pow(10, (-ripple / 20)));
      return;
    }

    DataType eps = static_cast<DataType>(std::sqrt(std::pow(10, (0.1 * ripple)) - 1.0));
    DataType mu = static_cast<DataType>(1.0 / order * boost::math::asinh(1 / eps));

    for(gsl::index i = -order+1; i < order; i += 2)
    {
      DataType theta = boost::math::constants::pi<DataType>() * i / (2*order);
      zpk.p.push_back(-std::sinh(std::complex<DataType>(mu, theta)));
    }

    std::complex<DataType> f = 1;

    for(gsl::index i = 0; i < zpk.p.size(); ++i)
    {
      f *= -zpk.p[i];
    }
    zpk.k = f.real();
    if(order % 2 == 0)
    {
      zpk.k = zpk.k / std::sqrt(1 + eps * eps);
    }
  }

  template<typename DataType, typename Container>
  void create_default_chebyshev1_coeffs(size_t order, DataType ripple, DataType Wn, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order), ripple, zpk);
    EQUtilities::populate_lp_coeffs(Wn, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bp_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bp_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bs_chebyshev1_coeffs(size_t order, DataType ripple, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_chebyshev1_analog_coefficients(static_cast<int>(order/2), ripple, zpk);
    EQUtilities::populate_bs_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }
}

namespace ATK
{
  template <typename DataType>
  Chebyshev1LowPassCoefficients<DataType>::Chebyshev1LowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1LowPassCoefficients<DataType_>::CoeffDataType Chebyshev1LowPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType_>
  void Chebyshev1LowPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1LowPassCoefficients<DataType_>::CoeffDataType Chebyshev1LowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1LowPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev1LowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_default_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1HighPassCoefficients<DataType>::Chebyshev1HighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1HighPassCoefficients<DataType_>::CoeffDataType Chebyshev1HighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType_>
  void Chebyshev1HighPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1HighPassCoefficients<DataType_>::CoeffDataType Chebyshev1HighPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1HighPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void Chebyshev1HighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_default_chebyshev1_coeffs(in_order, ripple, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(gsl::index i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  Chebyshev1BandPassCoefficients<DataType>::Chebyshev1BandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev1BandPassCoefficients<DataType_>::CoeffDataType, typename Chebyshev1BandPassCoefficients<DataType_>::CoeffDataType> Chebyshev1BandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandPassCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1BandPassCoefficients<DataType_>::CoeffDataType Chebyshev1BandPassCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1BandPassCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev1BandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_bp_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  Chebyshev1BandStopCoefficients<DataType>::Chebyshev1BandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename Chebyshev1BandStopCoefficients<DataType_>::CoeffDataType, typename Chebyshev1BandStopCoefficients<DataType_>::CoeffDataType> Chebyshev1BandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType_>
  void Chebyshev1BandStopCoefficients<DataType_>::set_ripple(CoeffDataType ripple)
  {
    this->ripple = ripple;
    setup();
  }

  template <typename DataType_>
  typename Chebyshev1BandStopCoefficients<DataType_>::CoeffDataType Chebyshev1BandStopCoefficients<DataType_>::get_ripple() const
  {
    return ripple;
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int Chebyshev1BandStopCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void Chebyshev1BandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    Chebyshev1Utilities::create_bs_chebyshev1_coeffs(in_order, ripple, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }
}


#ifndef CONSTANTS_HPP_
#define CONSTANTS_HPP_

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions.hpp>
#include <complex> // constant::i
#include <limits> // constant::omega
#include <string> // constant::omega

namespace constant {

template<typename T> T alladi_grinstead() {
	T c = 0, last;

	for(std::size_t n = 1; n == 1 || c != last; ++n) {
		last = c;
		c += (boost::math::zeta<T>(n + 1) - 1) / n;
	}

	static const T ALLADI_GRINSTEAD = exp(c - 1);
	return ALLADI_GRINSTEAD;
}

template<typename T> T aperys() {
	static const T APERYS = boost::math::constants::zeta_three<T>();
	return APERYS;
}

template<typename T> T buffons() {
	static const T BUFFONS = 2 / boost::math::constants::pi<T>();
	return BUFFONS;
}

template<typename T> T catalans() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		CATALANS = 0.125 * (boost::math::trigamma<T>(0.25) - PI_SQR);
	return CATALANS;
}

template<typename T> T delian() {
	static const T DELIAN = boost::math::cbrt<T>(2);
	return DELIAN;
}

template<typename T> T e() {
	static const T E = boost::math::constants::e<T>();
	return E;
}

template<typename T> T erdos_borwein() {
	T e = 0, last;

	for(std::size_t n = 1; n == 1 || e != last; ++n) {
		last = e;
		e += 1 / (exp2(static_cast<T>(n)) - 1);
	}

	static const T ERDOS_BORWEIN = e;
	return ERDOS_BORWEIN;
}

template<typename T> T euler_mascheroni() {
	static const T EULER_MASCHERONI = boost::math::constants::euler<T>();
	return EULER_MASCHERONI;
}

template<typename T> T gauss() {
	static const T ROOT_TWO = boost::math::constants::root_two<T>(),
		PI = boost::math::constants::pi<T>(),
		GAUSS = pow(boost::math::tgamma<T>(0.25), 2)
			/ (2 * ROOT_TWO * pow(PI, static_cast<T>(1.5)));
	return GAUSS;
}

template<typename T> T gelfond_schneider() {
	static const T ROOT_TWO = boost::math::constants::root_two<T>(),
		GELFOND_SCHNEIDER = exp2(ROOT_TWO);
	return GELFOND_SCHNEIDER;
}

template<typename T> T gelfonds() {
	static const T GELFONDS = boost::math::constants::e_pow_pi<T>();
	return GELFONDS;
}

template<typename T> T giesekings() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		GIESEKINGS = (9 - boost::math::trigamma<T>(2 / static_cast<T>(3))
			+ boost::math::trigamma<T>(4 / static_cast<T>(3)))
			/ (4 * ROOT_THREE);
	return GIESEKINGS;
}

template<typename T> T glaisher_kinkelin() {
	static const T GLAISHER_KINKELIN = boost::math::constants::glaisher<T>();
	return GLAISHER_KINKELIN;
}

template<typename T> T golden_ratio() {
	static const T GOLDEN_RATIO = boost::math::constants::phi<T>();
	return GOLDEN_RATIO;
}

template<typename T> std::complex<T> i() {
	static const std::complex<T> I(0, 1);
	return I;
}

template<typename T> T inverse_golden_ratio() {
	static const T INVERSE_GOLDEN_RATIO = boost::math::constants::phi<T>() - 1;
	return INVERSE_GOLDEN_RATIO;
}

template<typename T> T khinchin() {
	static const T KHINCHIN = boost::math::constants::khinchin<T>();
	return KHINCHIN;
}

// Not Levy constant
template<typename T> T khinchin_levy() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		LN_TWO = boost::math::constants::ln_two<T>(),
		KHINCHIN_LEVY = PI_SQR / (12 * LN_TWO);
	return KHINCHIN_LEVY;
}

// Not Glaisher-Kinkelin constant
template<typename T> T kinkelin() {
	static const T GLAISHER_KINKELIN = boost::math::constants::glaisher<T>(),
		KINKELIN = 1 / static_cast<T>(12) - log(GLAISHER_KINKELIN);
	return KINKELIN;
}

template<typename T> T knuth() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		KNUTH = (1 - (1 / ROOT_THREE)) / 2;
	return KNUTH;
}

template<typename T> T levys() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		LN_TWO = boost::math::constants::ln_two<T>(),
		LEVYS = exp(PI_SQR / (12 * LN_TWO));
	return LEVYS;
}

template<typename T> T liebs() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		LIEBS = (8 * ROOT_THREE) / 9;
	return LIEBS;
}

template<typename T> T lochs() {
	static const T LN_TWO = boost::math::constants::ln_two<T>(),
		LN_TEN = boost::math::constants::ln_ten<T>(),
		PI_SQR = boost::math::constants::pi_sqr<T>(),
		LOCHS = (6 * LN_TWO * LN_TEN) / PI_SQR;
	return LOCHS;
}

template<typename T> T niven() {
	T c = 1, last;

	for(std::size_t j = 2; j == 2 || c != last; ++j) {
		last = c;
		c += 1 - 1/boost::math::zeta<T>(j);
	}

	static const T NIVEN = c;
	return NIVEN;
}

template<typename T> T nortons() {
	static const T PI_SQR = boost::math::constants::pi_sqr<T>(),
		LN_PI = log(boost::math::constants::pi<T>()),
		EULER = boost::math::constants::euler<T>(),
		LN_GLAISHER_KINKELIN = log(boost::math::constants::glaisher<T>()),
		LN_TWO = boost::math::constants::ln_two<T>(),
		NORTONS = (6 * LN_TWO
			* (24 * LN_GLAISHER_KINKELIN - 3 + 2 * EULER + LN_TWO - 2 * LN_PI)
			- PI_SQR) / PI_SQR;
	return NORTONS;
}

template<typename T> T omega() {
	T w = 0;
	std::string w_pre, w_post;

	w_pre.reserve(std::numeric_limits<T>::digits10);
	w_post.reserve(std::numeric_limits<T>::digits10);

	do {
		w_pre = static_cast<std::string>(w);
		w_pre.resize(std::numeric_limits<T>::digits10);

		const T e_w = exp(w);

		w -= ((w * e_w) - 1)
			/ (e_w * (w + 1) - ((w + 2) * (w * e_w - 1) / ((w * 2) + 2)));

		w_post = static_cast<std::string>(w);
		w_post.resize(std::numeric_limits<T>::digits10);
	} while (w_pre != w_post);

	static const T OMEGA = w;
	return OMEGA;
}

template<typename T> T one() {
	static const T ONE = 1;
	return ONE;
}

template<typename T> T pi() {
	static const T PI = boost::math::constants::pi<T>();
	return PI;
}

template<typename T> T plastic_number() {
	static const T PLASTIC_NUMBER = (boost::math::cbrt<T>(108 + 12
		* sqrt(static_cast<T>(69))) + boost::math::cbrt<T>(108 - 12
		* sqrt(static_cast<T>(69)))) / static_cast<T>(6);
	return PLASTIC_NUMBER;
}

template<typename T> T pogsons() {
	static const T POGSONS = pow(10, 2 / static_cast<T>(5));
	return POGSONS;
}

template<typename T> T polyas_random_walk() {
	static const T PI_CUBED = boost::math::constants::pi_cubed<T>(),
		ROOT_SIX = sqrt(static_cast<T>(6)),
		POLYAS_RANDOM_WALK = 1 - 1/((ROOT_SIX / (32 * PI_CUBED))
			* boost::math::tgamma<T>(1 / static_cast<T>(24))
			* boost::math::tgamma<T>(5 / static_cast<T>(24))
			* boost::math::tgamma<T>(7 / static_cast<T>(24))
			* boost::math::tgamma<T>(11 / static_cast<T>(24)));
	return POLYAS_RANDOM_WALK;
}

template<typename T> T porters() {
	static const T LN_TWO = boost::math::constants::ln_two<T>(),
		LN_GLAISHER_KINKELIN = log(boost::math::constants::glaisher<T>()),
		LN_PI = log(boost::math::constants::pi<T>()),
		PI_SQR = boost::math::constants::pi_sqr<T>(),
		PORTERS = (6 * LN_TWO * (48 * LN_GLAISHER_KINKELIN
			- LN_TWO - 4 * LN_PI - 2)) / PI_SQR - 0.5;
	return PORTERS;
}

template<typename T> T prince_ruperts_cube() {
	static const T ROOT_TWO = boost::math::constants::root_two<T>(),
		PRINCE_RUPERTS_CUBE = (3 * ROOT_TWO) / 4;
	return PRINCE_RUPERTS_CUBE;
}

template<typename T> T pythagoras() {
	static const T PYTHAGORAS = boost::math::constants::root_two<T>();
	return PYTHAGORAS;
}

template<typename T> T robbins() {
	static const T PI = boost::math::constants::pi<T>(),
		ROOT_TWO = boost::math::constants::root_two<T>(),
		ROOT_THREE = boost::math::constants::root_three<T>(),
		ROBBINS = ((4 + 17 * ROOT_TWO - 6 * ROOT_THREE - 7 * PI) / 105)
			+ (log1p(ROOT_TWO) / 5) + ((2 * log(2 + ROOT_THREE)) / 5);
	return ROBBINS;
}

template<typename T> T sierpinski_k() {
	static const T PI = boost::math::constants::pi<T>(),
		PI_CUBED = boost::math::constants::pi_cubed<T>(),
		EULER = boost::math::constants::euler<T>(),
		SIERPINSKI_K = PI * log((4 * PI_CUBED * exp(2 * EULER))
			/ pow(boost::math::tgamma<T>(0.25), 4));
	return SIERPINSKI_K;
}

template<typename T> T sierpinski_s() {
	static const T PI_CUBED = boost::math::constants::pi_cubed<T>(),
		EULER = boost::math::constants::euler<T>(),
		SIERPINSKI_S = log((4 * PI_CUBED * exp(2 * EULER))
			/ pow(boost::math::tgamma<T>(0.25), 4));
	return SIERPINSKI_S;
}

template<typename T> T silver_ratio() {
	static const T SILVER_RATIO = boost::math::constants::root_two<T>() + 1;
	return SILVER_RATIO;
}

template<typename T> T theodorus() {
	static const T THEODORUS = boost::math::constants::root_three<T>();
	return THEODORUS;
}

template<typename T> T twenty_vertex_entropy() {
	static const T ROOT_THREE = boost::math::constants::root_three<T>(),
		TWENTY_VERTEX_ENTROPY = (3 * ROOT_THREE) / 2;
	return TWENTY_VERTEX_ENTROPY;
}

template<typename T> T weierstrass() {
	static const T ROOT_PI = boost::math::constants::root_pi<T>(),
		PI = boost::math::constants::pi<T>(),
		WEIERSTRASS = (exp2(static_cast<T>(1.25)) * ROOT_PI
			* exp(PI / 8)) / pow(boost::math::tgamma<T>(0.25), 2);
	return WEIERSTRASS;
}

template<typename T> T wylers() {
	static const T PI = boost::math::constants::pi<T>(),
		WYLERS = (9 / (8 * pow(PI, 4))) * pow(pow(PI, 5) / 1920, 0.25);
	return WYLERS;
}

template<typename T> T zero() {
	static const T ZERO = 0;
	return ZERO;
}

} // namespace constant

namespace constant {
namespace func {

// use std::uint_fastNN_t if you believe it helps
template<typename T, typename A = unsigned>
T champernowne(const A & b = 10) {
	T c = 0, last;

	for(std::size_t n = 1; n == 1 || c != last; ++n) {
		T sub = 0;

		for(std::size_t k = 1; k <= n; ++k) {
			sub += floor(log(k) / log(b));
		}

		last = c;
		c += n / pow(b, n + sub);
	}

	return c;
}

// throws error if r is negative and even
template<typename T, typename A = unsigned>
T favard(const A & r = 2) {
	static const T PI = boost::math::constants::pi<T>();

	if (r == 0) {
		return 1;
	} else if (r % 2) { // odd (dirichlet lambda)
		return (4 / PI) * ((1 - exp2(-(static_cast<T>(r) + 1)))
			* boost::math::zeta<T>(r + 1));
	} else { // even (dirichlet beta)
		return (-4 / PI) * (pow(-2, static_cast<T>(-2) * (r + 1))
			/ boost::math::tgamma<T>(r + 1))
			* (boost::math::polygamma<T>(r, 0.25)
			- boost::math::polygamma<T>(r, 0.75));
	}
}

} // namespace func
} // namespace constant

#endif // CONSTANTS_HPP_