#include <ros/ros.h>
#include <iostream>
#include <Eigen/Dense>
#include <mutex>
#include <thread>

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

#define PI 3.1415926535

std::mutex global_mtx_;
Eigen::VectorXf start_point(6);
Eigen::VectorXf robot_localization(7);  // x y yaw liner_v angle_v liner_acc angle_acc
std::vector<Eigen::Vector3f> Trajectory;

tf::TransformListener* listener;
ros::Publisher         vel_publisher_;
ros::Publisher         path_publisher_;
ros::Publisher         odom_publisher_;

bool get_new_trajectory = false;
bool end_flag = false;
bool get_localization = false;

/********************************************pure pursuit*********************************************/
float pure_pursuit_k = 0.1;
float pure_pursuit_Lfc = 2;
float pure_pursuit_Kp = 1.0;
float dt = 0.1;
float pure_pursuit_L = 2.7;
float pure_pursuit_liner_v = 5;
/*****************************************************************************************************/

float get_yaw(const geometry_msgs::Quaternion& q){
    tf::Quaternion quaternion;
    tf::quaternionMsgToTF(q, quaternion);
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);
    return float(yaw);
}

float calculation_yaw_difference(float now_yaw, float last_yaw){
    if(now_yaw - last_yaw > PI){
        return (2 * PI - now_yaw + last_yaw);
    }
    else if( now_yaw - last_yaw < -PI){
        return (2 * PI +now_yaw - last_yaw);
    }
    else{
        return (now_yaw - last_yaw);
    }
}

Eigen::Vector2f calculation_velocity(const Eigen::Vector3f& last_pose, const Eigen::Vector3f& now_pose, float delta_time){
    float liner_v = (now_pose.block<2,1>(0,0) - last_pose.block<2,1>(0,0)).norm() / delta_time;
    float angle_v = calculation_yaw_difference(now_pose(2), last_pose(2)) / delta_time;
    if(abs(liner_v) < 0.001)
        liner_v = 0.0;
    if(abs(angle_v) < 0.001)
        angle_v = 0.0;
    return Eigen::Vector2f(liner_v, angle_v);
}

Eigen::Vector2f calculation_acceleration(const Eigen::Vector2f& last_v, const Eigen::Vector2f& now_v, const Eigen::Vector3f& last_pose, const Eigen::Vector3f& now_pose){
    float x = (now_pose.block<2,1>(0,0) - last_pose.block<2,1>(0,0)).norm();
    float yaw = calculation_yaw_difference(now_pose(2), last_pose(2));
    float acc_x = (now_v(0) * now_v(0) - last_v(0) * last_v(0)) / (2.0 * x);
    float acc_a = (now_v(1) * now_v(1) - last_v(1) * last_v(1)) / (2.0 * yaw);
    if(abs(x) < 0.001)
        acc_x = 0.0;
    if(abs(yaw) < 0.001)
        acc_a = 0.0;
    return Eigen::Vector2f(acc_x, acc_a);
}

void odom_callback(const ros::TimerEvent&){

    tf::StampedTransform transform;
    static bool first_step = false;
    try{
        auto rostime = ros::Time(0);
        if(!first_step){
            ros::Duration(1).sleep();
            first_step = true;
        }
        else
            ros::Duration(0.1).sleep();
        listener->lookupTransform("/map","catvehicle/base_link",rostime,transform);
    }
    catch(tf::TransformException &ex){
        ROS_ERROR("%s", ex.what());
        return;
    }

    geometry_msgs::Quaternion q;
    q.x = transform.getRotation().x();
    q.y = transform.getRotation().y();
    q.z = transform.getRotation().z();
    q.w = transform.getRotation().w();

    Eigen::Vector3f now_T(transform.getOrigin().x(), transform.getOrigin().y(), get_yaw(q));
    static bool first_cal = true;
    static Eigen::Vector3f history_T = now_T;
    static Eigen::Vector2f history_velocity = Eigen::Vector2f::Zero();
    static ros::Time t0 = ros::Time::now();
    ros::Time t1 = ros::Time::now();

    global_mtx_.lock();
    start_point(0) = transform.getOrigin().x();
    start_point(1) = transform.getOrigin().y();
    start_point(2) = transform.getRotation().w();
    start_point(3) = transform.getRotation().x();
    start_point(4) = transform.getRotation().y();
    start_point(5) = transform.getRotation().z();
    if(first_cal){
        first_cal = false;
        global_mtx_.unlock();
        return;
    }
    auto robot_velocity = calculation_velocity(history_T, now_T, (t1 - t0).toSec());
    auto robot_acceleration = calculation_acceleration(history_velocity, robot_velocity, history_T, now_T);
    robot_localization << now_T(0), now_T(1), now_T(2),
                          robot_velocity(0), robot_velocity(1),
                          robot_acceleration(0), robot_acceleration(1);
    global_mtx_.unlock();
    get_localization = true;
    history_velocity = robot_velocity;
    history_T = now_T;
    t0 = t1;
}

//void path_callback(const visualization_msgs::MarkerArray::ConstPtr& msg){
//
//    if (msg->markers.size() == 0)
//        return;
//    global_mtx_.lock();
//    Trajectory.clear();
//    nav_msgs::Path path_msg;
//    nav_msgs::Odometry odom_msg;
//    path_msg.header.frame_id = "/map";
//    path_msg.header.stamp = ros::Time::now();
//    odom_msg.header.frame_id = "/map";
//    odom_msg.header.stamp = ros::Time::now();
//    path_publisher_.publish(path_msg);
//    for(size_t num = 0; num < msg->markers.size(); num++){
//        geometry_msgs::PoseStamped poses;
//        poses.pose.position.x  = msg->markers[num].pose.position.x;
//        poses.pose.position.y  = msg->markers[num].pose.position.y;
//        poses.pose.orientation = msg->markers[num].pose.orientation;
//        odom_msg.pose.pose = poses.pose;
//        odom_publisher_.publish(odom_msg);
//        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
//
//        path_msg.poses.push_back(poses);
//
//        Trajectory.push_back(Eigen::Vector3f(msg->markers[num].pose.position.x,
//                                             msg->markers[num].pose.position.y,
//                                             get_yaw(msg->markers[num].pose.orientation)));
//        std::cout << "Trajectory : " << Trajectory.back().transpose() << std::endl;
//    }
//    path_publisher_.publish(path_msg);
//    std::reverse(Trajectory.begin(),Trajectory.end());
//    global_mtx_.unlock();
//    get_new_trajectory = true;
//
//}
void path_callback(const nav_msgs::Path::ConstPtr& msg){

    if (msg->poses.size() == 0)
        return;
    global_mtx_.lock();
    Trajectory.clear();
    nav_msgs::Path path_msg;
    nav_msgs::Odometry odom_msg;
    path_msg.header.frame_id = "/map";
    path_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = "/map";
    odom_msg.header.stamp = ros::Time::now();
    path_publisher_.publish(path_msg);
    for(size_t num = 0; num < msg->poses.size(); num++){
        geometry_msgs::PoseStamped poses;
        poses.pose.position.x  = msg->poses[num].pose.position.x;
        poses.pose.position.y  = msg->poses[num].pose.position.y;
        if(num == 0)
            poses.pose.orientation = msg->poses[num + 1].pose.orientation;
        else
            poses.pose.orientation = msg->poses[num].pose.orientation;
        odom_msg.pose.pose = poses.pose;
//        odom_publisher_.publish(odom_msg);
//        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        path_msg.poses.push_back(poses);

        Trajectory.push_back(Eigen::Vector3f(msg->poses[num].pose.position.x,
                                             msg->poses[num].pose.position.y,
                                             get_yaw(msg->poses[num].pose.orientation)));
    }
    path_publisher_.publish(path_msg);
    std::reverse(Trajectory.begin(),Trajectory.end());
    global_mtx_.unlock();
    get_new_trajectory = true;

}

int calculation_current_index(){
    global_mtx_.lock();
    Eigen::Vector3f current_state = robot_localization.block<3,1>(0,0);
    int index = 0;
    float diff_distance_ = 1000000.0;
    for(size_t index_ = 0; index_ < Trajectory.size(); index_++){
        float temp_diff_distance_ = (current_state.block<2,1>(0,0) - Trajectory[index_].block<2,1>(0,0)).norm();
        if(temp_diff_distance_ <= diff_distance_){
            diff_distance_ = temp_diff_distance_;
            index = index_;
        }
    }

    float  Lf = pure_pursuit_k * robot_localization(3) + pure_pursuit_Lfc;  // 前视距离
    float L = 0.0;
    while(Lf > L && (index+1) < Trajectory.size()){
        Eigen::Vector2f diff_pts = Trajectory[index+1].block<2,1>(0,0) - Trajectory[index].block<2,1>(0,0);
        L += diff_pts.norm();
        index++;
    }
    global_mtx_.unlock();
    return index;
}

float calculation_control_liner_acceleration(float current_velocity){
    global_mtx_.lock();
    float acc = pure_pursuit_Kp * (pure_pursuit_liner_v - current_velocity);
    global_mtx_.unlock();
    return acc;
}

float calculation_control_angle_acceleration(int& current_index){
    int index = calculation_current_index();
    if(current_index >= index)
        index = current_index;
    Eigen::Vector3f current_point;
    if(index < Trajectory.size())
        current_point = Trajectory[index];
    else{
        current_point = Trajectory.back();
        index = Trajectory.size() - 1;
    }
    auto diff_P = current_point - robot_localization.block<3,1>(0,0);
    float alpha = std::atan2(diff_P.y(), diff_P.x()) - robot_localization(2);

    if(robot_localization(3) < 0){
        alpha = PI - alpha;
    }
    float Lf = pure_pursuit_k * robot_localization(3) + pure_pursuit_Lfc;
    float delta = std::atan2(2.0 * pure_pursuit_L * sin(alpha) / Lf, 1.0);
    current_index = index;
    static float delta_ = 0.0;
    float return_delta_ = (delta - delta_);
    delta_ = delta;
    return delta;
}

void cmd_publish(float acc_liner, float acc_angle, float dt){
    global_mtx_.lock();
    float v_l = robot_localization(3) + acc_liner * dt;
//    float v_a = robot_localization(4) + acc_angle * dt;
     float v_a = acc_angle - robot_localization(4);
    // float v_a = acc_angle;
    geometry_msgs::Twist vel_cmd;
    vel_cmd.linear.x = v_l;
    vel_cmd.angular.z = v_a;
    global_mtx_.unlock();
    std::cout << "\e[1;33;49m >>> v_l ：" << v_l << " v_a : "<< v_a <<" \e[0m"<< std::endl;
//    std::cout << "\e[1;33;49m >>> acc_liner ：" << acc_liner << " \e[0m" << std::endl;
    vel_publisher_.publish(vel_cmd);
}

void zero_velocity(){
    geometry_msgs::Twist vel_cmd;
    vel_cmd.linear.x = 0.0;
    vel_cmd.angular.z = 0.0;
    vel_publisher_.publish(vel_cmd);
}

bool arrived_goal(){
    std::cout << "\e[1;33;49m >>> to the goal" << (Trajectory.back().block<2,1>(0,0) - robot_localization.block<2,1>(0,0)).norm() << "m \e[0m"<< std::endl;
    if((Trajectory.back().block<2,1>(0,0) - robot_localization.block<2,1>(0,0)).norm() < 1.0) {
        std::cout << "\e[1;33;49m >>> reached! \e[0m"<< std::endl;
        return true;
    }
    else
        return false;
}

