#include "gtest/gtest.h"

#include <Configuration.h>
#include <Eigen/Dense>
#include <Utils/IO/IOUtilities.hpp>
#include <PnC/RobotSystem/RobotSystem.hpp>
#include <PnC/WBC/IHWBC/IHWBC.hpp>

// Draco Specific
#include <PnC/DracoPnC/ContactSet/ContactSet.hpp>
#include <PnC/DracoPnC/CtrlSet/CtrlSet.hpp>
#include <PnC/DracoPnC/TaskSet/TaskSet.hpp>
#include <PnC/DracoPnC/DracoDefinition.hpp>

void getInitialConfiguration(RobotSystem* & robot, Eigen::VectorXd & q_out, Eigen::VectorXd & qdot_out) {
    int lKneeIdx = robot->getDofIdx("lKnee");
    int lHipPitchIdx = robot->getDofIdx("lHipPitch");
    int rKneeIdx = robot->getDofIdx("rKnee");
    int rHipPitchIdx = robot->getDofIdx("rHipPitch");
    int lAnkleIdx = robot->getDofIdx("lAnkle");
    int rAnkleIdx = robot->getDofIdx("rAnkle");

    Eigen::VectorXd q = robot->getQ();
    Eigen::VectorXd qdot(robot->getNumDofs());
    qdot.setZero();

    q[2] = 1.193;
    double alpha(-M_PI / 4.);
    double beta(M_PI / 5.5);
    q[lHipPitchIdx] = alpha;
    q[lKneeIdx] = beta - alpha;
    q[rHipPitchIdx] = alpha;
    q[rKneeIdx] = beta - alpha;
    q[lAnkleIdx] = M_PI / 2 - beta;
    q[rAnkleIdx] = M_PI / 2 - beta;

    q_out = q;
    qdot_out = qdot;
}

// Test 3 types of IHWBC:
//   1) No target reaction force minimization
//   2) Term-by-term force minimization
//   3) Term-by-term no body task
//   4) Desired Contact Wrench minimization

TEST(IHWBC, no_target_reaction_force) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }

    // Initialize IHWBC
    IHWBC* ihwbc = new IHWBC(act_list);

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks
    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    Task* body_rpz_task_ = new BodyRxRyZTask(robot);
    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    task_list.push_back(body_rpz_task_);
    task_list.push_back(rfoot_center_rz_xyz_task);
    task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_body = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body = 1.0*Eigen::VectorXd::Ones(3);
    body_rpz_task_->setGain(kp_body, kd_body);

    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    // Set the desired task values
    // Body Task
    Eigen::VectorXd body_pos_des(7); body_pos_des.setZero();
    Eigen::VectorXd body_vel_des(6); body_vel_des.setZero();
    Eigen::VectorXd body_acc_des(6); body_acc_des.setZero();
    Eigen::Quaternion<double> body_des_quat(1, 0, 0, 0);

    body_pos_des[0] = body_des_quat.w();
    body_pos_des[1] = body_des_quat.x();
    body_pos_des[2] = body_des_quat.y();
    body_pos_des[3] = body_des_quat.z();

    body_pos_des[4] = q[0];
    body_pos_des[5] = q[1];
    body_pos_des[6] = q[2];// + 0.02;

    body_rpz_task_->updateTask(body_pos_des, body_vel_des, body_acc_des);

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();

    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    contact_list.push_back(rfoot_front_contact);
    contact_list.push_back(rfoot_back_contact);
    contact_list.push_back(lfoot_front_contact);
    contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }
    ASSERT_EQ(contact_dim_size, 12);

    Eigen::VectorXd Fd(contact_dim_size); Fd.setZero();

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    w_task_heirarchy[0] = 1e-4; // Body
    w_task_heirarchy[1] = 1.0; // rfoot
    w_task_heirarchy[2] = 1.0; // lfoot
    w_task_heirarchy[3] = 1e-6; // joint

    // When Fd is zero, the contact weight should be smaller than the
    // tasks to encourage using the reaction forces instead of qddot
    // for satisfying the floating base dynamics.
    double w_contact_weight = 1e-10; // Contact Weight

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-16;

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_contact_weight);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    double total_Fz = 0;
    int idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    // Assert that computed reaction forces must be less than 1 Newton
    double diff_Fz = robot->getRobotMass()*9.81 - total_Fz;
    ASSERT_LE(std::fabs(diff_Fz), 1.0);
}

// If we trust the MPC, we don't need a com/body task. As the reaction force will dictate the behavior
// However, if the com/body tasks are available, then the weights between the com/body task vs the contact force tracking will dictate
// the final output.
TEST(IHWBC, term_by_term_rf_computation) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }

    // Initialize IHWBC
    IHWBC* ihwbc = new IHWBC(act_list);

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks

    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    Task* body_rpz_task_ = new BodyRxRyZTask(robot);
    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    task_list.push_back(body_rpz_task_);
    task_list.push_back(rfoot_center_rz_xyz_task);
    task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_body = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body = 1.0*Eigen::VectorXd::Ones(3);
    body_rpz_task_->setGain(kp_body, kd_body);

    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    // Set the desired task values
    // Body Task
    Eigen::VectorXd body_pos_des(7); body_pos_des.setZero();
    Eigen::VectorXd body_vel_des(6); body_vel_des.setZero();
    Eigen::VectorXd body_acc_des(6); body_acc_des.setZero();
    Eigen::Quaternion<double> body_des_quat(1, 0, 0, 0);

    body_pos_des[0] = body_des_quat.w();
    body_pos_des[1] = body_des_quat.x();
    body_pos_des[2] = body_des_quat.y();
    body_pos_des[3] = body_des_quat.z();

    body_pos_des[4] = q[0];
    body_pos_des[5] = q[1];
    body_pos_des[6] = q[2];

    body_rpz_task_->updateTask(body_pos_des, body_vel_des, body_acc_des);

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();

    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    contact_list.push_back(rfoot_front_contact);
    contact_list.push_back(rfoot_back_contact);
    contact_list.push_back(lfoot_front_contact);
    contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }
    ASSERT_EQ(contact_dim_size, 12);

    Eigen::VectorXd Fd(contact_dim_size); Fd.setZero();

    // Distribute reaction forces equally according to the mass of the robot
    double Fz_dist = (robot->getRobotMass()*9.81)/contact_list.size();
    int idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        Fd[idx_offset + contact_list[i]->getFzIndex()] = Fz_dist;
        idx_offset += contact_list[i]->getDim();
    }

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    w_task_heirarchy[0] = 1e-4; // Body
    w_task_heirarchy[1] = 1.0; // rfoot
    w_task_heirarchy[2] = 1.0; // lfoot
    w_task_heirarchy[3] = 1e-6; // joint

    // w_task_heirarchy[0] = 1.0; // rfoot
    // w_task_heirarchy[1] = 1.0; // lfoot
    // w_task_heirarchy[2] = 1e-6; // joint

    // When Fd is nonzero, we need to make the contact weight large if we want to trust the output of the
    // MPC or planner
    // double w_contact_weight = 1e-10; // Contact Weight
    double w_contact_weight = 1.0; // Contact Weight
    // double w_contact_weight = 1.0/(robot->getRobotMass()*9.81);

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-16;

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_contact_weight);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    double total_Fz = 0;
    idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    // Assert that computed reaction forces must be less than 1 Newton
    double diff_Fz = robot->getRobotMass()*9.81 - total_Fz;
    ASSERT_LE(std::fabs(diff_Fz), 1.0);

}