void pure_pursuit(){
    while(true){
        std::cout << "\e[1;33;49m >>> wait for the path\e[0m"<< std::endl;
        while(true) {
            if(end_flag)
                break;
            if (get_new_trajectory && get_localization) {
                std::cout << "\e[1;33;49m >>> new path! \e[0m"<< std::endl;
                get_new_trajectory = false;
                break;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        std::cout << "\e[1;33;49m >>> tracking \e[0m"<< std::endl;
        int index = calculation_current_index();
        ros::Time t_0 = ros::Time::now();
        std::cout << "\e[1;33;49m >>> Trajectory.size() : "<< Trajectory.size() << "\e[0m"<< std::endl;
        std::cout << "\e[1;33;49m >>> index : "<< index << "\e[0m"<< std::endl;
        if(Trajectory.size() == 0)
            continue;
        while(index < Trajectory.size() - 1){
            if(arrived_goal()) {
                zero_velocity();
                break;
            }
            if(get_new_trajectory)
                break;
            index = calculation_current_index();
            std::cout << "\e[1;33;49m >>> index : "<< index << "\e[0m"<< std::endl;
            std::cout << "\e[1;33;49m >>> Trajectory(index) : "<< Trajectory[index].transpose() << "\e[0m"<< std::endl;
            std::cout << "\e[1;33;49m >>> robot_localization : "<< robot_localization.transpose() << "\e[0m"<< std::endl;
            float acc_liner = calculation_control_liner_acceleration(robot_localization(3));
            float acc_angle = calculation_control_angle_acceleration(index);
            ros::Time t_1 = ros::Time::now();
            float delta_t = (t_1 - t_0).toSec();
            cmd_publish(acc_liner, acc_angle, delta_t);
            t_0 = t_1;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        zero_velocity();

        if(end_flag)
            break;
    }
}

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

    ros::init(argc, argv, "pure_pursuit");
    ros::NodeHandle nh("~");

    tf::TransformListener listener_;
    listener = &listener_;

//    ros::Subscriber path_sub =  nh.subscribe<visualization_msgs::MarkerArray>("/new_path_vehicle_node", 100, &path_callback);
    ros::Subscriber path_sub =  nh.subscribe<nav_msgs::Path>("/new_path", 100, &path_callback);

    vel_publisher_ = nh.advertise<geometry_msgs::Twist>("/catvehicle/cmd_vel_safe", 1);
    path_publisher_= nh.advertise<nav_msgs::Path>("/visual_path",1, true);
    odom_publisher_= nh.advertise<nav_msgs::Odometry>("/odom_path",1, true);

    ros::Timer      odom_timer = nh.createTimer(ros::Duration(0.1), &odom_callback);

    std::thread pure_pursuit_thread(pure_pursuit);
    pure_pursuit_thread.detach();

    ros::spin();
    end_flag = true;
    zero_velocity();
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    return 0;
}


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include <gcs/constraints/linear_equality.hh>
#include <gcs/problem.hh>
#include <gcs/solve.hh>

#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

using namespace gcs;

using std::cerr;
using std::cout;
using std::endl;
using std::string;
using std::to_string;
using std::pair;
using std::vector;

using namespace std::literals::string_literals;

namespace po = boost::program_options;

auto main(int argc, char * argv[]) -> int
{
    po::options_description display_options{ "Program options" };
    display_options.add_options()
        ("help", "Display help information")
        ("prove", "Create a proof");

    po::options_description all_options{ "All options" };

    all_options.add(display_options);

    po::variables_map options_vars;

    try {
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .run(), options_vars);
        po::notify(options_vars);
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }

    if (options_vars.count("help")) {
        cout << "Usage: " << argv[0] << " [options]" << endl;
        cout << endl;
        cout << display_options << endl;
        return EXIT_SUCCESS;
    }

    Problem p = options_vars.count("prove") ? Problem{ Proof{ "cake.opb", "cake.veripb" } } : Problem{ };

    // https://www.minizinc.org/doc-2.5.5/en/modelling.html#an-arithmetic-optimisation-example
    auto banana = p.create_integer_range_variable(0_i, 100_i);
    auto chocolate = p.create_integer_range_variable(0_i, 100_i);
    p.post(LinearLessEqual{ Linear{ { 250_i, banana }, { 200_i, chocolate} }, 4000_i });
    p.post(LinearLessEqual{ Linear{ { 2_i, banana } }, 6_i });
    p.post(LinearLessEqual{ Linear{ { 75_i, banana },  { 150_i, chocolate} }, 2000_i });
    p.post(LinearLessEqual{ Linear{ { 100_i, banana }, { 150_i, chocolate} }, 500_i });
    p.post(LinearLessEqual{ Linear{ { 75_i, chocolate} }, 500_i });

    auto profit = p.create_integer_range_variable(0_i, 107500_i, "profit");
    p.post(LinearEquality{ Linear{ { 400_i, banana }, { 450_i, chocolate }, { -1_i, profit } }, 0_i });

    auto loss = p.create_integer_range_variable(-107500_i, 0_i, "loss");
    p.post(LinearEquality{ Linear{ { 1_i, profit }, { 1_i, loss } }, 0_i });

    p.branch_on(vector<IntegerVariableID>{ banana, chocolate });
    p.minimise(loss);
    auto stats = solve(p, [&] (const State & s) -> bool {
            cout << "banana cakes = " << s(banana) << ", chocolate cakes = " << s(chocolate) << ", profit = " << s(profit) << endl;
            return true;
            });

    cout << stats;

    return EXIT_SUCCESS;
}


#include <algorithm>
#include <cmath>
#include <iostream>
#include <map>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
using namespace Eigen;

#include "lstm/network.hpp"
#include "util.hpp"

int main() {
    // load data
    std::vector<std::vector<std::string>> X_train, y_train, X_dev, y_dev, X_test, y_test;
    util::load_dataset(X_train, "data/train/src.txt");
    util::load_dataset(y_train, "data/train/tgt.txt");
    util::load_dataset(X_dev, "data/dev/src.txt");
    util::load_dataset(y_dev, "data/dev/tgt.txt");
    util::load_dataset(X_test, "data/test/src.txt");
    util::load_dataset(y_test, "data/test/tgt.txt");

    // create vocabs
    std::map<std::string, int> word_to_idx;
    std::map<int, std::string> idx_to_word;
    util::generate_vocabs(X_train, word_to_idx, idx_to_word);

    std::cout << "Vocabulary (idx: token) with " << word_to_idx.size() << " tokens" << std::endl;
    std::cout << "--------------------------------------" << std::endl;
    for (const auto &p : idx_to_word) {
        std::cout << p.first << ": " << p.second << std::endl;
    }

    // create LSTM network
    // nn::LSTMCell lstm(128, 64, 64);
    nn::LSTMNetwork lstm(4, 128, 10, 32, 64);
    MatrixXf input(64, 10);
    input.setRandom();

    // train the model
    MatrixXf probs = lstm(input);
    std::cout << probs.rows() << "x" << probs.cols() << std::endl;

    return 0;
}


#include "nonlinear_elastic_energy.hpp"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

void NonlinearElasticEnergy::precompute(const TriMesh& mesh)
{
    elements_.resize(mesh.f.rows());

    Mat3x2d selector;
    selector << 0.0, 0.0,
                1.0, 0.0,
                0.0, 1.0;

    for (int idx=0; idx<mesh.f.rows(); idx++) {
        int i0 = mesh.f(idx, 0);
        int i1 = mesh.f(idx, 1);
        int i2 = mesh.f(idx, 2);

        Mat2d Dm;
        Dm.col(0) = mesh.u.segment<2>(2*i1) - mesh.u.segment<2>(2*i0);
        Dm.col(1) = mesh.u.segment<2>(2*i2) - mesh.u.segment<2>(2*i0);

        const Real A = 0.5 * (Vec3d() << Dm.col(0), 0.0).finished().cross(
                             (Vec3d() << Dm.col(1), 0.0).finished()).norm();

        elements_[idx].A = A;

        for (size_t i=0; i<2; i++)
            elements_[idx].edgeLengths[i] = Dm.col(i).norm();

        Mat3d P;
        P <<   1.0,   1.0,   1.0,
            mesh.u[2*i0], mesh.u[2*i1], mesh.u[2*i2],
            mesh.u[2*i0+1], mesh.u[2*i1+1], mesh.u[2*i2+1];

        // 2x3 matrix containing the gradients of the
        // 3 shape functions (one for each node)
        elements_[idx].Bm = A * (P.inverse() * selector).transpose();

        elements_[idx].DmInverse = Dm.inverse();

        Mat2d DmI = elements_[idx].DmInverse;
        elements_[idx].dFdu = MatrixXd::Zero(6,9);
        elements_[idx].dFdu << -DmI(0,0)-DmI(1,0), 0.0, 0.0, DmI(0,0), 0.0, 0.0, DmI(1,0), 0.0, 0.0,
                               -DmI(0,1)-DmI(1,1), 0.0, 0.0, DmI(0,1), 0.0, 0.0, DmI(1,1), 0.0, 0.0,
                                0.0, -DmI(0,0)-DmI(1,0), 0.0, 0.0, DmI(0,0), 0.0, 0.0, DmI(1,0), 0.0,
                                0.0, -DmI(0,1)-DmI(1,1), 0.0, 0.0, DmI(0,1), 0.0, 0.0, DmI(1,1), 0.0,
                                0.0, 0.0, -DmI(0,0)-DmI(1,0), 0.0, 0.0, DmI(0,0), 0.0, 0.0, DmI(1,0),
                                0.0, 0.0, -DmI(0,1)-DmI(1,1), 0.0, 0.0, DmI(0,1), 0.0, 0.0, DmI(1,1);

    }
}

void NonlinearElasticEnergy::getForceAndHessian(const TriMesh& mesh,
                                                const VecXd& x,
                                                VecXd& F,
                                                SparseMatrixd& dFdx,
                                                SparseMatrixd& dFdv) const {
    assert(F.size() >= x.size());

    for (int idx=0; idx<mesh.f.rows(); idx++) {
        // We are assuming that poisson ratio is 0.0, and thus, lambda = 0.0
        // Convert (E,v) (Young's modulus and poisson ratio) to lame parameters
        Real mu = ksx_ * 0.5;

        int idxs[3] = { mesh.f(idx,0), mesh.f(idx,1), mesh.f(idx,2) };

        Mat3x2d Dw;
        for (int i=0; i<2; i++) {
            Dw.col(i) = x.segment<3>(3*idxs[i+1]) - x.segment<3>(3*idxs[0]);
        }

        Vec2d ls = Dw.colwise().norm();
        for (int i=0; i<2; i++) {
            if (ls[i] > 2.5 * elements_[idx].edgeLengths[i]) {
                Dw.col(i) *= 2.5 * elements_[idx].edgeLengths[i] / ls[i];
            }
        }

        // Compute the deformation gradient Fdg,
        // 1st and 2nd Piola-Kirchoff stress tensors (S and P)
        Mat3x2d Fdg = Dw * elements_[idx].DmInverse;
	Mat2d     S = mu * (Fdg.transpose() * Fdg - Mat2d::Identity());
        Mat3x2d   P = Fdg * S;

        // Force is the 2nd P-K stress projected onto the node
        for (int i=0; i<3; i++) {
            F.segment<3>(3*idxs[i]).noalias() -= P * elements_[idx].Bm.col(i);
        }

        S = clampMatrixEigenvalues(S);

        // Derivative of 2nd P-K stress tensor w.r.t. F, the deformation gradient
        //
        // \partial S11 / \partial F = 4 mu ( F.col(0)  0 )
        // \partial S22 / \partial F = 4 mu ( 0  F.col(1) )
        // \partial S12 / \partial F = \partial S21 / \partial F = ( F.col(1)  F.col(0) )
        Mat3x2d dSdF[4];
        dSdF[0].col(0) = 2.0 * mu * Fdg.col(0);
        dSdF[0].col(1) = dSdF[3].col(0) = Vec3d::Zero();
        dSdF[3].col(1) = 2.0 * mu * Fdg.col(1);
        dSdF[1].col(0) = dSdF[2].col(0) = mu * Fdg.col(1);
        dSdF[1].col(1) = dSdF[2].col(1) = mu * Fdg.col(0);

        const Mat2x3d& dN = elements_[idx].Bm;

        // TODO: Work out compact expressions that take advantage of dFdu_jk only having one
        // non-zero row. The row is also a function of DmInverse above. No need to pre-compute more.
        Vec3d fi_jk;
        Mat2d dSdu_jk;
        for (size_t i=0; i<3; ++i)
        {
            for (size_t j=0; j<3; ++j)
            {
                for (size_t k=0; k<3; ++k) // \partial f_i / \partial u_j^k
                {
                    // 3 x 2 matrix that is the derivative of F with respect to u_j^k
                    //
                    Mat3x2d dFdu_jk = elements_[idx].dFdu.block<2,3>(2*k, 3*j).transpose();

                    for (size_t l=0; l<2; ++l)
                        for (size_t m=0; m<2; ++m)
                            dSdu_jk(l,m) = (dSdF[2*l+m].array() * dFdu_jk.array()).sum();

                    // Force on vertex i is f_i = F P dN_i, so
                    // derivative w.r.t. DOF u_j^k (j-th vertex, k-th DOF) is:
                    //
                    // \partial f_i / \partial u_j^k =
                    //     ( \partial F / \partial u_j^k S + F \partial S / \partial u_j^k ) dN_i
                    fi_jk = (dFdu_jk * S + Fdg * dSdu_jk) * dN.col(i);

                    for (size_t l=0; l<3; ++l)
                        //J.coeffRef(3*idxs[j]+k, 3*idxs[i]+l) += fi_jk[l];
                        dFdx.coeffRef(3*idxs[i]+l, 3*idxs[j]+k) -= fi_jk[l];
                }
            }
        }
    }
}

Mat2d NonlinearElasticEnergy::clampMatrixEigenvalues(Mat2d &input) const {
    static const Real eps = 1.0e-6;

    SelfAdjointEigenSolver<Mat2d> ev(input);
    assert(ev.info() == Success);

    return (ev.eigenvectors() *
            ev.eigenvalues().cwiseMax(eps).asDiagonal() *
            ev.eigenvectors().transpose());
}

void NonlinearElasticEnergy::getHessianPattern(const TriMesh& mesh, vector<SparseTripletd> &triplets) const {
    for (int idx=0; idx<mesh.f.rows(); idx++) {
        int idxs[3] = { mesh.f(idx,0), mesh.f(idx,1), mesh.f(idx,2) };
        for (size_t j=0; j<3; ++j)
            for (size_t k=0; k<3; ++k)
                for (size_t l=0; l<3; ++l)
                    for (size_t n=0; n<3; ++n)
                        triplets.push_back(SparseTripletd(3*idxs[j]+l,3*idxs[k]+n, 1.0));
    }
}


// Main code for peak bagging by means of nested sampling analysis
// Created by Enrico Corsaro @ INAF-OACT - August 2019
// e-mail: emncorsaro@gmail.com
// Source code file "Asymptotic.cpp"

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Dense>
#include "Functions.h"
#include "File.h"
#include "MultiEllipsoidSampler.h"
#include "KmeansClusterer.h"
#include "EuclideanMetric.h"
#include "Prior.h"
#include "UniformPrior.h"
#include "NormalLikelihood.h"
#include "RadialModesPatternModel.h"
#include "NonRadialModesPatternModel.h"
#include "PowerlawReducer.h"
#include "Results.h"
#include "Ellipsoid.h"
#include "PrincipalComponentProjector.h"

int main(int argc, char *argv[])
{
    // Check number of arguments for main function

    if (argc != 8)
    {
        cerr << "Usage: ./asymptotic <Catalog ID> <Star ID> <output sub-directory> <run number> <input prior base filename> "
                "<input nuMax> <input angular degree>" << endl;
        exit(EXIT_FAILURE);
    }

    // ---------------------------
    // ----- Read input data -----
    // ---------------------------

    unsigned long Nrows;
    int Ncols;
    ArrayXXd data;
    string CatalogID(argv[1]);
    string StarID(argv[2]);
    string runNumber(argv[4]);
    string inputPriorBaseName(argv[5]);
    string inputNuMax(argv[6]);
    string inputDegree(argv[7]);
    double nuMax = stod(inputNuMax);
    int angularDegree = stoi(inputDegree);

    // Read the local path for the working session from an input ASCII file
    ifstream inputFile;
    File::openInputFile(inputFile, "localPath.txt");
    File::sniffFile(inputFile, Nrows, Ncols);
    vector<string> myLocalPath;
    myLocalPath = File::vectorStringFromFile(inputFile, Nrows);
    inputFile.close();

    // Set up some string paths used in the computation
    string outputSubDirName(argv[3]);
    string baseOutputDirName = myLocalPath[0] + "results/" + CatalogID + StarID + "/";
    string outputDirName = baseOutputDirName + outputSubDirName + "/";
    string outputPathPrefix = outputDirName + runNumber + "/asymptotic_";
    string baseInputDirName = baseOutputDirName + outputSubDirName + "/data/";
    string inputFileName = baseOutputDirName + outputSubDirName + "/data/" + runNumber + ".txt";

    cout << "------------------------------------------------------ " << endl;
    cout << " Performing asymptotic fit for l = " + inputDegree + " modes in " + CatalogID + StarID << endl;
    cout << "------------------------------------------------------ " << endl;

    // Read the input dataset
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nrows, Ncols);
    data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    inputFile.close();

    // Creating frequency and PSD arrays
    ArrayXd covariates = data.col(0);
    ArrayXd observations = data.col(1);
    ArrayXd uncertainties = data.col(2);

    // -------------------------------------------------------
    // ----- First step. Set up all prior distributions ------
    // -------------------------------------------------------

    unsigned long Nparameters;              // Number of parameters for which prior distributions are defined

    // ---- Read prior hyper parameters for resolved modes -----
    inputFileName = outputDirName + inputPriorBaseName + "_" + runNumber + ".txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    ArrayXXd hyperParameters;

    if (Ncols == 1)
    {
        Ncols = 3;
        hyperParameters.conservativeResize(Nparameters,Ncols);
    }

    hyperParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    ArrayXd hyperParametersMinima = hyperParameters.col(0);
    ArrayXd hyperParametersMaxima = hyperParameters.col(1);
    // ---------------------------------------------------------

    int Ndimensions = Nparameters;              // Total number of dimensions of the peak bagging model

    if (angularDegree == 0)
    {
        if (Nparameters != 3)
        {
            cerr << "Wrong number of input prior hyper-parameters." << endl;
            cerr << "When performing an asymptotic fit for radial modes, three lines are " << endl;
            cerr << "expected from the input prior list (1 - DeltaNu, 2 - epsilon, 3 - alpha)." << endl;
        }
    }
    else
    {
        if (Nparameters != 5)
        {
            cerr << "Wrong number of input prior hyper-parameters." << endl;
            cerr << "When performing an asymptotic fit for radial modes, five lines are " << endl;
            cerr << "expected from the input prior list (1 - DeltaNu, 2 - epsilon, 3 - deltaNu0Degree, 4 - alpha, 5 - beta)." << endl;
        }
    }

    // Uniform Prior

    int NpriorTypes = 1;                                        // Total number of prior types included in the computation
    vector<Prior*> ptrPriors(NpriorTypes);

    double DeltaNu = 0;
    double epsilon = 0;
    double alpha = -99;
    double beta = -99;
    ArrayXd parametersMinima;                      // Minima for prior PDF
    ArrayXd parametersMaxima;                      // Maxima for prior PDF

    if (angularDegree == 0)
    {
        parametersMinima.resize(Ndimensions);
        parametersMaxima.resize(Ndimensions);
        parametersMinima << hyperParametersMinima;
        parametersMaxima << hyperParametersMaxima;

        if (hyperParametersMinima(1) == hyperParametersMaxima(1))
        {
            // In this case epsilon is a fixed value. Then reduce the number
            // of free parameters by 1 and remove the epsilon prior line from
            // the list of input prior parameters (only Deltanu and alpha are left).

            Ndimensions--;
            parametersMinima.resize(Ndimensions);
            parametersMaxima.resize(Ndimensions);
            parametersMinima << hyperParametersMinima(0), hyperParametersMinima(2);
            parametersMaxima << hyperParametersMaxima(0), hyperParametersMaxima(2);
            epsilon = hyperParametersMinima(1);
        }

        if (hyperParametersMinima(2) == hyperParametersMaxima(2))
        {
            // In this case alpha is a fixed value. Then reduce the number
            // of free parameters by 1 and remove the alpha prior line from
            // the list of input prior parameters (only DeltaNu and epsilon, or only DeltaNu are left).

            Ndimensions--;
            parametersMinima.conservativeResize(Ndimensions);
            parametersMaxima.conservativeResize(Ndimensions);
            alpha = hyperParametersMinima(2);
        }
    }

    if (angularDegree == 1 || angularDegree == 2 || angularDegree == 3)
    {
        // Here DeltaNu, epsilon are fixed parameters. Then reduce the
        // number of free parameters by 2 and remove DeltaNu and epsilon
        // prior lines from the list of input prior parameters.

        Ndimensions = 3;
        parametersMinima.resize(Ndimensions);
        parametersMaxima.resize(Ndimensions);
        parametersMinima << hyperParametersMinima.segment(2,Ndimensions);
        parametersMaxima << hyperParametersMaxima.segment(2,Ndimensions);
        DeltaNu = hyperParametersMinima(0);
        epsilon = hyperParametersMinima(1);

        // Check if alpha is fixed or not. If fixed, remove it from the prior
        // list and set its value to the one given in the prior file.

        if (hyperParametersMinima(3) == hyperParametersMaxima(3))
        {
            Ndimensions--;
            parametersMinima.resize(Ndimensions);
            parametersMaxima.resize(Ndimensions);
            parametersMinima << hyperParametersMinima(2), hyperParametersMinima(4);
            parametersMaxima << hyperParametersMaxima(2), hyperParametersMaxima(4);
            alpha = hyperParametersMinima(3);
        }

        // Check if beta is fixed or not. If fixed, remove it from the prior
        // list and set its value to the one given in the prior file.

        if (hyperParametersMinima(4) == hyperParametersMaxima(4))
        {
            Ndimensions--;
            parametersMinima.conservativeResize(Ndimensions);
            parametersMaxima.conservativeResize(Ndimensions);
            beta = hyperParametersMinima(4);
        }
    }

    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior;

    string fullPathHyperParameters = outputPathPrefix + "hyperParametersUniform.txt";
    uniformPrior.writeHyperParametersToFile(fullPathHyperParameters);

    // -------------------------------------------------------------------
    // ---- Second step. Set up the asymptotic model ---------------------
    // -------------------------------------------------------------------

    Model *model = nullptr;

    if (angularDegree == 0)
    {
        model = new RadialModesPatternModel(covariates, nuMax, epsilon, alpha);
    }

    if (angularDegree > 0)
    {
        model = new NonRadialModesPatternModel(covariates, angularDegree, nuMax, DeltaNu, epsilon, alpha, beta);
    }

    // -----------------------------------------------------------------
    // ---- Third step. Set up the likelihood function to be used ------
    // -----------------------------------------------------------------

    NormalLikelihood likelihood(observations, uncertainties, *model);

    // -------------------------------------------------------------------------------
    // ----- Fourth step. Set up the X-means clusterer using an Euclidean metric -----
    // -------------------------------------------------------------------------------

    inputFileName = outputDirName + "Xmeans_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);

    if (Nparameters != 2)
    {
        cerr << "Wrong number of input parameters for X-means algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    ArrayXd configuringParameters;
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    int minNclusters = configuringParameters(0);
    int maxNclusters = configuringParameters(1);

    if ((minNclusters <= 0) || (maxNclusters <= 0) || (maxNclusters < minNclusters))
    {
        cerr << "Minimum or maximum number of clusters cannot be <= 0, and " << endl;
        cerr << "minimum number of clusters cannot be larger than maximum number of clusters." << endl;
        exit(EXIT_FAILURE);
    }

    int Ntrials = 10;
    double relTolerance = 0.01;     // k-means

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = false;
    EuclideanMetric myMetric;

    KmeansClusterer clusterer(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // ---------------------------------------------------------------------
    // ----- Fifth step. Configure and start nested sampling inference -----
    // ---------------------------------------------------------------------

    inputFileName = outputDirName + "NSMC_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    configuringParameters.setZero();
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    if (Nparameters > 9 || Nparameters < 8)
    {
        cerr << "Wrong number of input parameters for NSMC algorithm." << endl;
        cerr << "There must be either 8 or 9 parameters. In this case the last parameter is always ignored." << endl;
        exit(EXIT_FAILURE);
    }

    // Print results on the screen

    bool printOnTheScreen = true;

    // Initial number of live points

    int initialNobjects = configuringParameters(0);

    // Minimum number of live points

    int minNobjects = configuringParameters(1);

    // Maximum number of attempts when trying to draw a new sampling point

    int maxNdrawAttempts = configuringParameters(2);

    // The first N iterations, we assume that there is only 1 cluster

    int NinitialIterationsWithoutClustering = configuringParameters(3);

    // Clustering is only happening every N iterations.

    int NiterationsWithSameClustering = configuringParameters(4);

    // Fraction by which each axis in an ellipsoid has to be enlarged
    // It can be a number >= 0, where 0 means no enlargement. configuringParameters(5)
    // Calibration from Corsaro et al. (2018)

    double initialEnlargementFraction = 0.369*pow(Ndimensions,0.574);

    // Exponent for remaining prior mass in ellipsoid enlargement fraction.
    // It is a number between 0 and 1. The smaller the slower the shrinkage of the ellipsoids.

    double shrinkingRate = configuringParameters(6);

    // Termination factor for nested sampling process.

    double terminationFactor = configuringParameters(7);

    // Total maximum number of nested iterations required to carry out the computation.
    // This is used only in the multi-modal approach.

    int maxNiterations = 0;

    MultiEllipsoidSampler nestedSampler(printOnTheScreen, ptrPriors, likelihood, myMetric, clusterer,
                                        initialNobjects, minNobjects, initialEnlargementFraction, shrinkingRate);

    double tolerance = 1.e2;
    double exponent = 0.4;
    PowerlawReducer livePointsReducer(nestedSampler, tolerance, exponent, terminationFactor);

    nestedSampler.run(livePointsReducer, NinitialIterationsWithoutClustering, NiterationsWithSameClustering,
                      maxNdrawAttempts, terminationFactor, maxNiterations, outputPathPrefix);

    nestedSampler.outputFile << "# List of configuring parameters used for the ellipsoidal sampler and X-means" << endl;
    nestedSampler.outputFile << "# Row #1: Minimum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #2: Maximum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #3: Initial Enlargement Fraction" << endl;
    nestedSampler.outputFile << "# Row #4: Shrinking Rate" << endl;
    nestedSampler.outputFile << minNclusters << endl;
    nestedSampler.outputFile << maxNclusters << endl;
    nestedSampler.outputFile << initialEnlargementFraction << endl;
    nestedSampler.outputFile << shrinkingRate << endl;
    nestedSampler.outputFile << "# Other information on the run" << endl;
    nestedSampler.outputFile << "# Row #1: Local working path used" << endl;
    nestedSampler.outputFile << "# Row #2: Catalog and Star ID" << endl;
    nestedSampler.outputFile << "# Row #3: Run Directory" << endl;
    nestedSampler.outputFile << "# Row #4: Run Number" << endl;
    nestedSampler.outputFile << "# Row #5: nuMax (microHz)" << endl;
    nestedSampler.outputFile << "# Row #7: angular degree (either 0, 1, 2, or 3)" << endl;
    nestedSampler.outputFile << myLocalPath[0] << endl;
    nestedSampler.outputFile << CatalogID + StarID << endl;
    nestedSampler.outputFile << outputSubDirName << endl;
    nestedSampler.outputFile << runNumber << endl;
    nestedSampler.outputFile << nuMax << endl;
    nestedSampler.outputFile << angularDegree << endl;
    nestedSampler.outputFile.close();

    // -------------------------------------------------------
    // ----- Last step. Save the results in output files -----
    // -------------------------------------------------------

    Results results(nestedSampler);
    results.writeParametersToFile("parameter");
    results.writeLogLikelihoodToFile("logLikelihood.txt");
    results.writePosteriorProbabilityToFile("posteriorDistribution.txt");
    results.writeLogEvidenceToFile("logEvidence.txt");
    results.writeLogMeanLiveEvidenceToFile("logMeanLiveEvidence.txt");
    results.writeEvidenceInformationToFile("evidenceInformation.txt");

    // Print out parameter estimates only in the case of a uni-modal high-dimensional fit.

    double credibleLevel = 68.3;
    bool writeMarginalDistributionToFile = true;
    results.writeParametersSummaryToFile("parameterSummary.txt", credibleLevel, writeMarginalDistributionToFile);

    cout << "Process # " << runNumber << " under subdir: " + outputSubDirName + " has been completed." << endl;

    return EXIT_SUCCESS;
}


#include "bend_forces.h"

#include "../adjacency.h"

#include <Eigen/Dense>
#include <igl/edges.h>
#include <igl/sparse_cached.h>
#include <iostream>
#include <iostream>
#include <cmath>
#include <cfloat>

using namespace std;
using namespace Eigen;

typedef Eigen::Triplet<double> Tri;

template <typename T> int sgn(T val) {
	return (T(0) < val) - (val < T(0));
}

// Refer to: Discrete bending forces and their Jacobians by Tamstorf et al.
//
//         x0
//         /\
//        /  \
//     e2/    \e1
//      /  t   \
//     /        \
//    /    e0    \
//  x1------------x2
//    \          /
//     \   t~   /
//      \      /
//    e2~\    /e1~
//        \  /
//         \/
//         x3
//
// Edge orientation: e0,e2,e2~ point away from x1
//                      e1,e1~ point away from x2
// E4 keeps them in the following order: x1, x2, x0, x3

Bend::Bend() {}

void Bend::init(
	const double k_bend,
	const double k_damping,
	const MatrixXd& X,			// in: vertex positions
	const MatrixXi& T			// in: mesh triangles
) {
	this->k_bend = k_bend;
	this->k_damping = k_damping;
	this->T = T;
	this->n = X.rows();
	this->m = T.rows();

	// create adjacency information - E4: list of 4 vertices for each face pair (each internal edge)
	vector< vector<int> > VF_adj;
	igl::edges(T, E);
	createVertexFaceAdjacencyList(T, VF_adj);
	createFaceEdgeAdjecencyList(T, E, VF_adj, FE_adj);
	createFacePairEdgeListWith4VerticeIDs(T, E, VF_adj, E4, EF6, EF_adj);
	this->no_edges = E4.rows();

	// define size of output
	n3 = 3 * n;
	k_entries = no_edges * 16 * 9;

	// precompute phi and scaling coefficient
	phi_bar.resize(no_edges);
	phi_hat.resize(no_edges);
	a.resize(no_edges);
	precompute_rest_shape(X);
}

void Bend::precompute_rest_shape(const MatrixXd & X) {
	for (int e = 0; e < no_edges; e++) {
		if (EF_adj(e, 1) != -1) { // no boundary edge
			// edges
			Vector3d e0 = X.row(E4(e, 2)) - X.row(E4(e, 1));
			Vector3d e2 = X.row(E4(e, 0)) - X.row(E4(e, 1));
			Vector3d e2_dot = X.row(E4(e, 3)) - X.row(E4(e, 1));

			// normals
			Vector3d normal = e0.cross(e2).normalized();
			Vector3d normal_dot = e2_dot.cross(e0).normalized();

			// phi of theta/2 of reference configuration
			Matrix3d det;
			det.col(0) = normal;
			det.col(1) = normal_dot;
			det.col(2) = e0;
			phi_bar(e) = 2. * sgn(det.determinant()) * (normal - normal_dot).norm() / (normal + normal_dot).norm();
			phi_hat(e) = phi_bar(e);

			//area and scaling coefficient
			double a0 = 0.5 * e0.cross(e2).norm();
			double a1 = 0.5 * e0.cross(e2_dot).norm();
			double length = e0.norm();
			a(e) = 3. * length * length / (a0 + a1);
		}
		else {
			phi_bar(e) = 0;
			phi_hat(e) = 0;
			a(e) = 0;
		}
	}
}

void Bend::compute_forces(
	const MatrixXd& X,			// in: vertex positions
	const double timestep,		// in: delta t
	VectorXd& F,				// out: forces
	SparseMatrix<double>& K,	// out: stiffness matrix
	SparseMatrix<double>& D		// out: damping matrix
) {
	F = VectorXd::Zero(n3);
	vector<Tri> triK, triD;
	triK.reserve(k_entries);
	triD.reserve(k_entries);

	// only do calculations if k_bend != 0
	if (k_bend == 0) {
		K = SparseMatrix<double>(n3, n3);	// zero matrices
		D = SparseMatrix<double>(n3, n3);
		return;
	}

	// compute
	MatrixXd normal(m, 3);
	for (int f = 0; f < m; f++) {
		Vector3d e1 = X.row(T(f, 1)) - X.row(T(f, 0));
		Vector3d e2 = X.row(T(f, 2)) - X.row(T(f, 0));
		normal.row(f) = e1.cross(e2).normalized();
	}

	VectorXd edge_length(no_edges);
	VectorXd phi_d(no_edges);
	VectorXd psi_d(no_edges);
	VectorXd psi_dd(no_edges);
	for (int e = 0; e < no_edges; e++) {
		// hinge edge
		Vector3d e0 = X.row(E4(e, 2)) - X.row(E4(e, 1));
		edge_length(e) = e0.norm();

		if (EF_adj(e, 1) != -1) { // no border edge
			Vector3d n1 = normal.row(EF_adj(e, 0));
			Vector3d n2 = normal.row(EF_adj(e, 1));

			Matrix3d det;
			det.col(0) = n1;
			det.col(1) = n2;
			det.col(2) = e0;

			double n1_plus_n2_norm = (n1 + n2).norm();
			double sec_theta_half = 2. / n1_plus_n2_norm;
			double phi = 2. * sgn(det.determinant()) * (n1 - n2).norm() / n1_plus_n2_norm;
			phi_d(e) = sec_theta_half * sec_theta_half;
			double phi_dd = 0.5 * phi * sec_theta_half;

			psi_d(e) = 2. * a(e) * ( k_bend * phi_d(e) * (phi - phi_bar(e))				// force
							    + k_damping * phi_d(e) * (phi - phi_hat(e)) / timestep );	// damping

			psi_dd(e) = 2. * a(e) * ( k_bend * (phi_dd * (phi - phi_bar(e)) + phi_d(e) * phi_d(e))				// force
								 + k_damping * (phi_dd * (phi - phi_hat(e)) + phi_d(e) * phi_d(e)) / timestep);	// damping

			// update last phi
			phi_hat(e) = phi;
		}
		else {
			psi_d(e) = 0;
			psi_dd(e) = 0;
		}
	}

	MatrixXd cos_alpha(m,3), h_inverse(m,3);
	vector<MatrixXd> H_triangle(m);
	for (int f = 0; f < m; f++) {
		vector<Vector3d> e(3);
		e[0] = X.row(T(f, 2)) - X.row(T(f, 1));		// edge opposite of v_0
		e[1] = X.row(T(f, 0)) - X.row(T(f, 2));		// opposite of v_1
		e[2] = X.row(T(f, 1)) - X.row(T(f, 0));		// opposite of v_2

		double area = 0.5 * e[0].cross(-e[2]).norm();

		vector<Vector3d> en(3);
		for (int i = 0; i < 3; i++)
			en[i] = e[i].normalized();			// direction is important, therefore we do not precompute them per edge earlier

		vector<Vector3d> edge_normal(3);
		for (int i = 0; i < 3; i++) {
			cos_alpha(f, i) = -en[(i + 1) % 3].dot(en[(i + 2) % 3]);	// angle at v_i
			h_inverse(f, i) = 0.5 * edge_length(FE_adj[f][i]) / area;	// height ending in v_i
			Vector3d nt = normal.row(f).transpose();
			edge_normal[i] = en[i].cross(nt);							// edge normal to edge opposite to v_i
		}

		vector<double> c(3);
		vector<Matrix3d> M(3), N(3), R(3);
		for (int i = 0; i < 3; i++) {
			int e_id = FE_adj[f][i];
			M[i] = normal.row(f).transpose() * edge_normal[i].transpose();
			N[i] = M[i] / (edge_length(e_id) * edge_length(e_id));

			bool edge_on_boundary = EF_adj(e_id, 1) == -1;
			double sigma = edge_on_boundary ? 0. : 1.;
			c[i] = sigma * psi_d(e_id);
			R[i] = c[i] * N[i];
		}

		vector<double> d(3);
		for (int i = 0; i < 3; i++) {
			int i_minus = (i + 2) % 3;
			int i_plus = (i + 1) % 3;
			d[i] = c[i_minus] * cos_alpha(f, i_plus) + c[i_plus] * cos_alpha(f, i_minus) - c[i];
		}

		// calculate the stiffness matrix contribution for a single triangle
		H_triangle[f].resize(9, 9);
		for (int i = 0; i < 3; i++) {
			int j = (i + 1) % 3;
			int k = (i + 2) % 3;

			double omega_ii = h_inverse(f, i) * h_inverse(f, i);
			Matrix3d H_ii = omega_ii * d[i] * (M[i].transpose() + M[i]) - R[j] - R[k];
			H_triangle[f].block(3*i, 3*i, 3, 3) = H_ii;

			// check if global orientation of edge matches with local ccw orientation -> only for matching orientations do we transpose R
			//
			//		   e0
			//   v2 -------- v1			Of e0 (i = 0) the first vertex in ccw order is
			//    \		    /			v1 = F(f, (i+1)%3)
			//     \   f   /
			//	e1  \     /  e2
			//       \	 /
			//		  \ /
			//        v0
			//
			int e_id = FE_adj[f][i];
			int first_vertex_local = T(f, j);
			int first_vertex_global = E(e_id, 0);
			Matrix3d R_local = R[k];
			if (first_vertex_local == first_vertex_global)
				R_local.transposeInPlace();

			double omega_ij = h_inverse(f, i) * h_inverse(f, j);
			Matrix3d H_ij = omega_ij * (d[i] * M[j].transpose() + d[j] * M[i]) + R_local;
			H_triangle[f].block(3 * i, 3 * j, 3, 3) = H_ij;
			H_triangle[f].block(3 * j, 3 * i, 3, 3) = H_ij.transpose();
		}
	}

	for (int e = 0; e < no_edges; e++) {
		if (EF_adj(e, 1) != -1) { // no border edge

			// indexing
			int f = EF_adj(e, 0);		// adjacent face 1
			int f_dot = EF_adj(e, 1);	// adjacent face 2

			vector<int> f_v(3), f_dot_v(3);	// f_v 0,1,2 corresponds to v0,v1,v2 --- f_dot_v 0,1,2 corresponds to v1,v2,v3
			for (int i = 0; i < 3; i++) {
				f_v[i] = EF6(e, i);
				f_dot_v[i] = EF6(e, 3 + i);
			}

			// first derivative
			VectorXd delta_theta(12);
			delta_theta.segment(0, 3) = -h_inverse(f, f_v[0]) * normal.row(f);
			delta_theta.segment(3, 3) = cos_alpha(f, f_v[2]) * h_inverse(f, f_v[1]) * normal.row(f) + cos_alpha(f_dot, f_dot_v[1]) * h_inverse(f_dot, f_dot_v[0]) * normal.row(f_dot);
			delta_theta.segment(6, 3) = cos_alpha(f, f_v[1]) * h_inverse(f, f_v[2]) * normal.row(f) + cos_alpha(f_dot, f_dot_v[0]) * h_inverse(f_dot, f_dot_v[1]) * normal.row(f_dot);
			delta_theta.segment(9, 3) = -h_inverse(f_dot, f_dot_v[2]) * normal.row(f_dot);

			for (int i = 0; i < 3; i++)
				for (int v = 0; v < 4; v++)
					F(E4(e, v) + i * n) += -psi_d(e) * delta_theta(3*v + i);

			// second derivative
			// stiffness matrix
			MatrixXd delta_f = MatrixXd::Zero(12, 12);
			delta_f -= psi_dd(e) * delta_theta * delta_theta.transpose();	// second of two terms that build delta_f

			for (int i = 0; i < 4; i++) {
				for (int j = 0; j <= i; j++) {	// go through vertice pairs i-j

					for (int p = 0; p < 3; p++) {			// go through x,y,z coordinates
						int q = 0;
						if (i == j) q = p;					// for central blocks, exploit symmetry
						for (; q < 3; q++) {
							if (i < 3 && j < 3) delta_f(3 * i + p, 3 * j + q) -= H_triangle[f](f_v[i] * 3 + p, f_v[j] * 3 + q);					// contribution of first term of face f
							if (i > 0 && j > 0) delta_f(3 * i + p, 3 * j + q) -= H_triangle[f_dot](f_dot_v[i - 1] * 3 + p, f_dot_v[j - 1] * 3 + q);	// contribution of first term of face f_dot
						}
					}
				}
			}

			// project onto positive eigenvalues
			// these 9x9 matrices are real symmetric => diagonizable (spectral theorem)
			// symmetry also gives orthogonal eigenvalues, therefore Eval.transpose() = Eval.inverse()
			SelfAdjointEigenSolver<MatrixXd> eig(-delta_f);
			VectorXd eigenvalues = eig.eigenvalues();
			if (eigenvalues.minCoeff() < -1e-10) {
				MatrixXd eigenvectors = eig.eigenvectors();
				MatrixXd S = MatrixXd::Zero(eigenvalues.rows(), eigenvalues.rows());
				for (int s = 0; s < eigenvalues.rows(); s++)
					S(s, s) = eigenvalues(s) > -1e-10 ? eigenvalues(s) : 0;
				delta_f = -eigenvectors * S * eigenvectors.transpose();
			}

			// create triplets
			for (int i = 0; i < 4; i++) {
				for (int j = 0; j <= i; j++) {	// go through vertice pairs i-j

					for (int p = 0; p < 3; p++) {			// go through x,y,z coordinates
						int q = 0;
						if (i == j) q = p;					// for central blocks, exploit symmetry
						for (; q < 3; q++) {
							int row = E4(e, i) + p * n;		// corresponds to vertex id (i) and shifted by number of vertices for y and z coordinates (p)
							int col = E4(e, j) + q * n;
							if(col <= row) triK.push_back(Tri(row, col, delta_f(3 * i + p, 3 * j + q)));
							else triK.push_back(Tri(col, row, delta_f(3 * i + p, 3 * j + q)));
							//if((i != j) || (i == j && p != q)) triK.push_back(Tri(col, row, value));					// exploit symmetry of delta_f blocks
						}
					}
				}
			}

			// damping matrix
			// this is a hand-made damping matrix, that is not mentioned in the paper. Allows for bigger time steps.
			VectorXd D_diagonal = -2. * a(f) * k_damping * phi_d(e) * delta_theta;
			for (int i = 0; i < 3; i++){
				for (int v = 0; v < 4; v++) {
					int row = E4(e, v) + i * n;
					triD.push_back(Tri(row, row, D_diagonal(3 * v + i)));
				}
			}
		}
	}

	// build the sparse matrices from the triplets
	if (K_data.rows() == 0) {
		this->K = SparseMatrix<double>(n3, n3);
		this->D = SparseMatrix<double>(n3, n3);
		igl::sparse_cached_precompute(triK, K_data, this->K);
		igl::sparse_cached_precompute(triD, D_data, this->D);
		cout << "initialize matrix structure" << endl;
		K = this->K;
		D = this->D;
	}
	else {
		igl::sparse_cached(triK, K_data, this->K);
		igl::sparse_cached(triD, D_data, this->D);
		K = this->K;
		D = this->D;
	}

	//K.setFromTriplets(triK.begin(), triK.end());
	//D.setFromTriplets(triD.begin(), triD.end());
}


#pragma once

#include <math.h>
#include <time.h>
#include <boost/multiprecision/gmp.hpp>

#include "FiniteFields.hpp"
#include "NumberTheoreticTransform.hpp"

namespace ligero {

/**
 *  Secret Sharing Interface
 *   Handles the sharing and reconstruction of the private witness, as well as
 * support for multiple tests for inter-block, intra-block and quadratic
 * constraints
 */
class SecretSharingNTT {
 public:
  /* Constructors allowing some flexibility in the choice of domains */
  ~SecretSharingNTT() {
    delete _smallDomain;
    delete _largeDomain;
  }
  SecretSharingNTT(size_t modulusIdx, size_t lSecretLength, size_t kDegree,
                   size_t nShares,
                   void (*computeSmall)(uint64_t *, uint64_t const *),
                   void (*computeLarge)(uint64_t *, uint64_t const *))
      : modulusIdx_(modulusIdx),
        lSecretLength_(lSecretLength),
        kDegree_(kDegree),
        nNumberShares_(nShares) {
    this->dCompositeDomainSize_ = nShares;

    _smallDomain = new NTT(modulusIdx, this->kDegree_, computeSmall);
    _largeDomain = new NTT(modulusIdx, this->nNumberShares_, computeLarge);
  };

  void share(uint64_t *secret);
  void reconstruct(uint64_t *eval, bool expanded = false);

  void shareMany(uint64_t *secrets, size_t lrows);
  void reconstructMany(uint64_t *secrets, size_t lrows);

  bool degreeTest(uint64_t *eval);
  bool zeroTest(uint64_t *eval, bool expanded = false);
  bool zeroSumTest(uint64_t *eval);

 protected:
  void padPolynominal(uint64_t *secret);
  void padIntrablocRandomness(uint64_t *secret);

  size_t modulusIdx_;
  size_t lSecretLength_;
  size_t kDegree_;
  size_t nNumberShares_;
  size_t dCompositeDomainSize_;

  NTT *_smallDomain;
  NTT *_largeDomain;
};

template <typename FieldT>
class SecretSharingInterface : SecretSharingNTT {
 public:
  /* Constructors allowing some flexibility in the choice of domains */
  SecretSharingInterface(size_t modulusIdx, size_t lSecretLength,
                         size_t kDegree, size_t nShares,
                         void (*computeSmall)(uint64_t *, uint64_t const *),
                         void (*computeLarge)(uint64_t *, uint64_t const *))
      : SecretSharingNTT(modulusIdx, lSecretLength, kDegree, nShares,
                         computeSmall, computeLarge) {
    this->dCompositeDomainSize_ = nShares;
  };

  using SecretSharingNTT::degreeTest;
  using SecretSharingNTT::reconstruct;
  using SecretSharingNTT::share;
  using SecretSharingNTT::zeroSumTest;
  using SecretSharingNTT::zeroTest;

  void share(FieldT *secret);
  void reconstruct(FieldT *eval, bool expanded = false);

  void padMany(FieldT *secrets, size_t lrows);
  void padIntrablocRandomness(FieldT *secret);
  void shareMany(FieldT *secrets, size_t lrows);
  void reconstructMany(FieldT *secrets, size_t lrows);

  bool degreeTest(FieldT *eval);
  bool zeroTest(FieldT *eval, bool expanded = false);
  bool zeroSumTest(FieldT *eval);
  FieldT sumReconstruct(FieldT *eval);

 protected:
  void padSecret(FieldT *secret);
  void padPolynominal(FieldT *secret);

  // adapter
  std::vector<uint64_t> demote(FieldT *eval, size_t length);
  void promote(uint64_t *eval, size_t length, FieldT *dest);
};

/** Implementation
/*
=============================================================================================
 */

/** Pads a block to k degree with numbers generated at random from the
 * underlying field
 * @param secret witness block to be padded
 */
template <typename FieldT>
inline void SecretSharingInterface<FieldT>::padSecret(FieldT *secret) {
  FieldT::randomVector(secret + this->lSecretLength_,
                       this->kDegree_ - this->lSecretLength_, true);
}

/** Pads randomness for intrabloc constraints up to degree k with zeroes
 * @param secret randomness block to be padded
 */
template <typename FieldT>
inline void SecretSharingInterface<FieldT>::padIntrablocRandomness(
    FieldT *secret) {
  for (size_t i = this->lSecretLength_; i < kDegree_; i++) {
    secret[i] = FieldT(0);
  }
}

/** Pads polynomial coefficients from k degree to n number of shares
 * @param secret array of polynomial coefficients to be padded
 */
template <typename FieldT>
inline void SecretSharingInterface<FieldT>::padPolynominal(FieldT *secret) {
  for (size_t i = this->kDegree_; i < nNumberShares_; i++) {
    secret[i] = FieldT(0);
  }
}

/** Pads multiple blocks
 * @param secret witness blocks to be padded
 * @param lrows number of blocks
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::padMany(FieldT *secret, size_t lrows) {
  /* Add randomness for padding */
  for (size_t i = 0; i < lrows; i++) {
    this->padSecret(secret + i * this->nNumberShares_);
  }
}

/** Secret-sharing interface for a block in the witness: this demotes field
 * elements into 64-bit numbers and calls the specialized 64-bit secret-sharing
 * method
 * @param secret witness block to be padded
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::share(FieldT *secret) {
  std::vector<uint64_t> secret64 = demote(secret, this->nNumberShares_);
  this->share(&secret64[0]);
  promote(&secret64[0], this->nNumberShares_, secret);
}

/** Reconstructs the secret corresponding to a given shared block
 * @param eval array of field elements representing a shared block
 * @param expandedDegree flag indicating that the degree of the polynomial for
 * this bloc is more than k
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::reconstruct(FieldT *eval,
                                                 bool expandedDegree) {
  std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);
  this->reconstruct(&eval64[0], expandedDegree);
  promote(&eval64[0], this->kDegree_, eval);
}

/** Demotes an array of field elements to 64-bit numbers
 * @param eval array of field elements
 * @param length length of the array to be demoted
 * @return array of demoted 64-bit numbers
 */
template <typename FieldT>
std::vector<uint64_t> SecretSharingInterface<FieldT>::demote(FieldT *eval,
                                                             size_t length) {
  std::vector<uint64_t> temp(length);
  for (size_t idx = 0; idx < length; idx++) {
    temp[idx] = static_cast<uint64_t>(eval[idx].getValue());
  }

  return temp;
}

/** promotes an array of 64-bit numbers to field elements
 * @param eval array of 64-bit numbers
 * @param length length of the array to be promoted
 * @param dest location of the promoted content
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::promote(uint64_t *eval, size_t length,
                                             FieldT *dest) {
  for (size_t idx = 0; idx < length; idx++) {
    dest[idx] = FieldT(eval[idx]);
  }
}

/** performs low degree testing on the shared block provided
 * @param eval pointer to the shared block of field elements
 * @return boolean indicating whether the test succeeded or failed
 */
template <typename FieldT>
bool SecretSharingInterface<FieldT>::degreeTest(FieldT *eval) {
  std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);
  return this->degreeTest(&eval64[0]);
}

/** tests whether the polynomial corresponding to the shared block provided
 * evaluates to 0 on each point of the secret domain
 * @param eval pointer to the shared block of field elements
 * @param largerDegree boolean indicates whether the underlying polynomial is of
 * degree > k
 * @return boolean indicating whether the test succeeded or failed
 */
template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroTest(FieldT *eval, bool largerDegree) {
  std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], largerDegree);

  for (size_t i = 0; i < this->lSecretLength_; i++) {
    if (!(localCopy[i] == FieldT(0))) return false;
  }

  return true;
}