// The body task is optional if we trust the MPC behavior
TEST(IHWBC, term_by_term_rf_computation_no_body_task) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }

    // Initialize IHWBC
    IHWBC* ihwbc = new IHWBC(act_list);

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks

    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    // Task* body_rpz_task_ = new BodyRxRyZTask(robot);
    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    // task_list.push_back(body_rpz_task_);
    task_list.push_back(rfoot_center_rz_xyz_task);
    task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_body = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body = 1.0*Eigen::VectorXd::Ones(3);
    // body_rpz_task_->setGain(kp_body, kd_body);

    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    // Set the desired task values
    // Body Task
    Eigen::VectorXd body_pos_des(7); body_pos_des.setZero();
    Eigen::VectorXd body_vel_des(6); body_vel_des.setZero();
    Eigen::VectorXd body_acc_des(6); body_acc_des.setZero();
    Eigen::Quaternion<double> body_des_quat(1, 0, 0, 0);

    body_pos_des[0] = body_des_quat.w();
    body_pos_des[1] = body_des_quat.x();
    body_pos_des[2] = body_des_quat.y();
    body_pos_des[3] = body_des_quat.z();

    body_pos_des[4] = q[0];
    body_pos_des[5] = q[1];
    body_pos_des[6] = q[2];// + 0.02;

    // body_rpz_task_->updateTask(body_pos_des, body_vel_des, body_acc_des);

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();

    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    contact_list.push_back(rfoot_front_contact);
    contact_list.push_back(rfoot_back_contact);
    contact_list.push_back(lfoot_front_contact);
    contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }
    ASSERT_EQ(contact_dim_size, 12);

    Eigen::VectorXd Fd(contact_dim_size); Fd.setZero();

    // Distribute reaction forces equally according to the mass of the robot
    double Fz_dist = (robot->getRobotMass()*9.81)/contact_list.size();
    int idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        Fd[idx_offset + contact_list[i]->getFzIndex()] = Fz_dist;
        idx_offset += contact_list[i]->getDim();
    }

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    // w_task_heirarchy[0] = 1e-4; // Body
    // w_task_heirarchy[1] = 1.0; // rfoot
    // w_task_heirarchy[2] = 1.0; // lfoot
    // w_task_heirarchy[3] = 1e-6; // joint

    w_task_heirarchy[0] = 1.0; // rfoot
    w_task_heirarchy[1] = 1.0; // lfoot
    w_task_heirarchy[2] = 1e-6; // joint

    // When Fd is nonzero, we need to make the contact weight large if we want to trust the output of the
    // MPC or planner
    // double w_contact_weight = 1e-10; // Contact Weight
    double w_contact_weight = 1.0; // Contact Weight
    // double w_contact_weight = 1.0/(robot->getRobotMass()*9.81);

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-16;

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_contact_weight);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    double total_Fz = 0;
    idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    // Assert that computed reaction forces must be less than 1 Newton
    double diff_Fz = robot->getRobotMass()*9.81 - total_Fz;
    ASSERT_LE(std::fabs(diff_Fz), 1.0);

}

// Similar to above, the body task is optional if we know the desired reaction force.
// If we want to enforce both, we have to select whether we want to weigh the reaction force tracking
// or the body task tracking more. We use w_contact_weight to regulate the relationship
TEST(IHWBC, desired_contact_wrench_computation_no_body_task) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }

    // Initialize IHWBC
    IHWBC* ihwbc = new IHWBC(act_list);

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks

    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    // Task* body_rpz_task_ = new BodyRxRyZTask(robot);
    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    // task_list.push_back(body_rpz_task_);
    task_list.push_back(rfoot_center_rz_xyz_task);
    task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_body = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body = 1.0*Eigen::VectorXd::Ones(3);
    // body_rpz_task_->setGain(kp_body, kd_body);

    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    // Set the desired task values
    // Body Task
    Eigen::VectorXd body_pos_des(7); body_pos_des.setZero();
    Eigen::VectorXd body_vel_des(6); body_vel_des.setZero();
    Eigen::VectorXd body_acc_des(6); body_acc_des.setZero();
    Eigen::Quaternion<double> body_des_quat(1, 0, 0, 0);

    body_pos_des[0] = body_des_quat.w();
    body_pos_des[1] = body_des_quat.x();
    body_pos_des[2] = body_des_quat.y();
    body_pos_des[3] = body_des_quat.z();

    body_pos_des[4] = q[0];
    body_pos_des[5] = q[1];
    body_pos_des[6] = q[2];

    // body_rpz_task_->updateTask(body_pos_des, body_vel_des, body_acc_des);

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();

    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    contact_list.push_back(rfoot_front_contact);
    contact_list.push_back(rfoot_back_contact);
    contact_list.push_back(lfoot_front_contact);
    contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }

    // Set A desired target contact wrench
    Eigen::VectorXd Fd(6); Fd.setZero();
    Fd[2] = (robot->getRobotMass()*9.81);

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs
    Eigen::VectorXd w_rf_contacts(contact_list.size());  // Vector of task priority weighs

    // Note that the distribution of contact weighting must sum to 1.0
    for(int i = 0; i < contact_list.size(); i++){
        w_rf_contacts[i] = 1.0/contact_list.size();
    }
    w_rf_contacts[0] = 0.35;
    w_rf_contacts[1] = 0.15;

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    // w_task_heirarchy[0] = 1e-4; // Body
    // w_task_heirarchy[1] = 1.0; // rfoot
    // w_task_heirarchy[2] = 1.0; // lfoot
    // w_task_heirarchy[3] = 1e-6; // joint

    w_task_heirarchy[0] = 1.0; // rfoot
    w_task_heirarchy[1] = 1.0; // lfoot
    w_task_heirarchy[2] = 1e-6; // joint

    // When Fd is nonzero, we need to make the contact weight large if we want to trust the output of the
    // MPC or planner
    // Any of these should work.
    // double w_contact_weight = 1e-10; // Contact Weight
    // double w_contact_weight = 1.0; // Contact Weight
    // double w_contact_weight = 1.0/(robot->getRobotMass()*9.81);
    double w_contact_weight = 1e-4/(robot->getRobotMass()*9.81);

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-12;//1e-16;

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_rf_contacts, w_contact_weight);
    ihwbc->setTargetWrenchMinimization(true);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;
    Eigen::VectorXd Fr_sum;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    double total_Fz = 0;
    int idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        Fr_sum += Fr_res.segment(idx_offset, contact_list[i]->getDim());
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(Fr_sum, std::cout, "Fr_sum");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    std::cout << "Fr_sum.norm() = " << Fr_sum.norm() << std::endl;

    // Assert that computed reaction forces must be less than 10 Newtons
    double diff_Fz = robot->getRobotMass()*9.81 - total_Fz;
    ASSERT_LE(std::fabs(diff_Fz), 10.0);

}

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;
}

// Timer
#include <chrono>

TEST(IHWBC, robot_centroidal_inertia) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);

    // Start Time
    std::chrono::high_resolution_clock::time_point t_Ig_start;
    std::chrono::high_resolution_clock::time_point t_Ig_end;
    double dur_Ig = 0.0;

    t_Ig_start = std::chrono::high_resolution_clock::now();

    robot->updateSystem(q, qdot);

    // End Time
    t_Ig_end = std::chrono::high_resolution_clock::now();
    dur_Ig = std::chrono::duration_cast< std::chrono::duration<double> >(t_Ig_end - t_Ig_start).count();

    // std::cout << "Ig compute time = " << dur_Ig << std::endl;

    Eigen::MatrixXd Ig_o = robot->getCentroidInertia();
    Eigen::MatrixXd I_body_o = Ig_o.block(0,0,3,3);

    myUtils::pretty_print(Ig_o, std::cout, "Ig_o");
    myUtils::pretty_print(I_body_o, std::cout, "I_body_o");

    double psi = M_PI/12.0;
    int n = 10;
    double dt_Ig_average = 0.0;
    for(int i = 0; i < 10; i++){
        q[5] = psi*(i+1);
        t_Ig_start = std::chrono::high_resolution_clock::now();
        robot->updateSystem(q, qdot);
        t_Ig_end = std::chrono::high_resolution_clock::now();
        dur_Ig = std::chrono::duration_cast< std::chrono::duration<double> >(t_Ig_end - t_Ig_start).count();
        dt_Ig_average += (dur_Ig/n);
    }

    // Eigen::MatrixXd Ig_yaw = robot->getCentroidInertia();
    // Eigen::MatrixXd I_body_yaw = Ig_yaw.block(0,0,3,3);
    // Eigen::MatrixXd I_body_yaw_test = R_yaw(psi)*I_body_o*R_yaw(psi).transpose();

    // myUtils::pretty_print(I_body_yaw, std::cout, "I_body_yaw");
    // myUtils::pretty_print(I_body_yaw_test, std::cout, "I_body_yaw_test");

    // double robot_mass = robot->getRobotMass();
    // std::cout << "robot_mass = " << robot_mass << std::endl;

    std::cout << "Avg centroidal inertia computation time = " << dt_Ig_average << std::endl;

}

// Torque limit test
TEST(IHWBC, torque_limit_test) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }

    // Initialize IHWBC
    IHWBC* ihwbc = new IHWBC(act_list);

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks

    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    Task* body_rpz_task_ = new BodyRxRyZTask(robot);
    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    task_list.push_back(body_rpz_task_);
    task_list.push_back(rfoot_center_rz_xyz_task);
    task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_body = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body = 1.0*Eigen::VectorXd::Ones(3);
    body_rpz_task_->setGain(kp_body, kd_body);

    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    // Set the desired task values
    // Body Task
    Eigen::VectorXd body_pos_des(7); body_pos_des.setZero();
    Eigen::VectorXd body_vel_des(6); body_vel_des.setZero();
    Eigen::VectorXd body_acc_des(6); body_acc_des.setZero();
    Eigen::Quaternion<double> body_des_quat(1, 0, 0, 0);

    body_pos_des[0] = body_des_quat.w();
    body_pos_des[1] = body_des_quat.x();
    body_pos_des[2] = body_des_quat.y();
    body_pos_des[3] = body_des_quat.z();

    body_pos_des[4] = q[0];
    body_pos_des[5] = q[1];
    body_pos_des[6] = q[2];

    body_rpz_task_->updateTask(body_pos_des, body_vel_des, body_acc_des);

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();

    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    contact_list.push_back(rfoot_front_contact);
    contact_list.push_back(rfoot_back_contact);
    contact_list.push_back(lfoot_front_contact);
    contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }
    ASSERT_EQ(contact_dim_size, 12);

    Eigen::VectorXd Fd(contact_dim_size); Fd.setZero();

    // Distribute reaction forces equally according to the mass of the robot
    double Fz_dist = (robot->getRobotMass()*9.81)/contact_list.size();
    int idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        Fd[idx_offset + contact_list[i]->getFzIndex()] = Fz_dist;
        idx_offset += contact_list[i]->getDim();
    }

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    w_task_heirarchy[0] = 1e-4; // Body
    w_task_heirarchy[1] = 1.0; // rfoot
    w_task_heirarchy[2] = 1.0; // lfoot
    w_task_heirarchy[3] = 1e-6; // joint

    // w_task_heirarchy[0] = 1.0; // rfoot
    // w_task_heirarchy[1] = 1.0; // lfoot
    // w_task_heirarchy[2] = 1e-6; // joint

    // When Fd is nonzero, we need to make the contact weight large if we want to trust the output of the
    // Keeping it the same magnitude as the body task seems to have some benefits.
    double w_contact_weight = 1e-4/(robot->getRobotMass()*9.81);

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-16;

    // Enable Torque Limits
    ihwbc->enableTorqueLimits(true);
    double tau_lim = 30.0;
    Eigen::VectorXd tau_min = -tau_lim*Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd tau_max = tau_lim*Eigen::VectorXd::Ones(Draco::n_adof);
    ihwbc->setTorqueLimits(tau_min, tau_max);

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_contact_weight);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    double total_Fz = 0;
    idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    // Assert that the computed torques are less than the set limit
    for(int i = 0; i < tau_cmd.size(); i++){
       ASSERT_LE(std::fabs(tau_cmd[i]), (tau_lim+0.001));
    }

}