/** tests whether the sum of the evaluations for the polynomial corresponding to
 * the shared block over all points of the secret domain is 0
 * @param eval pointer to the shared block of field elements
 * @return boolean indicating whether the test succeeded or failed
 */
template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroSumTest(FieldT *eval) {
  std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], true);

  FieldT sum = FieldT(0);
  for (size_t i = 0; i < this->lSecretLength_; i++) {
    sum += localCopy[i];
  }

  if (sum == FieldT(0))
    return true;
  else
    return false;
}

/** computes and returns the sum of the evaluations for the polynomial
 * corresponding to the shared block over all points of the secret domain
 * @param eval pointer to the shared block of field elements
 * @return sum of evaluations over the secret domain
 */
template <typename FieldT>
FieldT SecretSharingInterface<FieldT>::sumReconstruct(FieldT *eval) {
  std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], true);

  FieldT sum = FieldT(0);
  for (size_t i = 0; i < this->lSecretLength_; i++) {
    sum += localCopy[i];
  }

  return sum;
}

/** performs secret-sharing for multiple blocks at once. The sharing is
 * performed in-place, i.e. the shared block will be replacing the secret in
 * memory
 * @param secret pointer to a set of private blocks of field elements
 * @param lrows total number of blocks to share
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::shareMany(FieldT *secret, size_t lrows) {
  // std::cout << "Performing FFTs: " << lrows << std::endl;
  for (size_t i = 0; i < lrows; i++) {
    share(secret + i * this->nNumberShares_);
  }
}

/** reconstructs multiple private blocs based on the corresponding shared
 * elements at once. The sharing is performed in-place, i.e. the secrets will be
 * replacing the shared blocks in memory
 * @param eval pointer to a set of shared blocks of field elements
 * @param lrows total number of blocks to share
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::reconstructMany(FieldT *eval,
                                                     size_t lrows) {
  for (size_t i = 0; i < lrows; i++) {
    reconstruct(eval + i * this->nNumberShares_);
  }
}

/** pads intrabloc randomness with zeroes from the length of a block to the
 * polynomial degree
 * @param randomness pointer to a private block of field elements
 */