// Torque limit test
TEST(IHWBC, jpos_test) {
    RobotSystem* robot;
    robot = new RobotSystem(6, THIS_COM "RobotModel/Robot/Draco/DracoPnC_Dart.urdf");

    //  Initialize actuated list
    std::vector<bool> act_list;
    act_list.resize(robot->getNumDofs(), true);
    for (int i(0); i < robot->getNumVirtualDofs(); ++i){
        act_list[i] = false;
    }

    // Initialize IHWBC
    IHWBC* ihwbc = new IHWBC(act_list);

    // Initialize and Update the robot
    Eigen::VectorXd q, qdot;
    getInitialConfiguration(robot, q, qdot);
    robot->updateSystem(q, qdot);
    // myUtils::pretty_print(q, std::cout, "q");

    // Task and Contact list
    std::vector<Task*> task_list;
    std::vector<ContactSpec*> contact_list;

    // Create Tasks

    // Body RxRyZ tasks or CoM xyz task or Linear Momentum Task
    // Body Rx Ry Rz
    // Foot location task.
    // Joint Position Task. It appears that it's important to have this task to ensure that uncontrolled qddot does not blow up
    Task* body_rpz_task_ = new BodyRxRyZTask(robot);
    Task* rfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::rFootCenter);
    Task* lfoot_center_rz_xyz_task = new FootRzXYZTask(robot, DracoBodyNode::lFootCenter);
    Task* total_joint_task = new BasicTask(robot, BasicTaskType::JOINT, Draco::n_adof);

    // task_list.push_back(body_rpz_task_);
    // task_list.push_back(rfoot_center_rz_xyz_task);
    // task_list.push_back(lfoot_center_rz_xyz_task);
    task_list.push_back(total_joint_task);

    // Set Task Gains
    Eigen::VectorXd kp_body = 100*Eigen::VectorXd::Ones(3);
    Eigen::VectorXd kd_body = 1.0*Eigen::VectorXd::Ones(3);
    body_rpz_task_->setGain(kp_body, kd_body);

    Eigen::VectorXd kp_foot = 100*Eigen::VectorXd::Ones(4);
    Eigen::VectorXd kd_foot = 1.0*Eigen::VectorXd::Ones(4);
    rfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);
    lfoot_center_rz_xyz_task->setGain(kp_foot, kd_foot);

    Eigen::VectorXd kp_jp = Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd kd_jp = 0.1*Eigen::VectorXd::Ones(Draco::n_adof);
    total_joint_task->setGain(kp_jp, kd_jp);

    // Set the desired task values
    // Body Task
    Eigen::VectorXd body_pos_des(7); body_pos_des.setZero();
    Eigen::VectorXd body_vel_des(6); body_vel_des.setZero();
    Eigen::VectorXd body_acc_des(6); body_acc_des.setZero();
    Eigen::Quaternion<double> body_des_quat(1, 0, 0, 0);

    body_pos_des[0] = body_des_quat.w();
    body_pos_des[1] = body_des_quat.x();
    body_pos_des[2] = body_des_quat.y();
    body_pos_des[3] = body_des_quat.z();

    body_pos_des[4] = q[0];
    body_pos_des[5] = q[1];
    body_pos_des[6] = q[2];

    body_rpz_task_->updateTask(body_pos_des, body_vel_des, body_acc_des);

    // Foot Task
    Eigen::VectorXd rfoot_pos_des(7); rfoot_pos_des.setZero();
    Eigen::VectorXd lfoot_pos_des(7); lfoot_pos_des.setZero();
    Eigen::VectorXd foot_vel_des(6); foot_vel_des.setZero();
    Eigen::VectorXd foot_acc_des(6); foot_acc_des.setZero();

    Eigen::Quaternion<double> rfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).linear());
    Eigen::Quaternion<double> lfoot_ori_act(robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).linear());

    rfoot_pos_des[0] = rfoot_ori_act.w();
    rfoot_pos_des[1] = rfoot_ori_act.x();
    rfoot_pos_des[2] = rfoot_ori_act.y();
    rfoot_pos_des[3] = rfoot_ori_act.z();
    rfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::rFootCenter).translation();

    lfoot_pos_des[0] = lfoot_ori_act.w();
    lfoot_pos_des[1] = lfoot_ori_act.x();
    lfoot_pos_des[2] = lfoot_ori_act.y();
    lfoot_pos_des[3] = lfoot_ori_act.z();
    lfoot_pos_des.tail(3) = robot->getBodyNodeCoMIsometry(DracoBodyNode::lFootCenter).translation();

    // Joint Position Task
    Eigen::VectorXd jpos_des = 0.95*robot->getQ().tail(Draco::n_adof);
    Eigen::VectorXd jvel_des(Draco::n_adof);  jvel_des.setZero();
    Eigen::VectorXd jacc_des(Draco::n_adof);  jacc_des.setZero();

    rfoot_center_rz_xyz_task->updateTask(rfoot_pos_des, foot_vel_des, foot_acc_des);
    lfoot_center_rz_xyz_task->updateTask(lfoot_pos_des, foot_vel_des, foot_acc_des);
    total_joint_task->updateTask(jpos_des, jvel_des, jacc_des);

    // myUtils::pretty_print(body_pos_des, std::cout, "body_pos_des");
    // myUtils::pretty_print(rfoot_pos_des, std::cout, "rfoot_pos_des");
    // myUtils::pretty_print(lfoot_pos_des, std::cout, "lfoot_pos_des");
    // myUtils::pretty_print(jpos_des, std::cout, "jpos_des");

    // Create the contacts
    ContactSpec* rfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::rFootFront, 0.7);
    ContactSpec* rfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::rFootBack, 0.7);
    ContactSpec* lfoot_front_contact = new PointContactSpec(robot, DracoBodyNode::lFootFront, 0.7);
    ContactSpec* lfoot_back_contact = new PointContactSpec(robot, DracoBodyNode::lFootBack, 0.7);

    // contact_list.push_back(rfoot_front_contact);
    // contact_list.push_back(rfoot_back_contact);
    // contact_list.push_back(lfoot_front_contact);
    // contact_list.push_back(lfoot_back_contact);

    // ((PointContactSpec*)rfoot_front_contact)->setMaxFz(100.0);

    rfoot_front_contact->updateContactSpec();
    rfoot_back_contact->updateContactSpec();
    lfoot_front_contact->updateContactSpec();
    lfoot_back_contact->updateContactSpec();

    // Update dynamics
    Eigen::MatrixXd A = robot->getMassMatrix();
    Eigen::MatrixXd Ainv = robot->getInvMassMatrix();
    Eigen::VectorXd grav = robot->getGravity();
    Eigen::MatrixXd coriolis = robot->getCoriolis();

    // myUtils::pretty_print(A, std::cout, "A");

    // Containers
    Eigen::VectorXd tau_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Torque Command output from IHBC
    Eigen::VectorXd qddot_cmd(robot->getNumDofs() - robot->getNumVirtualDofs()); // Joint Acceleration Command output from IHBC

    int contact_dim_size = 0;
    for(int i = 0; i < contact_list.size(); i++){
        contact_dim_size += contact_list[i]->getDim();
    }
    // ASSERT_EQ(contact_dim_size, 12);

    Eigen::VectorXd Fd(contact_dim_size); Fd.setZero();

    // Distribute reaction forces equally according to the mass of the robot
    double Fz_dist = (robot->getRobotMass()*9.81)/contact_list.size();
    int idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        Fd[idx_offset + contact_list[i]->getFzIndex()] = Fz_dist;
        idx_offset += contact_list[i]->getDim();
    }

    // Initialize QP weights
    Eigen::VectorXd w_task_heirarchy(task_list.size());  // Vector of task priority weighs

    // Contact Tasks have weight 1.0
    // Pose Tasks have weight 1e-4
    // Joint Posture Tasks have weight 1e-6
    // w_task_heirarchy[0] = 1e-4; // Body
    // w_task_heirarchy[1] = 1.0; // rfoot
    // w_task_heirarchy[2] = 1.0; // lfoot
    w_task_heirarchy[0] = 1e-6; // joint

    // w_task_heirarchy[0] = 1.0; // rfoot
    // w_task_heirarchy[1] = 1.0; // lfoot
    // w_task_heirarchy[2] = 1e-6; // joint

    // When Fd is nonzero, we need to make the contact weight large if we want to trust the output of the
    // Keeping it the same magnitude as the body task seems to have some benefits.
    double w_contact_weight = 1e-4/(robot->getRobotMass()*9.81);

    // Regularization terms should always be the lowest cost.
    double lambda_qddot = 1e-16;
    double lambda_Fr = 1e-16;

    // Enable Torque Limits
    ihwbc->enableTorqueLimits(true);
    double tau_lim = 30.0;
    Eigen::VectorXd tau_min = -tau_lim*Eigen::VectorXd::Ones(Draco::n_adof);
    Eigen::VectorXd tau_max = tau_lim*Eigen::VectorXd::Ones(Draco::n_adof);
    ihwbc->setTorqueLimits(tau_min, tau_max);

    // Set QP weights
    ihwbc->setQPWeights(w_task_heirarchy, w_contact_weight);
    ihwbc->setRegularizationTerms(lambda_qddot, lambda_Fr);

    // Update and solve QP
    ihwbc->updateSetting(A, Ainv, coriolis, grav);
    ihwbc->solve(task_list, contact_list, Fd, tau_cmd, qddot_cmd);

    // QP dec variable results
    Eigen::VectorXd qddot_res;
    Eigen::VectorXd Fr_res;

    ihwbc->getQddotResult(qddot_res);
    ihwbc->getFrResult(Fr_res);

    double total_Fz = 0;
    idx_offset = 0;
    for(int i = 0; i < contact_list.size(); i++){
        total_Fz += Fr_res[idx_offset + contact_list[i]->getFzIndex()];
        idx_offset += contact_list[i]->getDim();
    }

    myUtils::pretty_print(Fd, std::cout, "Fd");
    myUtils::pretty_print(tau_cmd, std::cout, "tau_cmd");
    myUtils::pretty_print(qddot_cmd, std::cout, "qddot_cmd");
    myUtils::pretty_print(qddot_res, std::cout, "qddot_res");
    myUtils::pretty_print(Fr_res, std::cout, "Fr_res");
    std::cout << "robot mass = " << robot->getRobotMass() << std::endl;
    std::cout << "robot mass*grav = " << robot->getRobotMass()*9.81 << std::endl;
    std::cout << "total_Fz:" << total_Fz << std::endl;

    // Assert that the computed torques are less than the set limit
    for(int i = 0; i < tau_cmd.size(); i++){
       ASSERT_LE(std::fabs(tau_cmd[i]), (tau_lim+0.001));
    }

    Eigen::VectorXd qddot_test = Eigen::VectorXd::Zero(robot->getNumDofs());
    qddot_test[2] = -9.81;
    std::cout << "Aqddot = " << (A*qddot_test).transpose() << std::endl;
    std::cout << "grav = " << grav.transpose() << std::endl;

}


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_INTERLEAVE_ODD_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_INTERLEAVE_ODD_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( interleave_odd_
                          , (typename A0)
                          , bs::sse1_
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                          , bs::pack_<bd::single_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const A0 & a0, const A0 & a1) const BOOST_NOEXCEPT
    {
      return _mm_unpackhi_ps( _mm_shuffle_ps(a0,a0,_MM_SHUFFLE(3,1,3,1))
                            , _mm_shuffle_ps(a1,a1,_MM_SHUFFLE(3,1,3,1))
                            );
    }
  };
} } }