void SecretSharingNTT::padIntrablocRandomness(uint64_t *randomness) {
  for (size_t i = this->lSecretLength_; i < kDegree_; i++) {
    randomness[i] = uint64_t(0);
  }
}

/** pads polynomial coefficients with zeroes from degree k to number of shares n
 * @param coefs pointer to the coefficients
 */
void SecretSharingNTT::padPolynominal(uint64_t *coefs) {
  for (size_t i = this->kDegree_; i < nNumberShares_; i++) {
    coefs[i] = uint64_t(0);
  }
}

/** secret-sharing of a demoted vector of 64-bit secrets. The secret sharing is
 * performed in place so secrets are replaced by the shared block in memory
 * @param data pointer to the demoted vector of secrets
 */
void SecretSharingNTT::share(uint64_t *data) {
  // Interpolate Poly from Secret
  _smallDomain->inv_ntt(data);

  this->padPolynominal(data);

  // Evaluate Publicly Shared Data from Poly
  _largeDomain->ntt(data);
}

/** reconstructing a secret block based on the input shared block
 * so secrets are replaced by the shared block in memory
 * @param eval point to the shared block
 * @param exapandedDegree boolean indicating whether the polynomial linked to
 * this block has a degree > k
 */
void SecretSharingNTT::reconstruct(uint64_t *eval, bool expandedDegree) {
  size_t degree = kDegree_;

  // Interpolate Poly from Publicly Shared Data
  _largeDomain->inv_ntt(eval);

  // if (expandedDegree) degree = kDegree_*2;
  // Because we evaluate on roots of unity, we can use coefs[i] - coefs[i+k]
  // on k coefficients instead of a 2k evaluation
  if (expandedDegree) {
    for (size_t i = 0; i < kDegree_; i++) {
      if (eval[i + kDegree_] > eval[i]) {
        eval[i] = eval[i] - eval[i + kDegree_] +
                  params<uint64_t>::P[this->modulusIdx_];
      } else {
        eval[i] = eval[i] - eval[i + kDegree_];
      }
    }
  }

  // Evaluate Poly Into Secret
  _smallDomain->ntt(eval);
}

/** performs low degree testing on the demoted shared block provided
 * @param eval pointer to the shared block of demoted 64-bit numbers
 * @return boolean indicating whether the test succeeded or failed
 */
bool SecretSharingNTT::degreeTest(uint64_t *eval) {
  // Interpolate Poly from Publicly Shared Data
  std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
  _largeDomain->inv_ntt(&localCopy[0]);

  for (size_t i = kDegree_; i < nNumberShares_; i++) {
    if (!(localCopy[i] == uint64_t(0))) {
      DBG("val:" << localCopy[i] << ",degree>" << i);
      return false;
    }
  }

  return true;
}

/** tests whether the polynomial corresponding to the demoted shared block
 * provided evaluates to 0 on each point of the secret domain
 * @param eval pointer to the demoted shared block of 64-bit numbers
 * @param largerDegree boolean indicates whether the underlying polynomial is of
 * degree > k
 * @return boolean indicating whether the test succeeded or failed
 */
bool SecretSharingNTT::zeroTest(uint64_t *eval, bool largerDegree) {
  std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], largerDegree);

  for (size_t i = 0; i < this->lSecretLength_; i++) {
    if (!(localCopy[i] == uint64_t(0))) return false;
  }

  return true;
}

/** tests whether the sum of the evaluations for the polynomial corresponding to
 * the demoted shared block over all points of the secret domain is 0
 * @param eval pointer to the demoted shared block of 64-bit numbers
 * @return boolean indicating whether the test succeeded or failed
 */