#endif


#include "odoflow/FASTPointDetector.h"
#include "argus_utils/utils/ParamUtils.h"

#include <boost/foreach.hpp>
#include <opencv2/features2d.hpp>
#include <iostream>

namespace argus
{
FASTPointDetector::FASTPointDetector( ros::NodeHandle& nh, ros::NodeHandle& ph )
	: InterestPointDetector( nh, ph )
{
	_intensityThreshold.InitializeAndRead( ph, 50, "intensity_threshold",
	                                       "Minimum central pixel intensity difference" );
	_intensityThreshold.AddCheck<GreaterThanOrEqual>( 0 );
	_intensityThreshold.AddCheck<LessThanOrEqual>( 255 );
	_intensityThreshold.AddCheck<IntegerValued>( ROUND_CLOSEST );

	_enableNMS.InitializeAndRead( ph, true, "enable_nonmax_suppression",
	                              "Usage of non-maximum suppression" );

	_maxNumPoints.InitializeAndRead( ph, 255, "max_num_points",
	                                 "Maximum number of points to find" );
	_maxNumPoints.AddCheck<GreaterThanOrEqual>( 0 );
	_maxNumPoints.AddCheck<IntegerValued>( ROUND_CEIL );

	_detectorType.InitializeAndRead( ph, "FAST_7_12", "detector_type",
	                                 "FAST detector type" );
}

/*! \brief Comparator used for sorting keypoints by responses
 */
bool CompareKeypoints( const cv::KeyPoint& a, const cv::KeyPoint& b )
{
	return a.response < b.response;
}

InterestPoints FASTPointDetector::FindInterestPoints( const cv::Mat& image )
{
	std::vector<cv::KeyPoint> keypoints;
	cv::FAST( image,
	          keypoints,
	          _intensityThreshold,
	          _enableNMS,
	          StringToDetector( _detectorType ) );

	std::sort( keypoints.begin(), keypoints.end(), CompareKeypoints );

	InterestPoints points;
	unsigned int toCopy = std::min( keypoints.size(), (size_t) _maxNumPoints.GetValue() );
	points.reserve( toCopy );
	for( unsigned int i = 0; i < toCopy; i++ )
	{
		points.push_back( keypoints[i].pt );
	}
	return points;
}

int FASTPointDetector::StringToDetector( const std::string& str )
{
	if( str == "FAST_5_8" )
	{
		return cv::FastFeatureDetector::TYPE_5_8;
	}
	else if( str == "FAST_7_12" )
	{
		return cv::FastFeatureDetector::TYPE_7_12;
	}
	else if( str == "FAST_9_16" )
	{
		return cv::FastFeatureDetector::TYPE_9_16;
	}
	else
	{
		throw std::runtime_error( "Invalid FAST detector " + str +
		                          " specified. Must be FAST_5_8, FAST_7_12, or FAST_9_16" );
	}
}
}


/**
 * Copyright (c) 2016, Adrien Devresse <adrien.devresse@epfl.ch>
 *
 * Boost Software License - Version 1.0
 *
 * Permission is hereby granted, free of charge, to any person or organization
 * obtaining a copy of the software and accompanying documentation covered by
 * this license (the "Software") to use, reproduce, display, distribute,
 * execute, and transmit the Software, and to prepare derivative works of the
 * Software, and to permit third-parties to whom the Software is furnished to
 * do so, all subject to the following:
 *
 * The copyright notices in the Software and this entire statement, including
 * the above license grant, this restriction and the following disclaimer,
 * must be included in all copies of the Software, in whole or in part, and
 * all derivative works of the Software, unless such copies or derivative
 * works are solely in the form of machine-executable object code generated by
 * a source language processor.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 */

#include <boost/test/floating_point_comparison.hpp>

#include <boost/chrono.hpp>
#include <boost/random.hpp>

#include <hadoken/random/random.hpp>

using namespace boost::chrono;

typedef system_clock::time_point tp;
typedef system_clock cl;

std::size_t test_random_mersenne_twister(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    boost::random::mt19937 twister_engine;

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(twister_engine);
    }

    t2 = cl::now();

    std::cout << "mersenne_twister: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_abstract_mersenne_twister(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    boost::random::mt19937 twister_engine;
    hadoken::random_engine_mapper_32 abstract_engine(std::move(twister_engine));

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(abstract_engine);
    }

    t2 = cl::now();

    std::cout << "abstract_mersenne_twister: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_ranlux(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    boost::random::ranlux24 ranlux_engine;

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(ranlux_engine);
    }

    t2 = cl::now();

    std::cout << "ranlux: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_abstract_ranlux(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    boost::random::ranlux24 ranlux_engine;
    hadoken::random_engine_mapper_32 abstract_engine(std::move(ranlux_engine));

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(abstract_engine);
    }

    t2 = cl::now();

    std::cout << "abstract_ranlux: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_tau88(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    boost::random::taus88 tau_engine;

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(tau_engine);
    }

    t2 = cl::now();

    std::cout << "tau88: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_abstract_tau88(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    boost::random::taus88 tau_engine;
    hadoken::random_engine_mapper_32 abstract_engine(std::move(tau_engine));

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(abstract_engine);
    }

    t2 = cl::now();

    std::cout << "abstract_tau88: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_abstract_threefry(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;
    hadoken::counter_engine<hadoken::threefry4x64> threefry_engine;
    hadoken::random_engine_mapper_64 abstract_engine(std::move(threefry_engine));

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(abstract_engine);
    }

    t2 = cl::now();

    std::cout << "abstract_threefry: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_threefry(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;

    hadoken::counter_engine<hadoken::threefry4x64> threefry_engine;

    t1 = cl::now();

    for (std::size_t i = 0; i < iter; ++i) {
        res += dist(threefry_engine);
    }

    t2 = cl::now();

    std::cout << "threefry: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

std::size_t test_random_threefry_block_fake(std::size_t iter) {

    std::size_t res = 0;

    tp t1, t2;

    boost::random::uniform_int_distribution<std::size_t> dist;

    hadoken::counter_engine<hadoken::threefry4x64> threefry_engine;

    t1 = cl::now();

    const std::size_t size_block =
        sizeof(typename hadoken::counter_engine<hadoken::threefry4x64>::ctr_type) / sizeof(std::size_t);

    for (std::size_t i = 0; i < (iter / size_block); ++i) {
        auto block = threefry_engine.generate_block();
        for (std::size_t i = 0; i < size_block; ++i) {
            res += block[0];
        }
    }

    t2 = cl::now();

    std::cout << "threefry block gen: " << boost::chrono::duration_cast<milliseconds>(t2 - t1) << std::endl;
    return res;
}

int main() {

    const std::size_t n_exec = 10000000;
    std::size_t junk = 0;

    std::cout << "generation of " << n_exec << " random numbers" << std::endl;

    junk += test_random_mersenne_twister(n_exec);

    junk += test_random_abstract_mersenne_twister(n_exec);

    junk += test_random_ranlux(n_exec);

    junk += test_random_abstract_ranlux(n_exec);

    junk += test_random_tau88(n_exec);

    junk += test_random_abstract_tau88(n_exec);

    junk += test_random_threefry(n_exec);

    junk += test_random_threefry_block_fake(n_exec);

    junk += test_random_abstract_threefry(n_exec);

    std::cout << "end junk " << junk << std::endl;
}


#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <boost/timer.hpp>
#include <algorithm>

#include "core/Config.h"
#include "core/Feature.h"
#include "core/Featuredetection.h"
#include "core/Tracker.h"
#include <core/Map.h>
#include "io/Zed_svo_wrapper.h"
#include "utils/SLAMTools.h"

using namespace std;