bool SecretSharingNTT::zeroSumTest(uint64_t *eval) {
  std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], true);

  uint64_t sum = uint64_t(0);
  for (size_t i = 0; i < this->lSecretLength_; i++) {
    sum += localCopy[i];
  }

  if (sum == uint64_t(0))
    return true;
  else
    return false;
}

/** performs secret-sharing for multiple demoted blocks at once. The sharing is
 * performed in-place, i.e. the shared block will be replacing the secret in
 * memory
 * @param secret pointer to a set of demoted private blocks of 64-bit numbers
 * @param lrows total number of blocks to share
 */
void SecretSharingNTT::shareMany(uint64_t *secret, size_t lrows) {
  // std::cout << "Performing FFTs: " << lrows << std::endl;
  for (size_t i = 0; i < lrows; i++) {
    share(secret + i * this->nNumberShares_);
  }
}

/** reconstructs multiple private blocs based on the corresponding demoted
 * shared elements at once. The sharing is performed in-place, i.e. the secrets
 * will be replacing the shared blocks in memory
 * @param eval pointer to a set of demoted shared blocks of 64-bit numbers
 * @param lrows total number of blocks to share
 */
void SecretSharingNTT::reconstructMany(uint64_t *secret, size_t lrows) {
  for (size_t i = 0; i < lrows; i++) {
    reconstruct(secret + i * this->nNumberShares_);
  }
}

}  // namespace ligero


#ifndef GEOMETRY_HH
#define GEOMETRY_HH

/*!
 * \file Geometry.hh
 * \brief Definition of the Geometry class
 */

#include <vector>
#include <Eigen/Core>
#include "io/JobIStream.hh"

/*!
 * \brief The geometry of the system
 *
 * Class Geometry hold the positions and atom types of the nuclei in the
 * system.
 */
class Geometry
{
public:
	/*!
	 * \brief Constructor
	 *
	 * Create a new, empty Geometry
	 */
	Geometry(): _positions(), _masses(), _charges(), _symbols() {}

	//! Return the number of atoms in the Geometry
	int size() const { return _positions.cols(); }

	//! Return the element symbol of atom \a idx
	const std::string& symbol(int idx) const
	{
		checkIndex(idx);
		return _symbols[idx];
	}

	//! Return the nuclear charges of the atoms
	const Eigen::VectorXd& charges() const
	{
		return _charges;
	}
	//! Return the total charge of the the nuclei
	double totalCharge() const
	{
		return _charges.sum();
	}
	//! Return the energy due to mutual repulsion of the nuclei
	double nuclearRepulsion() const;

	//! Return the masses of the atoms
	const Eigen::VectorXd& masses() const
	{
		return _masses;
	}

	//! Return the position of atom \a idx
	Eigen::Vector3d position(int idx) const
	{
		checkIndex(idx);
		return _positions.col(idx);
	}
	//! Return tthe positions of the atoms
	const Eigen::MatrixXd& positions() const
	{
		return _positions;
	}

	//! Compute the moment of inertia tensor for this molecule
	Eigen::Matrix3d inertia() const;

	/*!
	 * \brief Change the size of the Geometry
	 *
	 * Change the number of atoms in the Geometry to \a n. Any information
	 * currently in the Geometry will be lost.
	 */
	void resize(int n)
	{
		_positions.resize(3, n);
		_masses.resize(n);
		_charges.resize(n);
		_symbols.resize(n);
	}
	/*!
	 * \brief Set an atom
	 *
	 * On position \a idx in this Geometry, place an atom with element symbol
	 * \a symbol at position (\a x, \a y, \a z). Any previous atom with the
	 * same index will be overwritten.
	 * \param idx    The index of the atom in this Geometry
	 * \param symbol The element symbol of the atom
	 * \param x      The x-coordinate of the atom
	 * \param y      The y-coordinate of the atom
	 * \param z      The z-coordinate of the atom
	 * \exception UnknownElement throw when the element symbol
	 *    cannot be found in the periodic table.
	 */
	void setAtom(int idx, const std::string& symbol, double x, double y,
		double z);
	/*!
	 * \brief Update the position of an atom
	 *
	 * Set the position of the atom at index \a idx to \a pos.
	 * \param idx The index of the atom in this Geometry
	 * \param pos The new position of the atom
	 */
	void setPosition(int idx, const Eigen::Vector3d& pos)
	{
		checkIndex(idx);
		_positions.col(idx) = pos;
	}
	/*!
	 * \brief Rotate to a principal axis frame
	 *
	 * Shift and rotate the system so that its center of mass is located at
	 * the origin, and its principal axes align with the \f$x\f$, \f$y\f$
	 * and \f$z\f$ axes.
	 */
	void toPrincipalAxes();

	/*!
	 * \brief print this Geometry
	 *
	 * Write a textual representation of this Geometry to output stream
	 * \a os.
	 * \param os The output stream to write to
	 * \return The updated output stream
	 */
	std::ostream& print(std::ostream& os) const;
	/*!
	 * \brief Read a Geometry
	 *
	 * Read a description of this geometry from input stream \a is. The
	 * geometry can be given either in XYZ coordinates, or in Z-matrix
	 * format.
	 * \param is The input stream to read from
	 * \return The updated input stream
	 */
	JobIStream& scan(JobIStream& is);

private:
	//! The positions of the atoms
	Eigen::MatrixXd _positions;
	//! The mass of the atoms
	Eigen::VectorXd _masses;
	//! The charges of the atoms
	Eigen::VectorXd _charges;
	//! The atom symbols
	std::vector<std::string> _symbols;

	/*!
	 * \brief Check if an atom index is valid
	 *
	 * Check if \a idx is a valid atom index in this Geometry. The check is
	 * only performed if the program is compiled with the DEBUG symbol
	 * defined.
	 * \param idx The atom index to check
	 * \exception InvalidIndex thrown when the index is out of bounds
	 */
#ifdef DEBUG
	void checkIndex(int idx) const
	{
		if (idx < 0 || idx >= size())
			throw InvalidIndex(idx);
	}
#else
	void checkIndex(int) const {}
#endif
};

namespace {

/*!
 * \brief print a Geometry
 *
 * Write a textual representation of Geometry \a geom to output stream \a os.
 * \param os   The output stream to write to
 * \param geom The Geometry to print
 * \return The updated output stream
 */
inline std::ostream& operator<<(std::ostream& os, const Geometry& geom)
{
	return geom.print(os);
}

/*!
 * \brief Read a Geometry
 *
 * Read a description of a geometry from input stream \a is and store it
 * in \a geom. The geometry can be given either in XYZ coordinates, or in
 * Z-matrix format.
 * \param is   The input stream to read from
 * \param geom The Geomtery to read
 * \return The updated input stream
 */
inline JobIStream& operator>>(JobIStream& is, Geometry& geom)
{
	return geom.scan(is);
}

} // namespace

#endif // GEOMETRY_HH


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>
#include <CGAL/Polygon_mesh_processing/border.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>

#include <CGAL/IO/PLY.h>

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <fstream>
#include <vector>
#include <set>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point;
typedef CGAL::Surface_mesh<Point>                           Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor        vertex_descriptor;
typedef boost::graph_traits<Mesh>::halfedge_descriptor      halfedge_descriptor;
typedef boost::graph_traits<Mesh>::face_descriptor          face_descriptor;

namespace PMP = CGAL::Polygon_mesh_processing;

bool is_small_hole(halfedge_descriptor h, Mesh & mesh,
                   double max_hole_diam, int max_num_hole_edges)
{
  int num_hole_edges = 0;
  CGAL::Bbox_3 hole_bbox;
  for (halfedge_descriptor hc : CGAL::halfedges_around_face(h, mesh))
  {
    const Point& p = mesh.point(target(hc, mesh));

    hole_bbox += p.bbox();
    ++num_hole_edges;

    // Exit early, to avoid unnecessary traversal of large holes
    if (num_hole_edges > max_num_hole_edges) return false;
    if (hole_bbox.xmax() - hole_bbox.xmin() > max_hole_diam) return false;
    if (hole_bbox.ymax() - hole_bbox.ymin() > max_hole_diam) return false;
    if (hole_bbox.zmax() - hole_bbox.zmin() > max_hole_diam) return false;
  }

  return true;
}

// Incrementally fill the holes that are no larger than given diameter
// and with no more than a given number of edges (if specified).

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

  if (argc < 5) {
    std::cout << "Usage: " << argv[0]
              << " max_hole_diameter max_num_hole_edges input.ply output.ply\n";
    return 1;
  }

  double max_hole_diam    = atof(argv[1]);
  int max_num_hole_edges  = atoi(argv[2]);
  const char* input_file  = argv[3];
  const char* output_file = argv[4];

  std::cout << "Reading mesh:       " << input_file << std::endl;
  std::cout << "Max num hole edges: " << max_num_hole_edges << "\n";
  std::cout << "Max hole diameter:  " << max_hole_diam << "\n";

  Mesh mesh;
  if(!PMP::IO::read_polygon_mesh(input_file, mesh)) {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  unsigned int nb_holes = 0;
  std::vector<halfedge_descriptor> border_cycles;

  // collect one halfedge per boundary cycle
  PMP::extract_boundary_cycles(mesh, std::back_inserter(border_cycles));

  for(halfedge_descriptor h : border_cycles)
  {
    if(max_hole_diam > 0 && max_num_hole_edges > 0 &&
       !is_small_hole(h, mesh, max_hole_diam, max_num_hole_edges))
      continue;

    std::vector<face_descriptor>  patch_facets;
    std::vector<vertex_descriptor> patch_vertices;
    bool success =
      std::get<0>(PMP::triangulate_refine_and_fair_hole(mesh,
                                                        h,
                                                        std::back_inserter(patch_facets),
                                                        std::back_inserter(patch_vertices)));

    //std::cout << "* Number of facets in constructed patch: " << patch_facets.size() << std::endl;
    //std::cout << "  Number of vertices in constructed patch: " << patch_vertices.size() << std::endl;
    //std::cout << "  Is fairing successful: " << success << std::endl;
    ++nb_holes;
  }

  //std::cout << std::endl;
  //std::cout << nb_holes << " holes have been filled" << std::endl;

  std::cout << "Writing output mesh: " << output_file << std::endl;
  CGAL::IO::write_PLY(output_file, mesh,
                      CGAL::parameters::stream_precision(17).use_binary_mode(false));

  return 0;
}


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

MSI applications for interactive analysis in MITK (M2aia)

Copyright (c) Jonas Cordes

All rights reserved.

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

See LICENSE.txt for details.

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

#include "mitkIOUtil.h"
#include <algorithm>
#include <m2Morphology.h>
#include <m2TestingConfig.h>
#include <mitkTestFixture.h>
#include <mitkTestingMacros.h>
#include <numeric>
#include <random>

//#include <boost/algorithm/string.hpp>

class m2MorphologyTestSuite : public mitk::TestFixture
{
  CPPUNIT_TEST_SUITE(m2MorphologyTestSuite);
  MITK_TEST(ApplyErosion_RandomGaussianSignal_shouldReturnTrue);
  MITK_TEST(ApplyDilatation_RandomGaussianSignal_shouldReturnTrue);
  MITK_TEST(TopHat_RandomGaussianSignal_shouldReturnTrue);

  CPPUNIT_TEST_SUITE_END();

  /*
  Signal was generated once using the following procedure
    -----
	const double mean = 1.0;
    const double stddev = 0.33;
    std::default_random_engine generator;
    generator.seed(142191);
    std::normal_distribution<double> dist(mean, stddev);

    std::vector<double> signal;
    auto it = std::inserter(signal, std::begin(signal));
    for (int i = 0; i < 1000; ++i)
      it = dist(generator);
    -----
  */

private:
  std::vector<double> ReadDoubleVector(const std::string &fileNameInM2aiaDir, char delim = '\n')
  {
    std::vector<double> signal;
    std::ifstream f(GetTestDataFilePath(fileNameInM2aiaDir, M2AIA_DATA_DIR));
    std::string line;
    while (std::getline(f, line, delim))
    {
      signal.push_back(std::stoi(line));
    }
    return signal;
  }

public:
  void ApplyDilatation_RandomGaussianSignal_shouldReturnTrue()
  {
    std::vector<double> signal = ReadDoubleVector("signal.data");
    std::vector<double> expected = ReadDoubleVector("dilation.result");

    std::vector<double> result(signal.size());
    m2::Signal::Dilation(signal, 2, result);

    CPPUNIT_ASSERT(std::equal(std::begin(result), std::end(result), std::begin(expected)));
  }

  void ApplyErosion_RandomGaussianSignal_shouldReturnTrue()
  {
    std::vector<double> signal = ReadDoubleVector("signal.data");
    std::vector<double> expected = ReadDoubleVector("erosion.result");

    std::vector<double> result(signal.size());
    m2::Signal::Erosion(signal, 2, result);

    CPPUNIT_ASSERT(std::equal(std::begin(result), std::end(result), std::begin(expected)));
  }

  void TopHat_RandomGaussianSignal_shouldReturnTrue()
  {
    std::vector<double> signal = ReadDoubleVector("signal.data");
    std::vector<double> expected = ReadDoubleVector("tophat.result");

    std::vector<double> result(signal.size());
    m2::Signal::Erosion(signal, 2, result);
    m2::Signal::Dilation(result, 2, result);

    CPPUNIT_ASSERT(std::equal(std::begin(result), std::end(result), std::begin(expected)));
  }
};

MITK_TEST_SUITE_REGISTRATION(m2Morphology)


#include "quadrotor_simulator/Quadrotor.h"
#include "ode/boost/numeric/odeint.hpp"
#include <Eigen/Geometry>
#include <boost/bind.hpp>
#include <iostream>

#include <ros/ros.h>
namespace odeint = boost::numeric::odeint;