int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: test_localmap config_file svo_file"<<endl;
        return -1;
    }

    string path_to_config ( argv[1] );
    string svo_file ( argv[2] );

    zed_slam::Config::init ( path_to_config );
    zed_slam::Zed_svo_wrapper* svo_wrapper = new zed_slam::Zed_svo_wrapper ( svo_file );

    zed_slam::Frame::Ptr frame1 = svo_wrapper->next();
    zed_slam::FrameDrawer viewer;
    if ( frame1 == nullptr )
        return -1;

    // track these features in frame 2
    zed_slam::Tracker tracker;
    zed_slam::FeatureDetection featureDetection;
    featureDetection.detectFAST( frame1 );

    zed_slam::LocalMap  localMap;
    viewer.setLocalMap(&localMap);
    localMap.insertKeyFrame( frame1 );

    int loop=1;
    while ( 1 )
    {
        cout<<"********* loop "<<loop<<" **********"<<endl;
        zed_slam::Frame::Ptr frame2 = svo_wrapper->next();
        if ( frame2 == nullptr )
            break;

        boost::timer timer;
        tracker.trackKLT ( frame1, frame2 );
        cout<<"trackKLT uses time "<<timer.elapsed() <<endl;

        // estimate the pose of frame 2
        boost::timer timer_pose;
        tracker.poseEstimationTwoFrames ( frame1, frame2 );

        if ( frame2->features_.size() <= zed_slam::Config::get<int> ( "min_features" ) )
        {
            // add new features into frame 2
            featureDetection.detectFAST ( frame2 );
        }

        loop++;
        viewer.addFrame( frame2 );
        // check key-frames
        zed_slam::SLAMTools::drawFeatures( frame2 );
        cv::waitKey(1);

        if ( localMap.checkKeyFrame(frame2) )
        {
            cout<<"this is a new key-frame"<<endl;
            viewer.addKeyframe( frame2 );
            localMap.insertKeyFrame( frame2 );
            cv::waitKey(0);
            //break;
        }
        frame1 = frame2;
    }

    viewer.stop();
    return 0;
}


/*
 * Copyright (C) 2012-2015 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Modified by : Dharmin B.
*/
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <ignition/math.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

namespace gazebo
{
  class AnimatedObject : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        // Store the pointer to the model
        this->model = _parent;

        double animation_time = 0.0;
        int number_of_frames = 0;
        bool loop = false;
        std::vector<double> key_frames;
        std::vector<double> x_positions;
        std::vector<double> y_positions;
        std::vector<double> orientations;

        // get arguments
        if (_sdf->HasElement("animation_time")){
            animation_time = _sdf->Get<double>("animation_time");
        }
        if (_sdf->HasElement("loop")){
            loop = _sdf->Get<bool>("loop");
        }
        if (_sdf->HasElement("number_of_frames")){
            number_of_frames = _sdf->Get<int>("number_of_frames");
        }
        key_frames = this->extract_vector_from_string_parameter("key_frames", _sdf);
        x_positions = this->extract_vector_from_string_parameter("x_positions", _sdf);
        y_positions = this->extract_vector_from_string_parameter("y_positions", _sdf);
        orientations = this->extract_vector_from_string_parameter("orientations", _sdf);

        // create the animation
        gazebo::common::PoseAnimationPtr anim(
              new gazebo::common::PoseAnimation("animation", animation_time, loop));

        gazebo::common::PoseKeyFrame *key;

        // check if number of frames is same as the number of key frame paramters
        if (number_of_frames == key_frames.size() &&
                number_of_frames == x_positions.size() &&
                number_of_frames == y_positions.size() &&
                number_of_frames == orientations.size()){
            // set key frames in the animation
            for (int i = 0; i < number_of_frames; ++i) {
                key = anim->CreateKeyFrame(key_frames.at(i));
                key->Translation(ignition::math::Vector3d(x_positions.at(i), y_positions.at(i), 0));
                key->Rotation(ignition::math::Quaterniond(0, 0, orientations.at(i)));
            }
            if (loop) {
                key = anim->CreateKeyFrame(animation_time);
                key->Translation(ignition::math::Vector3d(x_positions.at(0), y_positions.at(0), 0));
                key->Rotation(ignition::math::Quaterniond(0, 0, orientations.at(0)));
            }
            // set the animation
            _parent->SetAnimation(anim);
        }
        else{
            std::cout << "Moving obstacle plugin received erroneous parameters" << std::endl;
        }
    }

    // return a vector of double after reading a parameter
    private: std::vector<double> extract_vector_from_string_parameter(std::string param_name, sdf::ElementPtr _sdf)
    {
        std::string vector_string;
        std::vector<double> parameter;
        if (_sdf->HasElement(param_name)){
            vector_string = _sdf->Get<std::string>(param_name);
        }
        else {
            parameter.push_back(0);
            return parameter;
        }

        // split space separated arguments
        std::istringstream iss(vector_string);
        std::vector<std::string> results(std::istream_iterator<std::string>{iss},
                                         std::istream_iterator<std::string>());
        for (std::string i : results) {
            parameter.push_back (std::stod(i));
        }
        return parameter;
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(AnimatedObject)
}


#include <cassert>
#include <fstream>
#include <limits>

#include <boost/program_options.hpp>

#include <xolotl/options/Options.h>
#include <xolotl/util/TokenizedLineReader.h>

namespace bpo = boost::program_options;