namespace QuadrotorSimulator
{

Quadrotor::Quadrotor(void)
{
  alpha0     = 48; // degree
  g_         = 9.81;
  mass_      = 0.98; // 0.5;
  double Ixx = 2.64e-3, Iyy = 2.64e-3, Izz = 4.96e-3;
  prop_radius_ = 0.062;
  J_           = Eigen::Vector3d(Ixx, Iyy, Izz).asDiagonal();

  kf_ = 8.98132e-9;
  // km_ = 2.5e-9; // from Nate
  // km = (Cq/Ct)*Dia*kf
  // Cq/Ct for 8 inch props from UIUC prop db ~ 0.07
  km_ = 0.07 * (3 * prop_radius_) * kf_;

  arm_length_          = 0.26;
  motor_time_constant_ = 1.0 / 30;
  min_rpm_             = 1200;
  max_rpm_             = 35000;

  state_.x = Eigen::Vector3d::Zero();
  // state_.x << 40.0, -60.0, 10.0;
  state_.v         = Eigen::Vector3d::Zero();
  state_.R         = Eigen::Matrix3d::Identity();
  state_.omega     = Eigen::Vector3d::Zero();
  state_.motor_rpm = Eigen::Array4d::Zero();

  external_force_.setZero();

  updateInternalState();

  input_ = Eigen::Array4d::Zero();
}

void
Quadrotor::step(double dt)
{
  auto save = internal_state_;

  odeint::integrate(boost::ref(*this), internal_state_, 0.0, dt, dt);

  for (int i = 0; i < 22; ++i)
  {
    if (std::isnan(internal_state_[i]))
    {
      std::cout << "dump " << i << " << pos ";
      for (int j = 0; j < 22; ++j)
      {
        std::cout << save[j] << " ";
      }
      std::cout << std::endl;
      internal_state_ = save;
      break;
    }
  }

  for (int i = 0; i < 3; i++)
  {
    state_.x(i) = internal_state_[0 + i];
    state_.v(i) = internal_state_[3 + i];
    state_.R(i, 0) = internal_state_[6 + i];
    state_.R(i, 1) = internal_state_[9 + i];
    state_.R(i, 2) = internal_state_[12 + i];
    state_.omega(i) = internal_state_[15 + i];
  }
  state_.motor_rpm(0) = internal_state_[18];
  state_.motor_rpm(1) = internal_state_[19];
  state_.motor_rpm(2) = internal_state_[20];
  state_.motor_rpm(3) = internal_state_[21];

  // Re-orthonormalize R (polar decomposition)
  Eigen::LLT<Eigen::Matrix3d> llt(state_.R.transpose() * state_.R);
  Eigen::Matrix3d             P = llt.matrixL();
  Eigen::Matrix3d             R = state_.R * P.inverse();
  state_.R                      = R;

  // Don't go below zero, simulate floor
  if (state_.x(2) < 0.0 && state_.v(2) < 0)
  {
    state_.x(2) = 0;
    state_.v(2) = 0;
  }
  updateInternalState();
}

void
Quadrotor::operator()(const Quadrotor::InternalState& x,
                      Quadrotor::InternalState& dxdt, const double /* t */)
{
  State cur_state;
  for (int i = 0; i < 3; i++)
  {
    cur_state.x(i) = x[0 + i];
    cur_state.v(i) = x[3 + i];
    cur_state.R(i, 0) = x[6 + i];
    cur_state.R(i, 1) = x[9 + i];
    cur_state.R(i, 2) = x[12 + i];
    cur_state.omega(i) = x[15 + i];
  }
  for (int i = 0; i < 4; i++)
  {
    cur_state.motor_rpm(i) = x[18 + i];
  }

  // std::cout << "Omega: " << cur_state.omega << std::endl;
  // std::cout << "motor_rpm: " << cur_state.motor_rpm << std::endl;

  // Re-orthonormalize R (polar decomposition)
  Eigen::LLT<Eigen::Matrix3d> llt(cur_state.R.transpose() * cur_state.R);
  Eigen::Matrix3d             P = llt.matrixL();
  Eigen::Matrix3d             R = cur_state.R * P.inverse();

  Eigen::Vector3d x_dot, v_dot, omega_dot;
  Eigen::Matrix3d R_dot;
  Eigen::Array4d  motor_rpm_dot;
  Eigen::Vector3d vnorm;
  Eigen::Array4d  motor_rpm_sq;
  Eigen::Matrix3d omega_vee(Eigen::Matrix3d::Zero());

  omega_vee(2, 1) = cur_state.omega(0);
  omega_vee(1, 2) = -cur_state.omega(0);
  omega_vee(0, 2) = cur_state.omega(1);
  omega_vee(2, 0) = -cur_state.omega(1);
  omega_vee(1, 0) = cur_state.omega(2);
  omega_vee(0, 1) = -cur_state.omega(2);

  motor_rpm_sq = cur_state.motor_rpm.array().square();

  //! @todo implement
  Eigen::Array4d blade_linear_velocity;
  Eigen::Array4d motor_linear_velocity;
  Eigen::Array4d AOA;
  blade_linear_velocity = 0.104719755 // rpm to rad/s
                          * cur_state.motor_rpm.array() * prop_radius_;
  for (int i = 0; i < 4; ++i)
    AOA[i]   = alpha0 -
             atan2(motor_linear_velocity[i], blade_linear_velocity[i]) * //
               180 / 3.14159265;
  //! @todo end

  // double totalF = kf_ * motor_rpm_sq.sum();
  double thrust = kf_ * motor_rpm_sq.sum();

  Eigen::Vector3d moments;
  moments(0) = kf_ * (motor_rpm_sq(2) - motor_rpm_sq(3)) * arm_length_;
  moments(1) = kf_ * (motor_rpm_sq(1) - motor_rpm_sq(0)) * arm_length_;
  moments(2) = km_ * (motor_rpm_sq(0) + motor_rpm_sq(1) - motor_rpm_sq(2) -
                      motor_rpm_sq(3));

  double resistance = 0.1 *                                        // C
                      3.14159265 * (arm_length_) * (arm_length_) * // S
                      cur_state.v.norm() * cur_state.v.norm();

  //  ROS_INFO("resistance: %lf, Thrust: %lf%% ", resistance,
  //           motor_rpm_sq.sum() / (4 * max_rpm_ * max_rpm_) * 100.0);

  vnorm = cur_state.v;
  if (vnorm.norm() != 0)
  {
    vnorm.normalize();
  }
  x_dot = cur_state.v;
  v_dot = -Eigen::Vector3d(0, 0, g_) + thrust * R.col(2) / mass_ +
          external_force_ / mass_ /*; //*/ - resistance * vnorm / mass_;

  acc_ = v_dot;
  //  acc_[2] = -acc_[2]; // to NED

  R_dot = R * omega_vee;
  omega_dot =
    J_.inverse() *
    (moments - cur_state.omega.cross(J_ * cur_state.omega) + external_moment_);
  motor_rpm_dot = (input_ - cur_state.motor_rpm) / motor_time_constant_;

  for (int i = 0; i < 3; i++)
  {
    dxdt[0 + i]  = x_dot(i);
    dxdt[3 + i]  = v_dot(i);
    dxdt[6 + i]  = R_dot(i, 0);
    dxdt[9 + i]  = R_dot(i, 1);
    dxdt[12 + i] = R_dot(i, 2);
    dxdt[15 + i] = omega_dot(i);
  }
  for (int i = 0; i < 4; i++)
  {
    dxdt[18 + i] = motor_rpm_dot(i);
  }
  for (int i = 0; i < 22; ++i)
  {
    if (std::isnan(dxdt[i]))
    {
      dxdt[i] = 0;
      //      std::cout << "nan apply to 0 for " << i << std::endl;
    }
  }
}

void
Quadrotor::setInput(double u1, double u2, double u3, double u4)
{
  input_(0) = u1;
  input_(1) = u2;
  input_(2) = u3;
  input_(3) = u4;
  for (int i = 0; i < 4; i++)
  {
    if (std::isnan(input_(i)))
    {
      input_(i) = (max_rpm_ + min_rpm_) / 2;
      std::cout << "NAN input ";
    }
    if (input_(i) > max_rpm_)
      input_(i) = max_rpm_;
    else if (input_(i) < min_rpm_)
      input_(i) = min_rpm_;
  }
}

const Quadrotor::State&
Quadrotor::getState(void) const
{
  return state_;
}
void
Quadrotor::setState(const Quadrotor::State& state)
{
  state_.x         = state.x;
  state_.v         = state.v;
  state_.R         = state.R;
  state_.omega     = state.omega;
  state_.motor_rpm = state.motor_rpm;

  updateInternalState();
}

void
Quadrotor::setStatePos(const Eigen::Vector3d& Pos)
{
  state_.x = Pos;

  updateInternalState();
}

double
Quadrotor::getMass(void) const
{
  return mass_;
}
void
Quadrotor::setMass(double mass)
{
  mass_ = mass;
}

double
Quadrotor::getGravity(void) const
{
  return g_;
}
void
Quadrotor::setGravity(double g)
{
  g_ = g;
}

const Eigen::Matrix3d&
Quadrotor::getInertia(void) const
{
  return J_;
}
void
Quadrotor::setInertia(const Eigen::Matrix3d& inertia)
{
  if (inertia != inertia.transpose())
  {
    std::cerr << "Inertia matrix not symmetric, not setting" << std::endl;
    return;
  }
  J_ = inertia;
}

double
Quadrotor::getArmLength(void) const
{
  return arm_length_;
}
void
Quadrotor::setArmLength(double d)
{
  if (d <= 0)
  {
    std::cerr << "Arm length <= 0, not setting" << std::endl;
    return;
  }

  arm_length_ = d;
}

double
Quadrotor::getPropRadius(void) const
{
  return prop_radius_;
}
void
Quadrotor::setPropRadius(double r)
{
  if (r <= 0)
  {
    std::cerr << "Prop radius <= 0, not setting" << std::endl;
    return;
  }
  prop_radius_ = r;
}

double
Quadrotor::getPropellerThrustCoefficient(void) const
{
  return kf_;
}
void
Quadrotor::setPropellerThrustCoefficient(double kf)
{
  if (kf <= 0)
  {
    std::cerr << "Thrust coefficient <= 0, not setting" << std::endl;
    return;
  }

  kf_ = kf;
}

double
Quadrotor::getPropellerMomentCoefficient(void) const
{
  return km_;
}
void
Quadrotor::setPropellerMomentCoefficient(double km)
{
  if (km <= 0)
  {
    std::cerr << "Moment coefficient <= 0, not setting" << std::endl;
    return;
  }

  km_ = km;
}

double
Quadrotor::getMotorTimeConstant(void) const
{
  return motor_time_constant_;
}
void
Quadrotor::setMotorTimeConstant(double k)
{
  if (k <= 0)
  {
    std::cerr << "Motor time constant <= 0, not setting" << std::endl;
    return;
  }

  motor_time_constant_ = k;
}

const Eigen::Vector3d&
Quadrotor::getExternalForce(void) const
{
  return external_force_;
}
void
Quadrotor::setExternalForce(const Eigen::Vector3d& force)
{
  external_force_ = force;
}

const Eigen::Vector3d&
Quadrotor::getExternalMoment(void) const
{
  return external_moment_;
}
void
Quadrotor::setExternalMoment(const Eigen::Vector3d& moment)
{
  external_moment_ = moment;
}

double
Quadrotor::getMaxRPM(void) const
{
  return max_rpm_;
}
void
Quadrotor::setMaxRPM(double max_rpm)
{
  if (max_rpm <= 0)
  {
    std::cerr << "Max rpm <= 0, not setting" << std::endl;
    return;
  }
  max_rpm_ = max_rpm;
}

double
Quadrotor::getMinRPM(void) const
{
  return min_rpm_;
}
void
Quadrotor::setMinRPM(double min_rpm)
{
  if (min_rpm < 0)
  {
    std::cerr << "Min rpm < 0, not setting" << std::endl;
    return;
  }
  min_rpm_ = min_rpm;
}

void
Quadrotor::updateInternalState(void)
{
  for (int i = 0; i < 3; i++)
  {
    internal_state_[0 + i]  = state_.x(i);
    internal_state_[3 + i]  = state_.v(i);
    internal_state_[6 + i]  = state_.R(i, 0);
    internal_state_[9 + i]  = state_.R(i, 1);
    internal_state_[12 + i] = state_.R(i, 2);
    internal_state_[15 + i] = state_.omega(i);
  }
  internal_state_[18] = state_.motor_rpm(0);
  internal_state_[19] = state_.motor_rpm(1);
  internal_state_[20] = state_.motor_rpm(2);
  internal_state_[21] = state_.motor_rpm(3);
}

Eigen::Vector3d
Quadrotor::getAcc() const
{
  return acc_;
}
}


#define BOOST_TEST_MODULE CUSPARSE
#include <cuda.h>
#if (CUDA_VERSION <= 10010)
#  define VEXCL_USE_CUSPARSE
#endif
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/spmat.hpp>
#include "random_matrix.hpp"
#include "context_setup.hpp"

#if (CUDA_VERSION <= 10010)
BOOST_AUTO_TEST_CASE(hyb_matrix)
{
    const size_t n = 1024;
    const size_t m = 2048;

    std::vector<int>    row;
    std::vector<int>    col;
    std::vector<double> val;
    std::vector<double> x = random_vector<double>(m);

    random_matrix(n, m, 16, row, col, val);

    vex::SpMat<double, int, int> A(ctx, n, m, row.data(), col.data(), val.data());

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double y) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(y, sum, 1e-8);
            });

    Y += 0.5 * (A * X);

    check_sample(Y, [&](size_t idx, double y) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(y, 1.5 * sum, 1e-8);
            });

}

BOOST_AUTO_TEST_CASE(crs_matrix)
{
    std::vector<vex::command_queue> single(1, ctx.queue(0));

    const size_t n = 1024;
    const size_t m = 2048;

    std::vector<int>    row;
    std::vector<int>    col;
    std::vector<double> val;
    std::vector<double> x = random_vector<double>(m);

    random_matrix(n, m, 16, row, col, val);

    vex::backend::cuda::spmat_crs<double>
        A(ctx.queue(0), n, m, row.data(), col.data(), val.data());

    vex::vector<double> X(single, x);
    vex::vector<double> Y(single, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double y) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(y, sum, 1e-8);
            });

    Y += 0.5 * (A * X);

    check_sample(Y, [&](size_t idx, double y) {
            double sum = 0;
            for(int j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(y, 1.5 * sum, 1e-8);
            });

}
#endif

BOOST_AUTO_TEST_SUITE_END()