namespace xolotl
{
namespace options
{
Options::Options() :
	shouldRunFlag(true),
	exitCode(EXIT_SUCCESS),
	petscArg(""),
	networkFilename(""),
	tempHandlerName(""),
	tempParam{},
	tempProfileFilename(""),
	fluxFlag(false),
	fluxAmplitude(0.0),
	fluxTimeProfileFlag(false),
	perfHandlerName(""),
	vizHandlerName(""),
	materialName(""),
	initialVConcentration(0.0),
	voidPortion(50.0),
	dimensionNumber(1),
	gridTypeName(""),
	gridParam{},
	gridFilename(""),
	gbList(""),
	groupingMin(std::numeric_limits<int>::max()),
	groupingWidthA(1),
	groupingWidthB(0),
	sputteringYield(0.0),
	useHDF5Flag(true),
	maxImpurity(8),
	maxD(0),
	maxT(0),
	maxV(20),
	maxI(6),
	leftBoundary(1),
	rightBoundary(1),
	bottomBoundary(1),
	topBoundary(1),
	frontBoundary(1),
	backBoundary(1),
	xBC("mirror"),
	burstingDepth(10.0),
	burstingFactor(0.1),
	rngUseSeed(false),
	rngSeed(0),
	rngPrintSeed(false),
	zeta(0.73),
	density(10.162795276841),
	pulseTime(0.0),
	pulseProportion(0.0),
	latticeParameter(-1.0),
	impurityRadius(-1.0),
	biasFactor(1.15),
	hydrogenFactor(0.25),
	xenonDiffusivity(-1.0),
	fissionYield(0.25),
	heVRatio(4.0),
	migrationThreshold(std::numeric_limits<double>::infinity())
{
	return;
}

Options::~Options(void)
{
}

void
Options::readParams(int argc, const char* argv[])
{
	// Check that a file name is given
	if (argc < 2) {
		std::cerr << "Options: parameter file name must not be empty"
				  << std::endl;
		shouldRunFlag = false;
		exitCode = EXIT_FAILURE;
		return;
	}

	// The name of the parameter file
	std::string param_file;

	// Parse the command line options.
	bpo::options_description desc("Command line options");
	desc.add_options()("help", "show this help message")("parameterFile",
		bpo::value<std::string>(&param_file),
		"When running Xolotl the name of the parameter file should immediately "
		"follow the executable: xolotl/xolotl param.txt");

	bpo::positional_options_description p;
	p.add("parameterFile", -1);

	bpo::variables_map opts;

	bpo::store(
		bpo::command_line_parser(argc, argv).options(desc).positional(p).run(),
		opts);
	bpo::notify(opts);

	// Declare a group of options that will be
	// allowed both on command line and in
	// config file
	bpo::options_description config("Parameters");
	config.add_options()("networkFile",
		bpo::value<std::string>(&networkFilename),
		"The HDF5 file to use for restart.")("tempHandler",
		bpo::value<std::string>(&tempHandlerName)->default_value("constant"),
		"Temperature handler to use. (default = constant; available "
		"constant,gradient,heat,profile")("tempParam",
		bpo::value<std::string>(),
		"At most two parameters for temperature handler. Alternatives:"
		"constant -> temp; "
		"gradient -> surfaceTemp bulkTemp; "
		"heat -> heatFlux bulkTemp")("tempFile",
		bpo::value<std::string>(&tempProfileFilename),
		"A temperature profile is given by the specified file, "
		"then linear interpolation is used to fit the data."
		" NOTE: no need for tempParam here.")("flux",
		bpo::value<double>(&fluxAmplitude),
		"The value of the incoming flux in #/nm2/s. If the Fuel case is used "
		"it actually corresponds to the fission rate in #/nm3/s.")("fluxFile",
		bpo::value<std::string>(&fluxTimeProfileFilePath),
		"A time profile for the flux is given by the specified file, "
		"then linear interpolation is used to fit the data."
		"(NOTE: If a flux profile file is given, "
		"a constant flux should NOT be given)")("perfHandler",
		bpo::value<std::string>(&perfHandlerName)->default_value("os"),
		"Which set of performance handlers to use. (default = os, available "
		"dummy,os,papi).")("vizHandler",
		bpo::value<std::string>(&vizHandlerName)->default_value("dummy"),
		"Which set of handlers to use for the visualization. (default = dummy, "
		"available std,dummy).")("dimensions",
		bpo::value<int>(&dimensionNumber),
		"Number of dimensions for the simulation.")("material",
		bpo::value<std::string>(&materialName),
		"The material options are as follows: {W100, W110, W111, "
		"W211, Pulsed, Fuel, Fe, 800H}.")("initialV",
		bpo::value<double>(&initialVConcentration),
		"The value of the initial concentration of vacancies in the material.")(
		"zeta", bpo::value<double>(&zeta)->default_value(0.73),
		"The value of the electronic stopping power in the material (0.73 by "
		"default).")("voidPortion", bpo::value<double>(&voidPortion),
		"The value (in %) of the void portion at the start of the simulation.")(
		"gridType",
		bpo::value<std::string>(&gridTypeName)->default_value("uniform"),
		"Grid type to use along X. (default = uniform; available "
		"uniform,nonuniform,geometric,cheby,read")("gridParam",
		bpo::value<std::string>(),
		"At most six parameters for the grid. Alternatives:"
		"uniform -> nX hX; "
		"nonuniform -> nX; "
		"geometric -> nX ratio"
		"cheby -> nX width"
		"The four additional parameters are for a uniform grid in Y and Z -> "
		"nY hY nZ hZ.")("gridFile", bpo::value<std::string>(&gridFilename),
		"A grid spacing is given by the specified file."
		" NOTE: you need gridParam here only if in 2D/3D.")("petscArgs",
		bpo::value<std::string>(&petscArg),
		"All the arguments that will be given to PETSc.")("process",
		bpo::value<std::string>(),
		"List of all the processes to use in the simulation (reaction, diff, "
		"advec, modifiedTM, movingSurface, bursting, attenuation, resolution, "
		"heterogeneous, sink).")("grain", bpo::value<std::string>(&gbList),
		"This option allows the user to add GB in the X, Y, or Z directions. "
		"To do so, simply write the direction followed "
		"by the distance in nm, for instance: X 3.0 Z 2.5 Z 10.0 .")("grouping",
		bpo::value<std::string>(),
		"The grouping parameters: the first integer is the size at which the "
		"grouping starts (HeV clusters in the PSI case, Xe in the NE case), "
		"the second is the first width of the groups (He for PSI, Xe for NE), "
		"and the third one in the second width of the groups (V for PSI).")(
		"sputtering", bpo::value<double>(&sputteringYield),
		"The sputtering yield (in atoms/ion) that will be used.")("netParam",
		bpo::value<std::string>(),
		"This option allows the user to define the boundaries of the network. "
		"To do so, simply write the values in order "
		"maxHe/Xe maxD maxT maxV maxI.")("radiusSize",
		bpo::value<std::string>(),
		"This option allows the user to set a minimum size for the computation "
		"for the average radii, in the same order as the netParam option "
		"(default is 0).")("boundary", bpo::value<std::string>(),
		"This option allows the user to choose the boundary conditions. "
		"The first one correspond to the left side (surface) "
		"and second one to the right (bulk), "
		"then two for Y and two for Z. "
		"0 means mirror or periodic, 1 means free surface.")("xBCType",
		bpo::value<std::string>(&xBC),
		"The boundary conditions to use in the X direction, mirror (default) "
		"or periodic.")("burstingDepth", bpo::value<double>(&burstingDepth),
		"The depth (in nm) after which there is an exponential decrease in the "
		"probability of bursting (10.0 nm if nothing is specified).")(
		"burstingFactor", bpo::value<double>(&burstingFactor),
		"This option allows the user to set the factor used in computing the "
		"likelihood of a bursting event.")("rng", bpo::value<std::string>(),
		"Allows user to specify seed used to initialize random number "
		"generator (default = determined from current time) and "
		"whether each process should print the seed value "
		"it uses (default = don't print).")("density",
		bpo::value<double>(&density),
		"Sets a density in nm-3 for the number of xenon per volume in a bubble "
		"for the NE case (default is 10.162795276841 nm-3 as before).")("pulse",
		bpo::value<std::string>(),
		"The total length of the pulse (in s) if the Pulsed material is used, "
		"and the proportion of it that is "
		"ON.")("lattice", bpo::value<double>(&latticeParameter),
		"The length of the lattice side in nm.")("impurityRadius",
		bpo::value<double>(&impurityRadius),
		"The radius of the main impurity (He or Xe) in nm.")("biasFactor",
		bpo::value<double>(&biasFactor),
		"This option allows the user to set the bias factor reflecting the "
		"fact that interstitial "
		"clusters have a larger surrounding strain field.")("hydrogenFactor",
		bpo::value<double>(&hydrogenFactor),
		"The factor between the size of He and H.")("xenonDiffusivity",
		bpo::value<double>(&xenonDiffusivity),
		"The diffusion coefficient for xenon in nm2 s-1.")("fissionYield",
		bpo::value<double>(&fissionYield),
		"The number of xenon created for each fission (default is 0.25).")(
		"heVRatio", bpo::value<double>(&heVRatio),
		"The number of He atoms allowed per V in a bubble.")(
		"migrationThreshold", bpo::value<double>(&migrationThreshold),
		"Set a limit on the migration energy above which the diffusion will be "
		"ignored.")("fluxDepthProfileFilePath",
		bpo::value<fs::path>(&fluxDepthProfileFilePath),
		"The path to the custom flux profile file; the default is an empty "
		"string that will use the default material associated flux handler.");

	bpo::options_description visible("Allowed options");
	visible.add(desc).add(config);

	if (opts.count("help")) {
		std::cout << visible << '\n';
		shouldRunFlag = false;
		exitCode = EXIT_FAILURE;
		return;
	}

	// Check that the file exist
	std::ifstream ifs(argv[1]);
	if (!ifs) {
		std::cerr << "Options: unable to open parameter file: " << argv[1]
				  << std::endl;
		std::cout << visible << '\n';
		shouldRunFlag = false;
		exitCode = EXIT_FAILURE;
		return;
	}

	if (shouldRunFlag) {
		std::ifstream ifs(param_file);
		if (!ifs) {
			std::cerr << "Options: unable to open parameter file: "
					  << param_file << std::endl;
			exitCode = EXIT_FAILURE;
			return;
		}
		store(parse_config_file(ifs, config), opts);
		notify(opts);

		// Take care of the temperature
		if (opts.count("tempParam")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<double> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["tempParam"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();
			if (tokens.size() > 2) {
				std::cerr << "Options: too many temperature parameters (expect "
							 "2 or less)"
						  << std::endl;
				exitCode = EXIT_FAILURE;
				return;
			}
			for (std::size_t i = 0; i < tokens.size(); ++i) {
				tempParam[i] = tokens[i];
			}
		}

		if (opts.count("tempFile")) {
			// Check that the profile file exists
			std::ifstream inFile(tempProfileFilename);
			if (!inFile) {
				std::cerr << "\nOptions: could not open file containing "
							 "temperature profile data. "
							 "Aborting!\n"
						  << std::endl;
				shouldRunFlag = false;
				exitCode = EXIT_FAILURE;
			}
		}

		// Take care of the flux
		if (opts.count("flux")) {
			fluxFlag = true;
		}
		if (opts.count("fluxFile")) {
			// Check that the profile file exists
			std::ifstream inFile(fluxTimeProfileFilePath.c_str());
			if (!inFile) {
				std::cerr << "\nOptions: could not open file containing flux "
							 "profile data. "
							 "Aborting!\n"
						  << std::endl;
				shouldRunFlag = false;
				exitCode = EXIT_FAILURE;
			}
			else {
				// Set the flag to use a flux profile to true
				fluxTimeProfileFlag = true;
			}
		}

		// Take care of the performance handler
		if (opts.count("perfHandler")) {
			std::string perfHandlers[] = {"dummy", "os", "papi"};
			if (std::find(begin(perfHandlers), end(perfHandlers),
					perfHandlerName) == end(perfHandlers)) {
				std::cerr << "\nOptions: could not understand the performance "
							 "handler type. "
							 "Aborting!\n"
						  << std::endl;
				shouldRunFlag = false;
				exitCode = EXIT_FAILURE;
			}
		}

		// Take care of the visualization handler
		if (opts.count("vizHandler")) {
			// Determine the type of handlers we are being asked to use
			if (!(vizHandlerName == "std" || vizHandlerName == "dummy")) {
				std::cerr << "\nOptions: unrecognized argument in the "
							 "visualization option handler."
							 "Aborting!\n"
						  << std::endl;
				shouldRunFlag = false;
				exitCode = EXIT_FAILURE;
			}
		}

		// Take care of the grid
		if (opts.count("gridParam")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<double> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["gridParam"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();
			if (tokens.size() > 6) {
				std::cerr
					<< "Options: too many grid parameters (expect 6 or less)"
					<< std::endl;
				exitCode = EXIT_FAILURE;
				return;
			}
			for (std::size_t i = 0; i < tokens.size(); ++i) {
				gridParam[i] = tokens[i];
			}
		}

		if (opts.count("gridFile")) {
			// Check that the file exists
			std::ifstream inFile(gridFilename);
			if (!inFile) {
				std::cerr << "\nOptions: could not open file containing "
							 "grid data. "
							 "Aborting!\n"
						  << std::endl;
				shouldRunFlag = false;
				exitCode = EXIT_FAILURE;
			}
		}

		// Take care of the radius minimum size
		if (opts.count("radiusSize")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<int> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["radiusSize"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();

			// Set the values
			for (int i = 0; i < tokens.size(); i++) {
				radiusMinSizes.push_back(tokens[i]);
			}
		}

		// Take care of the processes
		if (opts.count("process")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<std::string> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["process"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();

			// Initialize the map of processes
			processMap["reaction"] = false;
			processMap["diff"] = false;
			processMap["advec"] = false;
			processMap["modifiedTM"] = false;
			processMap["movingSurface"] = false;
			processMap["bursting"] = false;
			processMap["attenuation"] = false;
			processMap["resolution"] = false;
			processMap["heterogeneous"] = false;
			processMap["sink"] = false;

			// Loop on the tokens
			for (int i = 0; i < tokens.size(); ++i) {
				// Look for the key
				if (processMap.find(tokens[i]) == processMap.end()) {
					// Send an error
					std::cerr << "\nOptions: The process name is not known: "
							  << tokens[i] << std::endl
							  << "Aborting!\n"
							  << std::endl;
					shouldRunFlag = false;
					exitCode = EXIT_FAILURE;
				}
				else {
					// Switch the value to true in the map
					processMap[tokens[i]] = true;
				}
			}
		}

		// Take care of the gouping
		if (opts.count("grouping")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<int> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["grouping"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();

			// Set grouping minimum size
			groupingMin = tokens[0];
			// Set the grouping width in the first direction
			groupingWidthA = tokens[1];
			// Set the grouping width in the second direction
			if (tokens.size() > 2)
				groupingWidthB = tokens[2];
		}

		// Take care of the network parameters
		if (opts.count("netParam")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<std::string> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["netParam"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();

			// Set the flag to not use the HDF5 file
			useHDF5Flag = false;

			// Set the value for the impurities
			maxImpurity = strtol(tokens[0].c_str(), NULL, 10);

			// Check if we have other values
			if (tokens.size() > 1) {
				// Set the deuterium size
				maxD = strtol(tokens[1].c_str(), NULL, 10);
				// Set the tritium size
				maxT = strtol(tokens[2].c_str(), NULL, 10);
				// Set the vacancy size
				maxV = strtol(tokens[3].c_str(), NULL, 10);
				// Set the interstitial size
				maxI = strtol(tokens[4].c_str(), NULL, 10);
			}
		}

		// Take care of the boundary conditions
		if (opts.count("boundary")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<int> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["boundary"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();

			// Set the left boundary
			leftBoundary = tokens[0];
			// Set the right boundary
			rightBoundary = tokens[1];
			if (tokens.size() > 2)
				// Set the bottom boundary
				bottomBoundary = tokens[2];
			if (tokens.size() > 3)
				// Set the top boundary
				topBoundary = tokens[3];
			if (tokens.size() > 4)
				// Set the front boundary
				frontBoundary = tokens[4];
			if (tokens.size() > 5)
				// Set the back boundary
				backBoundary = tokens[5];
		}

		// Take care of the rng
		if (opts.count("rng")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<std::string> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["rng"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();
			try {
				size_t currIdx = 0;

				// Determine whether we should print the seed value.
				bool shouldPrintSeed = false;
				if (tokens[currIdx] == "print") {
					shouldPrintSeed = true;
					++currIdx;
				}
				rngPrintSeed = shouldPrintSeed;

				if (currIdx < tokens.size()) {
					// Convert arg to an integer.
					char* ep = NULL;
					auto useed = strtoul(tokens[currIdx].c_str(), &ep, 10);
					if (ep !=
						(tokens[currIdx].c_str() + tokens[currIdx].length())) {
						std::cerr
							<< "\nOptions: Invalid random number generator "
							   "seed, must be a non-negative integer."
							   "Aborting!\n"
							<< std::endl;
					}
					setRNGSeed(useed);
				}
			}
			catch (const std::invalid_argument& e) {
				std::cerr
					<< "\nOptions: unrecognized argument in setting the rng."
					   "Aborting!\n"
					<< std::endl;
				shouldRunFlag = false;
				exitCode = EXIT_FAILURE;
			}
		}

		// Take care of the flux pulse
		if (opts.count("pulse")) {
			// Build an input stream from the argument string.
			util::TokenizedLineReader<double> reader;
			auto argSS = std::make_shared<std::istringstream>(
				opts["pulse"].as<std::string>());
			reader.setInputStream(argSS);

			// Break the argument into tokens.
			auto tokens = reader.loadLine();

			pulseTime = tokens[0];
			pulseProportion = tokens[1];
		}
	}

	return;
}

} // end namespace options
} // end namespace xolotl