#include "ros/ros.h"

#include <tf/transform_listener.h>
// #include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/Twist.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

#include <sstream>
#include <fstream>
#include <chrono>

#include <termios.h>
#include <stdio.h>
#include <iostream>

/* crazyflie_driver */
#include "crazyflie_driver/AddCrazyflie.h"
#include "crazyflie_driver/LogBlock.h"
#include "crazyflie_driver/GenericLogData.h"
#include "crazyflie_driver/UpdateParams.h"
#include "crazyflie_driver/UploadTrajectory.h"
#include "crazyflie_driver/NotifySetpointsStop.h"
#undef major
#undef minor
#include "crazyflie_driver/Hover.h"
#include "crazyflie_driver/Takeoff.h"
#include "crazyflie_driver/Land.h"
#include "crazyflie_driver/GoTo.h"
#include "crazyflie_driver/StartTrajectory.h"
#include "crazyflie_driver/SetGroupMask.h"
#include "crazyflie_driver/FullState.h"
#include "crazyflie_driver/Position.h"
#include "crazyflie_driver/VelocityWorld.h"

/* Includes form iLQR main.cpp */
#include <unsupported/Eigen/AdolcForward>
#include <Eigen/Dense>
#include <adolc/adolc.h>
#include <adolc/adouble.h>
#include <vector>
#include <time.h>
#include <math.h>
#include <fstream>
#include <thread>
#include <future>
#include <algorithm>
#include <iterator>
#include "cost.h"
#include "dynamics.h"
#include "iLQR.h"
#include "utils.h"
#include <unistd.h>

/* simple moving average filter */
#define USE_SMA_FILTER 0
#define SMA_WINDOW_LEN 10

/* first order exponential low-pass filter */
#define USE_EXP_FILTER 1
#define EXP_FILT_ALPHA 0.9f

// static stateVector prevState_cf1;
// static stateVector stateWindowBuf_cf1[SMA_WINDOW_LEN];

// static stateVector prevState_cf2;
// static stateVector stateWindowBuf_cf2[SMA_WINDOW_LEN];

static stateVector CFState_cf1;
static stateVector CFState_cf2;
static stateVector CFState_cf3;
static stateVector CFState_cf4;

static pose currentPose;
// static pose prevPose_cf1;

// static pose currentPose_cf2;
// static pose prevPose_cf2;

std::ofstream droneDatalogFile;
std::ofstream optimizerDatalogFile;

bool first = true;

void poseCallback(const tf2_msgs::TFMessage::ConstPtr& msg) {

    /* Collect pose and timestamp information */
    std::string cf_id = msg->transforms[0].child_frame_id;
    int secs = msg->transforms[0].header.stamp.sec;
    int nsecs = msg->transforms[0].header.stamp.nsec;
    float x = msg->transforms[0].transform.translation.x;
    float y = msg->transforms[0].transform.translation.y;
    float z = msg->transforms[0].transform.translation.z;
    float qx = msg->transforms[0].transform.rotation.x;
    float qy = msg->transforms[0].transform.rotation.y;
    float qz = msg->transforms[0].transform.rotation.z;
    float qw = msg->transforms[0].transform.rotation.w;
    tf2::Quaternion q(qx, qy, qz, qw);

    /* Quaternion to roll,pitch,yaw conversion */
    double roll, pitch, yaw;
    tf2::Matrix3x3 m(q);
    m.getRPY(roll, pitch, yaw);

    /* Calculate timestamp in nanoseconds*/
    unsigned long timeStamp = (secs * 1e9) + nsecs;
    double deltaT = 0.0;

    if (cf_id == "cf1") {
        CFState_cf1.x = x;
        CFState_cf1.y = y;
        CFState_cf1.z = z;
        CFState_cf1.roll = roll;
        CFState_cf1.pitch = pitch;
        CFState_cf1.yaw = yaw;
    } else if (cf_id == "cf2") {
        CFState_cf2.x = x;
        CFState_cf2.y = y;
        CFState_cf2.z = z;
        CFState_cf2.roll = roll;
        CFState_cf2.pitch = pitch;
        CFState_cf2.yaw = yaw;
    } else if (cf_id == "cf3") {
        CFState_cf3.x = x;
        CFState_cf3.y = y;
        CFState_cf3.z = z;
        CFState_cf3.roll = roll;
        CFState_cf3.pitch = pitch;
        CFState_cf3.yaw = yaw;
    } else if (cf_id == "cf4") {
        CFState_cf4.x = x;
        CFState_cf4.y = y;
        CFState_cf4.z = z;
        CFState_cf4.roll = roll;
        CFState_cf4.pitch = pitch;
        CFState_cf4.yaw = yaw;
    }

    std::vector<stateVector> states{CFState_cf1, CFState_cf2, CFState_cf3, CFState_cf4};

    logDroneStates(droneDatalogFile, states);

    // std::printf("\n");
    // std::printf("##### CF1:\n");
    // std::printf("X: %f,\t Y: %f,\t Z: %f\t\n ROLL: %f,\t PITCH: %f,\t YAW %f\n",
    //             CFState_cf1.x, CFState_cf1.y, CFState_cf1.z, CFState_cf1.roll, CFState_cf1.pitch, CFState_cf1.yaw);
    // std::printf("X_d: %f\t, Y_d: %f\t, Z_d: %f\t\nROLL_d: %f\t, PITCH_d: %f\t, YAW_d %f\n",
    //             CFState_cf1.x_d, CFState_cf1.y_d, CFState_cf1.z_d, CFState_cf1.roll_d, CFState_cf1.pitch_d, CFState_cf1.yaw_d);

    // std::printf("\n##### CF2:\n");
    // std::printf("X: %f,\t Y: %f,\t Z: %f\t\n ROLL: %f,\t PITCH: %f,\t YAW %f\n",
    //             CFState_cf2.x, CFState_cf2.y, CFState_cf2.z, CFState_cf2.roll, CFState_cf2.pitch, CFState_cf2.yaw);
    // std::printf("X_d: %f\t, Y_d: %f\t, Z_d: %f\t\nROLL_d: %f\t, PITCH_d: %f\t, YAW_d %f\n",
    //             CFState_cf2.x_d, CFState_cf2.y_d, CFState_cf2.z_d, CFState_cf2.roll_d, CFState_cf2.pitch_d, CFState_cf2.yaw_d);
}

/* Modification of getch() to be non-blocking */
int getch() {
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering
  newt.c_cc[VMIN] = 0; newt.c_cc[VTIME] = 0;
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings
  int c = getchar();                         // read character (non-blocking)
  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}

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

    srand((unsigned int) time(0));

    init_datalog(droneDatalogFile,
                 "/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/",
                 "droneStates_2x");

    init_datalog(optimizerDatalogFile,
                 "/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/",
                 "optimizer_2x");

    ros::init(argc, argv, "iLQR_Node");

    ros::NodeHandle n;

    ros::Subscriber state_subscriber = n.subscribe("tf", 1000, poseCallback);

    /* Publisher to cmd_position topic (currently doesn't work) */
    // ros::Publisher pub = n.advertise<crazyflie_driver::Position>("cf1/cmd_position",1);
    // crazyflie_driver::Position position_cmd;

    /* Service clients for simple high-level commands */
    ros::ServiceClient takeoffClient = n.serviceClient<crazyflie_driver::Takeoff>("/takeoff");
    ros::ServiceClient landClient = n.serviceClient<crazyflie_driver::Land>("/land");
    ros::ServiceClient GoToClient_cf1 = n.serviceClient<crazyflie_driver::GoTo>("/cf1/go_to");
    ros::ServiceClient GoToClient_cf2 = n.serviceClient<crazyflie_driver::GoTo>("/cf2/go_to");
    ros::ServiceClient GoToClient_cf3 = n.serviceClient<crazyflie_driver::GoTo>("/cf3/go_to");
    ros::ServiceClient GoToClient_cf4 = n.serviceClient<crazyflie_driver::GoTo>("/cf4/go_to");

    /* Wait for <Enter> key press to begin mission */
    std::cout << "\t########## PRESS <Enter> TO BEGIN MISSION ##########" << std::endl;
    while (true) {
        int c = getch();
            if (c == '\n') {
                std::cout << "\t########## BEGINNING MISSION ##########" << std::endl;
                break;
            }
            ros::spinOnce();
    }

    /* Construct GoTo service once to be used in loop */
    crazyflie_driver::GoTo srvGoTo_cf1;
    srvGoTo_cf1.request.groupMask = 0;  // signal all CFs (I think?)
    crazyflie_driver::GoTo srvGoTo_cf2;
    srvGoTo_cf2.request.groupMask = 0;
    crazyflie_driver::GoTo srvGoTo_cf3;
    srvGoTo_cf3.request.groupMask = 0;
    crazyflie_driver::GoTo srvGoTo_cf4;
    srvGoTo_cf4.request.groupMask = 0;

    /* Send takeoff command through /Takeoff service */
    crazyflie_driver::Takeoff srvTakeoff;
    srvTakeoff.request.groupMask = 0;
    srvTakeoff.request.height = 1.0;
    srvTakeoff.request.duration = ros::Duration(3.0);
    takeoffClient.call(srvTakeoff);
    ros::Duration(3.0).sleep();

    // srvGoTo_cf1.request.goal.x = 0.0;
    // srvGoTo_cf1.request.goal.y = 1.25;
    // srvGoTo_cf1.request.goal.z = 1.5;
    // srvGoTo_cf1.request.yaw = 0.0;
    // srvGoTo_cf1.request.duration = ros::Duration(2.0);
    // GoToClient_cf1.call(srvGoTo_cf1);
    // srvGoTo_cf2.request.goal.x = 1.25;
    // srvGoTo_cf2.request.goal.y = 0.0;
    // srvGoTo_cf2.request.goal.z = 1.5;
    // srvGoTo_cf2.request.yaw = 0.0;
    // srvGoTo_cf2.request.duration = ros::Duration(2.0);
    // GoToClient_cf2.call(srvGoTo_cf2);
    // // ros::Duration(2.0).sleep();
    // srvGoTo_cf3.request.goal.x = 0.0;
    // srvGoTo_cf3.request.goal.y = 1.0;
    // srvGoTo_cf3.request.goal.z = 1.0;
    // srvGoTo_cf3.request.yaw = 0.0;
    // srvGoTo_cf3.request.duration = ros::Duration(2.0);
    // GoToClient_cf3.call(srvGoTo_cf3);
    // srvGoTo_cf4.request.goal.x = 1.5;
    // srvGoTo_cf4.request.goal.y = 0.0;
    // srvGoTo_cf4.request.goal.z = 1.0;
    // srvGoTo_cf4.request.yaw = 0.0;
    // srvGoTo_cf4.request.duration = ros::Duration(2.0);
    // GoToClient_cf4.call(srvGoTo_cf4);
    // ros::Duration(2.0).sleep();

//     /* iLQR solver parameters */
//     constexpr size_t horizon=5;
//     constexpr int total_steps=300;
//     unsigned int tag1(1),tag2(2),tag3(3),tag4(4),tag5(5),
//                  tag6(6),tag7(7),tag8(8),tag9(9);
//
//     double time_step=0.2;
//
//
//     /*First Order 2 Drone Simulation*/
//
// 	cost<12,12> drone2_running_cost(Drone_First_Order_Cost::running_cost2,tag7);
// 	cost<12,12> drone2_terminal_cost(Drone_First_Order_Cost::terminal_cost2,tag8);
// 	dynamics<12,12> drone2_dynamics(Drone_First_Order_Dynamics::dynamics_2,tag9,time_step);
// 	iLQR<12,12,horizon>::input_trajectory u_init_drone2;
// 	iLQR<12,12,horizon> drone2_solver(drone2_running_cost,drone2_terminal_cost,drone2_dynamics);
// 	Eigen::Matrix<double,12,1> u_drone2=Eigen::Matrix<double,12,1>::Zero();
// 	Eigen::Matrix<double,12,1> x0_drone2=Eigen::Matrix<double,12,1>::Zero();
// 	Eigen::Matrix<double,12,1> x_goal_drone2;
// 	iLQR<12,12,horizon>::state_input_trajectory soln_drone2;
// 	iLQR<12,12,total_steps>::state_input_trajectory soln_drone2_rhc;
//
// 	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// 	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
//     for (auto &item : u_init_drone2)
//         item = Eigen::Matrix<double,12,1>::Random()*5;
//
// //	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
// //	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
// 	clock_t t_start, t_end;
//
//
// 	double seconds;
// 	drone2_solver.set_MPC(u_init_drone2);
// 	int execution_steps=1;
// 	soln_drone2_rhc.first[0]=x0_drone2;
// 	// std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
// 	// std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
// 	t_start = clock();
//
//     x_goal_drone2 << 1.25, 0.0, 2.5, 0,0,0,
//                      0.0, 1.25,  2.5, 0,0,0;

    while (ros::ok())
//     // while (true)
	{
//
//         ros::spinOnce();
//
//
// 		x0_drone2 << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                      CFState_cf1.roll, CFState_cf1.pitch, CFState_cf1.yaw,
//                      CFState_cf2.x, CFState_cf2.y, CFState_cf2.z,
//                      CFState_cf2.roll, CFState_cf2.pitch, CFState_cf2.yaw;
//
//
// 		soln_drone2=drone2_solver.run_MPC(x0_drone2,
// 				x_goal_drone2, 50, 2, execution_steps);
//
//         /* Optimizer output logging */
//         std::string data;
//         for (int i = 0; i < horizon; ++i) {
//             for (int q = 0; q < 12; ++q) {
//                 data += std::to_string(soln_drone2.first[i][q]) + ",";
//             }
//
//         }
//         const auto p1 = std::chrono::system_clock::now();
//     	data += std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>(p1.time_since_epoch()).count()) + "\n";
//     	optimizerDatalogFile << data;
//
//         int idx_to_use = 3;
//
//         std::printf("\n#### SENDING WAYPOINT CF1  ");
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone2.first[idx_to_use][0], soln_drone2.first[idx_to_use][1], soln_drone2.first[idx_to_use][2]);
//         std::printf("\n#### SENDING WAYPOINT CF2   ");
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone2.first[idx_to_use][6], soln_drone2.first[idx_to_use][7], soln_drone2.first[idx_to_use][8]);
//
//         srvGoTo_cf1.request.goal.x = soln_drone2.first[idx_to_use][0];
//         srvGoTo_cf1.request.goal.y = soln_drone2.first[idx_to_use][1];
//         srvGoTo_cf1.request.goal.z = soln_drone2.first[idx_to_use][2];
//         srvGoTo_cf1.request.yaw = 0.0;
//         srvGoTo_cf1.request.duration = ros::Duration(0.8);
//         GoToClient_cf1.call(srvGoTo_cf1);
//
//         srvGoTo_cf2.request.goal.x = soln_drone2.first[idx_to_use][6];
//         srvGoTo_cf2.request.goal.y = soln_drone2.first[idx_to_use][7];
//         srvGoTo_cf2.request.goal.z = soln_drone2.first[idx_to_use][8];
//         srvGoTo_cf2.request.yaw = 0.0;
//         srvGoTo_cf2.request.duration = ros::Duration(0.8);
//         GoToClient_cf2.call(srvGoTo_cf2);
//
        int c = getch();   // call your non-blocking input function
        if (c == '\n') {
            std::cout << "########## LANDING ##########" << std::endl;
            break;
        }
        if (c == 'o') {
            std::cout << "########## RETURN TO ORIGIN ##########" << std::endl;
            srvGoTo_cf1.request.goal.x = -0.2;
            srvGoTo_cf1.request.goal.y = 1.1;
            srvGoTo_cf1.request.goal.z = 1.5;
            srvGoTo_cf1.request.yaw = 0.0;
            srvGoTo_cf1.request.duration = ros::Duration(3.0);
            GoToClient_cf1.call(srvGoTo_cf1);
            srvGoTo_cf2.request.goal.x = 0.55;
            srvGoTo_cf2.request.goal.y = 0.3;
            srvGoTo_cf2.request.goal.z = 0.5;
            srvGoTo_cf2.request.yaw = 0.0;
            srvGoTo_cf2.request.duration = ros::Duration(3.0);
            GoToClient_cf2.call(srvGoTo_cf2);
            ros::Duration(3.0).sleep();
            break;
        }
//
//
	}
// 	t_end = clock();
// ////
// //////
// ////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// ////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
// 	// write_file<12,12,total_steps>(state_path,input_path, soln_drone2_rhc);
// ////
// 	std::cout<<"finished"<<std::endl;
// 	seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
// 	printf("Time: %f s\n", seconds);

    /*First Order 2 Drone Simulation Ends*/

    crazyflie_driver::Land srvLand;
    srvLand.request.duration = ros::Duration(3.0);
    landClient.call(srvLand);

    ros::spin();

    return 0;
}

/*############################################################################*/
/*############################################################################*/
/*############################################################################*/

// /*First Order Drone Simulation */
//
//
//
// 	cost<6,6> drone_running_cost(Drone_First_Order_Cost::running_cost,tag7);
// 	cost<6,6> drone_terminal_cost(Drone_First_Order_Cost::terminal_cost,tag8);
// 	dynamics<6,6> drone_dynamics(Drone_First_Order_Dynamics::dynamics,tag9,time_step);
// 	iLQR<6,6,horizon>::input_trajectory u_init_drone;
// 	iLQR<6,6,horizon> drone_solver(drone_running_cost,drone_terminal_cost,drone_dynamics);
// 	Eigen::Matrix<double,6,1> u_drone=Eigen::Matrix<double,6,1>::Zero();
// 	Eigen::Matrix<double,6,1> x0_drone=Eigen::Matrix<double,6,1>::Zero();
// 	Eigen::Matrix<double,6,1> x_goal_drone;
// 	iLQR<6,6,horizon>::state_input_trajectory soln_drone;
// 	iLQR<6,6,total_steps>::state_input_trajectory soln_drone_rhc;
//
// 	//	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// 		std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
// 	//	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
// 	//	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
//
//
//
// 		clock_t t_start, t_end;
// 	//
// 	////
// 		x0_drone<<0,0,0.5, 0,0,0;
// 	//	x0_integrator+=0.1*Eigen::Matrix<double,3,1>::Random();
// 		double seconds;
// 		drone_solver.set_MPC(u_init_drone);
// 		int execution_steps=1;
// 		// soln_drone_rhc.first[0]=x0_drone;
// 		// std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
// 		// std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
// 		t_start = clock();
// 		// for(int i=0;i<total_steps;++i)
//
//         x_goal_drone<<1,1,1, 0,0,0;
//
//         while (ros::ok())
// 		{
//             ros::spinOnce();
//
// 	//		if (i>total_steps/3)
// 	//			x_goal_integrator<<0,0,0;
// 			// std::cout<<"iteration "<<i<<std::endl;
// 			// double scale=0.01;
// 			// auto term1=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// 			// auto term2=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term3=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term4=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term5=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term6=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term7=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term8=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term9=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term10=soln_drone_rhc.first[i]+scale*Eigen::Matrix<double,6,1>::Random();
// //			auto term=(term1+term2+term3+term4+term5+term6+term7+term8+term9+term10)/10;
//
//             x0_drone << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                         CFState_cf1.roll, CFState_cf1.pitch,  CFState_cf1.yaw;
//
// 			soln_drone=drone_solver.run_MPC(x0_drone,
// 					x_goal_drone, 5, execution_steps);
//
//             int idx_to_use = 1;
//
//             std::printf("\n#### SENDING WAYPOINT   ");
//             std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[idx_to_use][0], soln_drone.first[idx_to_use][1], soln_drone.first[idx_to_use][2]);
//
//             srvGoTo_cf1.request.goal.x = soln_drone.first[idx_to_use][0];
//             srvGoTo_cf1.request.goal.y = soln_drone.first[idx_to_use][1];
//             srvGoTo_cf1.request.goal.z = soln_drone.first[idx_to_use][2];
//             srvGoTo_cf1.request.yaw = 0.0;
//             srvGoTo_cf1.request.duration = ros::Duration(1.0);
//             // GoToClient_cf1.call(srvGoTo_cf1);
//
//             int c = getch();   // call your non-blocking input function
//             if (c == '\n') {
//                 std::cout << "########## LANDING ##########" << std::endl;
//                 break;
//             }
//             if (c == 'o') {
//                 std::cout << "########## RETURN TO ORIGIN ##########" << std::endl;
//                 srvGoTo_cf1.request.goal.x = 0.5;
//                 srvGoTo_cf1.request.goal.y = 0.5;
//                 srvGoTo_cf1.request.goal.z = 0.5;
//                 srvGoTo_cf1.request.yaw = 0.0;
//                 srvGoTo_cf1.request.duration = ros::Duration(3.0);
//                 GoToClient_cf1.call(srvGoTo_cf1);
//                 srvGoTo_cf2.request.goal.x = -0.5;
//                 srvGoTo_cf2.request.goal.y = -0.5;
//                 srvGoTo_cf2.request.goal.z = 0.5;
//                 srvGoTo_cf2.request.yaw = 0.0;
//                 srvGoTo_cf2.request.duration = ros::Duration(3.0);
//                 GoToClient_cf2.call(srvGoTo_cf2);
//                 ros::Duration(3.0).sleep();
//                 break;
//             }
// 	////
// 	////////		unsigned int microsecond = 1000000;
// 	////////		usleep(2 * microsecond);
// 	////		write_file<12,4,horizon>(state_path,input_path, soln_drone);
// 			// soln_drone_rhc.first[i+1]=soln_drone.first[1];
// 			// soln_drone_rhc.second[i]=soln_drone.second[0];
// 		}
// 		t_end = clock();
// 	////
// 	//////
// 	////////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// 	////////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
// 		// write_file<6,6,total_steps>(state_path,input_path, soln_drone_rhc);
// 	////
// 		std::cout<<"finished"<<std::endl;
// 		seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
// 		printf("Time: %f s\n", seconds);
//
//
//
//
// /*First Order Drone Simulation Ends*/

// /* This part is single drone simulation*/
//
//     cost<12,4> running_cost_dr(Drone_Cost::running_cost,tag1);
//     cost<12,4> terminal_cost_dr(Drone_Cost::terminal_cost,tag2);
//     dynamics<12,4> dynamics_dr(Drone_Dynamics::dynamics,tag3,time_step);
//
//     iLQR<12,4,horizon> drone_solver(running_cost_dr,terminal_cost_dr,dynamics_dr);
//     Eigen::Matrix<double,4,1> u_drone=Eigen::Matrix<double,4,1>::Ones();
//     Eigen::Matrix<double,12,1> X0_drone=Eigen::Matrix<double,12,1>::Zero();
// ////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//     Eigen::Matrix<double,12,1> X_goal_drone;
//
//     u_drone=u_drone*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// //
//     iLQR<12,4,horizon>::input_trajectory u_init_drone;
//     std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
//     iLQR<12,4,horizon>::state_input_trajectory soln_drone;
//     iLQR<12,4,total_steps>::state_input_trajectory soln_drone_rhc;
//
//     clock_t t_start, t_end;
//
// //
//     double seconds;
//     drone_solver.set_MPC(u_init_drone);
//     int execution_steps=1;
//     soln_drone_rhc.first[0]=X0_drone;
//     // std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//     // std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//     t_start = clock();
//     // for(int i=0;i<total_steps;++i)
//
//
//     X_goal_drone<<-1.0,1.0,1.0, 0,0,0, 0,0,0, 0,0,0;
//
//
//     while (ros::ok())
//     {
//         // if (i>total_steps/3)
//         //     X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//         // std::cout<<"iteration "<<i<<std::endl;
//
//         ros::spinOnce();
// ////
//         X0_drone << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                     CFState_cf1.roll, CFState_cf1.pitch,  CFState_cf1.yaw,
//                     CFState_cf1.x_d, CFState_cf1.y_d, CFState_cf1.z_d,
//                     CFState_cf1.roll_d, CFState_cf1.pitch_d, CFState_cf1.yaw_d;
//
//         soln_drone=drone_solver.run_MPC(X0_drone, X_goal_drone, 20, execution_steps);
//
//         // ros::Duration(0.5).sleep();
// //
// //////		unsigned int microsecond = 1000000;
// //////		usleep(2 * microsecond);
// //		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//         // soln_drone_rhc.first[i+1]=soln_drone.first[1];
//         // soln_drone_rhc.second[i]=soln_drone.second[0];
//
//         // std::printf("\n\n");
//         // std::cout << X0_drone << std::endl;
//         // std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[4][0], soln_drone.first[4][1], soln_drone.first[4][2]);
//
//         std::printf("\n#### SENDING WAYPOINT   ");
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[4][0], soln_drone.first[4][1], soln_drone.first[4][2]);
//
//         srvGoTo_cf1.request.goal.x = soln_drone.first[4][0];
//         srvGoTo_cf1.request.goal.y = soln_drone.first[4][1];
//         srvGoTo_cf1.request.goal.z = soln_drone.first[4][2];
//         srvGoTo_cf1.request.yaw = 0.0;
//         srvGoTo_cf1.request.duration = ros::Duration(3.0);
//         GoToClient_cf1.call(srvGoTo_cf1);
//
//         int c = getch();   // call your non-blocking input function
//         if (c == '\n') {
//             std::cout << "########## LANDING ##########" << std::endl;
//             break;
//         }
//
//         // ros::spinOnce();
//
//     }
//     t_end = clock();
//
//
// ////
// ////
// //////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// //////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//     // write_file<12,4,total_steps>(state_path,input_path, soln_drone_rhc);
// //
//     std::cout<<"finished"<<std::endl;
//     seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//     printf("Time: %f s\n", seconds);
//
//
// /* End of single drone simulation*/

// cost<3,3> integrator_running_cost(Single_Integrator_Cost::running_cost,tag7);
// cost<3,3> integrator_terminal_cost(Single_Integrator_Cost::terminal_cost,tag8);
// dynamics<3,3> integrator_dynamics(Single_Integrator_3D::dynamics,tag9,time_step);
// iLQR<3,3,horizon>::input_trajectory u_init_integrator;
// iLQR<3,3,horizon> integrator_solver(integrator_running_cost,integrator_terminal_cost,integrator_dynamics);
// Eigen::Matrix<double,3,1> u_integrator=Eigen::Matrix<double,3,1>::Zero();
// Eigen::Matrix<double,3,1> x0_integrator=Eigen::Matrix<double,3,1>::Zero();
// Eigen::Matrix<double,3,1> x_goal_integrator;
// iLQR<3,3,horizon>::state_input_trajectory soln_integrator;
// iLQR<3,3,total_steps>::state_input_trajectory soln_integrator_rhc;
//
// //	u_drone2=u_drone2*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// //	std::fill(std::begin(u_init_drone2), std::end(u_init_drone2), u_drone2);
// //	iLQR<12*2,4*2,horizon>::state_input_trajectory soln_drone2;
// //	iLQR<12*2,4*2,total_steps>::state_input_trajectory soln_drone2_rhc;
//
//
// clock_t t_start, t_end;
//
// double seconds;
// integrator_solver.set_MPC(u_init_integrator);
// int execution_steps=1;
// soln_integrator_rhc.first[0]=x0_integrator;
// std::string state_path="/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/states.txt";
// std::string input_path="/home/ayberk/Documents/ACTIONLAB/crazyswarm/ros_ws/src/iLQR/datalog/inputs.txt";
// t_start = clock();
//
// /* Goal position of trajectory */
// x_goal_integrator << -1.0,1.0,1.0;
//
// while (ros::ok())
// {
//
//     /* Spin once for ROS callback to work */
//     ros::spinOnce();
//
//     /* populate state vector with current state */
//     x0_integrator << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z;
//
//     /* run iLQR algorithm */
// 	soln_integrator = integrator_solver.run_MPC(x0_integrator,
// 			                                    x_goal_integrator,
//                                                 20,
//                                                 execution_steps);
//
//     /* Write solution to file for debugging */
//     write_file<3,3,horizon>(state_path, input_path, soln_integrator);
// 	// soln_integrator_rhc.first[i+1]=soln_integrator.first[1];
// 	// soln_integrator_rhc.second[i]=soln_integrator.second[0];
//
//     // std::printf("\nX0_integrator: \n");
//     // std::cout << x0_integrator << std::endl;
//
//     std::printf("\n#### SENDING WAYPOINT");
//     std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_integrator.first[1][0], soln_integrator.first[1][1], soln_integrator.first[1][2]);
//
//     /* Populate GoTo command with waypoint and call service */
//     srvGoTo_cf1.request.goal.x = soln_integrator.first[1][0];
//     srvGoTo_cf1.request.goal.y = soln_integrator.first[1][1];
//     srvGoTo_cf1.request.goal.z = soln_integrator.first[1][2];
//     srvGoTo_cf1.request.yaw = 0.0;
//     srvGoTo_cf1.request.duration = ros::Duration(1.0);
//     GoToClient_cf1.call(srvGoTo_cf1);
//
//     // srvGoTo_cf2.request.goal.x = soln_integrator.first[4][0];
//     // srvGoTo_cf2.request.goal.y = soln_integrator.first[4][1];
//     // srvGoTo_cf2.request.goal.z = soln_integrator.first[4][2];
//     // srvGoTo_cf2.request.yaw = 0.0;
//     // srvGoTo_cf2.request.duration = ros::Duration(3.0);
//     // GoToClient_cf2.call(srvGoTo_cf2);
//
//
//     /* Check if <Enter> key way pressed */
//     int c = getch();
//     if (c == '\n') {
//         std::cout << "\t##### LANDING #####" << std::endl;
//         break;
//     }
//
//
// }
// t_end = clock();
//
//
// // soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// // write_file<12,4,horizon>(state_path,input_path, soln_drone);
// // write_file<3,3,total_steps>(state_path,input_path, soln_integrator_rhc);
//
//
// std::cout<<"finished"<<std::endl;
// seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
// printf("Time: %f s\n", seconds);

// /* This part is single drone simulation*/
//
//     cost<12,4> running_cost_dr(Drone_Cost::running_cost,tag1);
//     cost<12,4> terminal_cost_dr(Drone_Cost::terminal_cost,tag2);
//     dynamics<12,4> dynamics_dr(Drone_Dynamics::dynamics,tag3,time_step);
//
//     iLQR<12,4,horizon> drone_solver(running_cost_dr,terminal_cost_dr,dynamics_dr);
//     Eigen::Matrix<double,4,1> u_drone=Eigen::Matrix<double,4,1>::Ones();
//     Eigen::Matrix<double,12,1> X0_drone=Eigen::Matrix<double,12,1>::Zero();
// ////	X0_drone<<0,-50,10, M_PI/20,0,0, 0,0,0, 0,0,0;
//     Eigen::Matrix<double,12,1> X_goal_drone;
//
//     u_drone=u_drone*sqrt(Drone_Dynamics::mass*Drone_Dynamics::g/(4*Drone_Dynamics::C_T));
// //
//     iLQR<12,4,horizon>::input_trajectory u_init_drone;
//     std::fill(std::begin(u_init_drone), std::end(u_init_drone), u_drone);
//     iLQR<12,4,horizon>::state_input_trajectory soln_drone;
//     iLQR<12,4,total_steps>::state_input_trajectory soln_drone_rhc;
//
//     std::cout << "DEBUG 1" << std::endl;
//
//     clock_t t_start, t_end;
//
// //
//     double seconds;
//     drone_solver.set_MPC(u_init_drone);
//     int execution_steps=1;
//     soln_drone_rhc.first[0]=X0_drone;
//     // std::string state_path="/Users/talhakavuncu/Desktop/research/cpp_code/states.txt";
//     // std::string input_path="/Users/talhakavuncu/Desktop/research/cpp_code/inputs.txt";
//     t_start = clock();
//     // for(int i=0;i<total_steps;++i)
//
//
//     X_goal_drone<<1.0,1.0,1.5, 0,0,0, 0,0,0, 0,0,0;
//
//
//     while (ros::ok())
//     {
//         // if (i>total_steps/3)
//         //     X_goal_drone<<0,0,0, 0,0,0, 0,0,0, 0,0,0;
//         // std::cout<<"iteration "<<i<<std::endl;
//
//         ros::spinOnce();
// ////
//         X0_drone << CFState_cf1.x, CFState_cf1.y, CFState_cf1.z,
//                     CFState_cf1.roll, CFState_cf1.pitch,  CFState_cf1.yaw,
//                     CFState_cf1.x_d, CFState_cf1.y_d, CFState_cf1.z_d,
//                     CFState_cf1.roll_d, CFState_cf1.pitch_d, CFState_cf1.yaw_d;
//
//         soln_drone=drone_solver.run_MPC(X0_drone, X_goal_drone, 100, execution_steps);
//
//         // ros::Duration(0.5).sleep();
// //
// //////		unsigned int microsecond = 1000000;
// //////		usleep(2 * microsecond);
// //		write_file<12,4,horizon>(state_path,input_path, soln_drone);
//         // soln_drone_rhc.first[i+1]=soln_drone.first[1];
//         // soln_drone_rhc.second[i]=soln_drone.second[0];
//
//         std::printf("\n\n");
//         std::cout << X0_drone << std::endl;
//         std::printf("X: %f,\t Y: %f,\t Z: %f\n", soln_drone.first[39][0], soln_drone.first[39][1], soln_drone.first[39][2]);
//
//         srvGoTo_cf2.request.goal.x = soln_drone.first[39][0];
//         srvGoTo_cf2.request.goal.y = soln_drone.first[39][1];
//         srvGoTo_cf2.request.goal.z = soln_drone.first[39][2];
//         srvGoTo_cf2.request.yaw = 0.0;
//         srvGoTo_cf2.request.duration = ros::Duration(5.0);
//         GoToClient_cf1.call(srvGoTo_cf2);
//
//         int c = getch();   // call your non-blocking input function
//         if (c == '\n') {
//             std::cout << "LANDING" << std::endl;
//             break;
//         }
//
//         // ros::spinOnce();
//
//     }
//     t_end = clock();
//
//
// ////
// ////
// //////	soln_drone=drone_solver.solve_open_loop(X0_drone, X0_drone, 200, u_init_drone, horizon);
// //////	write_file<12,4,horizon>(state_path,input_path, soln_drone);
//     // write_file<12,4,total_steps>(state_path,input_path, soln_drone_rhc);
// //
//     std::cout<<"finished"<<std::endl;
//     seconds = 1/((double)(t_end - t_start) / CLOCKS_PER_SEC/total_steps);
//     printf("Time: %f s\n", seconds);
//
//
// /* End of single drone simulation*/

/*############################################################################*/
/*############################################################################*/
/*############################################################################*/

// /* Simple moving average filter */
// #if (USE_SMA_FILTER == 1)
//
//     /* Propogate state measurements */
//     for (int idx = 1; idx < SMA_WINDOW_LEN; ++idx) {
//         stateWindowBuf_cf1[idx] = stateWindowBuf_cf1[idx-1];
//         stateWindowBuf_cf2[idx] = stateWindowBuf_cf2[idx-1];
//     }
//
//     /* Append current state to head of buffer */
//     stateWindowBuf_cf1[0] = CFState_cf1;
//     stateWindowBuf_cf2[0] = CFState_cf2;
//
//     // /* Set all state variables to zero */
//     // memset(&CFState_cf1, 0x00, sizeof(stateVector));
//     // memset(&CFState_cf2, 0x00, sizeof(stateVector));
//
//     static stateVector tempState_cf1, tempState_cf2;
//
//     /* Accumulate state variables */
//     for (int idx = 0; idx < SMA_WINDOW_LEN; ++idx) {
//         tempState_cf1.x += stateWindowBuf_cf1[idx].x;
//     	tempState_cf1.y += stateWindowBuf_cf1[idx].y;
//     	tempState_cf1.z += stateWindowBuf_cf1[idx].z;
//         tempState_cf1.roll += stateWindowBuf_cf1[idx].roll;
//     	tempState_cf1.pitch += stateWindowBuf_cf1[idx].pitch;
//     	tempState_cf1.yaw += stateWindowBuf_cf1[idx].yaw;
//         tempState_cf1.x_d += stateWindowBuf_cf1[idx].x_d;
//     	tempState_cf1.y_d += stateWindowBuf_cf1[idx].y_d;
//     	tempState_cf1.z_d += stateWindowBuf_cf1[idx].z_d;
//     	tempState_cf1.roll_d += stateWindowBuf_cf1[idx].roll_d;
//     	tempState_cf1.pitch_d += stateWindowBuf_cf1[idx].pitch_d;
//     	tempState_cf1.yaw_d += stateWindowBuf_cf1[idx].yaw_d;
//
//         tempState_cf2.x += stateWindowBuf_cf2[idx].x;
//     	tempState_cf2.y += stateWindowBuf_cf2[idx].y;
//     	tempState_cf2.z += stateWindowBuf_cf2[idx].z;
//         tempState_cf2.roll += stateWindowBuf_cf2[idx].roll;
//     	tempState_cf2.pitch += stateWindowBuf_cf2[idx].pitch;
//     	tempState_cf2.yaw += stateWindowBuf_cf2[idx].yaw;
//         tempState_cf2.x_d += stateWindowBuf_cf2[idx].x_d;
//     	tempState_cf2.y_d += stateWindowBuf_cf2[idx].y_d;
//     	tempState_cf2.z_d += stateWindowBuf_cf2[idx].z_d;
//     	tempState_cf2.roll_d += stateWindowBuf_cf2[idx].roll_d;
//     	tempState_cf2.pitch_d += stateWindowBuf_cf2[idx].pitch_d;
//     	tempState_cf2.yaw_d += stateWindowBuf_cf2[idx].yaw_d;
//     }
//
//     CFState_cf1.x = tempState_cf1.x / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.y = tempState_cf1.y / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.z = tempState_cf1.z / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.x = tempState_cf1.roll / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.y = tempState_cf1.pitch / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.z = tempState_cf1.yaw / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.x_d = tempState_cf1.x_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.y_d = tempState_cf1.y_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.z_d = tempState_cf1.z_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.roll_d = tempState_cf1.roll_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.pitch_d = tempState_cf1.pitch_d / ((float) SMA_WINDOW_LEN);
//     CFState_cf1.yaw_d = tempState_cf1.yaw_d / ((float) SMA_WINDOW_LEN);
//
//     // CFState_cf2.x = CFState_cf2.x / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.y = CFState_cf2.y / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.z = CFState_cf2.z / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.x = CFState_cf2.roll / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.y = CFState_cf2.pitch / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.z = CFState_cf2.yaw / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.x_d = CFState_cf2.x_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.y_d = CFState_cf2.y_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.z_d = CFState_cf2.z_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.roll_d = CFState_cf2.roll_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.pitch_d = CFState_cf2.pitch_d / ((float) SMA_WINDOW_LEN);
//     // CFState_cf2.yaw_d = CFState_cf2.yaw_d / ((float) SMA_WINDOW_LEN);
//
// #endif

// /* First order exponential low-pass filter */
// #if (USE_EXP_FILTER == 1)

    // float alpha = 0.9;
    //
    // CFState_cf1.x = (prevState_cf1.x * alpha) + (1.0 - alpha) * CFState_cf1.x;
    // CFState_cf1.y = (prevState_cf1.y * alpha) + (1.0 - alpha) * CFState_cf1.y;
    // CFState_cf1.z = (prevState_cf1.z * alpha) + (1.0 - alpha) * CFState_cf1.z;
    // CFState_cf1.roll = (prevState_cf1.roll * alpha) + (1.0 - alpha) * CFState_cf1.roll;
    // CFState_cf1.pitch = (prevState_cf1.pitch * alpha) + (1.0 - alpha) * CFState_cf1.pitch;
    // CFState_cf1.yaw = (prevState_cf1.yaw * alpha) + (1.0 - alpha) * CFState_cf1.yaw;
    // CFState_cf1.x_d = (prevState_cf1.x_d * alpha) + (1.0 - alpha) * CFState_cf1.x_d;
    // CFState_cf1.y_d = (prevState_cf1.y_d * alpha) + (1.0 - alpha) * CFState_cf1.y_d;
    // CFState_cf1.z_d = (prevState_cf1.z_d * alpha) + (1.0 - alpha) * CFState_cf1.z_d;
    // CFState_cf1.roll_d = (prevState_cf1.roll_d * alpha) + (1.0 - alpha) * CFState_cf1.roll_d;
    // CFState_cf1.pitch_d = (prevState_cf1.pitch_d * alpha) + (1.0 - alpha) * CFState_cf1.pitch_d;
    // CFState_cf1.yaw_d = (prevState_cf1.yaw_d * alpha) + (1.0 - alpha) * CFState_cf1.yaw_d;

    // CFState_cf2.x = (prevState_cf2.x * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.x;
    // CFState_cf2.y = (prevState_cf2.y * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.y;
    // CFState_cf2.z = (prevState_cf2.z * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.z;
    // CFState_cf2.roll = (prevState_cf2.roll * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.roll;
    // CFState_cf2.pitch = (prevState_cf2.pitch * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.pitch;
    // CFState_cf2.yaw = (prevState_cf2.yaw * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.yaw;
    // CFState_cf2.x_d = (prevState_cf2.x_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.x_d;
    // CFState_cf2.y_d = (prevState_cf2.y_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.y_d;
    // CFState_cf2.z_d = (prevState_cf2.z_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.z_d;
    // CFState_cf2.roll_d = (prevState_cf2.roll_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.roll_d;
    // CFState_cf2.pitch_d = (prevState_cf2.pitch_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.pitch_d;
    // CFState_cf2.yaw_d = (prevState_cf2.yaw_d * EXP_FILT_ALPHA) + (1.0 - EXP_FILT_ALPHA) * CFState_cf2.yaw_d;

    // prevState_cf1 = CFState_cf1;
    // prevState_cf2 = CFState_cf2;

// #endif

// /* Draw square trajectory */
//
// srvGoTo_cf2.request.goal.x = 0.0;
// srvGoTo_cf2.request.goal.y = 0.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 0.0;
// srvGoTo_cf2.request.goal.y = 1.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 1.0;
// srvGoTo_cf2.request.goal.y = 1.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 1.0;
// srvGoTo_cf2.request.goal.y = 0.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();
//
// srvGoTo_cf2.request.goal.x = 0.0;
// srvGoTo_cf2.request.goal.y = 0.0;
// srvGoTo_cf2.request.goal.z = 2.0;
// srvGoTo_cf2.request.yaw = 0.0;
// srvGoTo_cf2.request.duration = ros::Duration(3.0);
// GoToClient_cf1.call(srvGoTo_cf2);
//
// ros::Duration(1.0).sleep();

// /* Control loop */
// while (ros::ok()) {
//
//     int c = getch();   // call your non-blocking input function
//     if (c == '\n') {
//         std::cout << "LANDING" << std::endl;
//         break;
//     }
//
//     ros::spinOnce();
//
// 	// position_cmd.x = 0;
// 	// position_cmd.y = 0;
// 	// position_cmd.z = 2.0;
// 	// position_cmd.yaw = 0.0;
// 	// pub.publish(position_cmd);
//
//
// }


#pragma once

// deal.II includes -------------------------------------------------------
#include <deal.II/base/exceptions.h>
#include <deal.II/base/index_set.h>
#include <deal.II/base/mpi.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/tensor.h>
#include <deal.II/base/timer.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_handler.h>

#ifdef DEBUG
#include <deal.II/base/conditional_ostream.h>
#endif

// system includes --------------------------------------------------------
#include <yaml-cpp/yaml.h>
#include <boost/multi_array.hpp>
#include <cmath>
#include <complex>
#include <cstdio>
#include <fstream>
#include <functional>
#include <limits>
#include <stdexcept>
#include <tuple>
#include <unordered_map>
// trilinos includes ------------------------------------------------------
#include <Epetra_Export.h>
#include <Epetra_Import.h>
#include <Epetra_Map.h>
#include <Epetra_Vector.h>

// own includes -----------------------------------------------------------
#include "bc_traits.hpp"
#include "grid/dof_mapper_periodic_distributed.hpp"
#include "impl/bd_faces_manager_redist.hpp"
#include "impl/bd_faces_manager_simple.hpp"
#include "quadrature/qhermitew.hpp"
#include "quadrature/qmaxwell.hpp"
#include "spectral/basis/dof_mapper.hpp"
#include "spectral/basis/indexer.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/hermite_to_nodal.hpp"
#include "spectral/polar_to_hermite.hpp"
#include "spectral/rotate_basis.hpp"
#include "base/numbers.hpp"

namespace boltzmann {
namespace local_ {
// helper struct to keep quadrature rules for inflow rhs
struct inflow_bc_quad
{
  /**
   *
   * @param npts quad. degree
   * @param Tw   inflow temperature
   * @param w    basis weight, e.g. exp(-r^2/2) => w=0.5
   *
   *
   * @return
   */
  inflow_bc_quad(int npts, double Tw, double w = 0.5);

  inflow_bc_quad() {}

  QHermiteW x_quad;
  QMaxwell y_quad;
  // hermite polynomials

  typedef HermiteNW<double> hermw_t;
  std::shared_ptr<hermw_t> hermwx;
  std::shared_ptr<hermw_t> hermwy;
};

inflow_bc_quad::inflow_bc_quad(int npts, double Tw, double w)
    : x_quad(w + 1. / (2 * Tw), npts)
    , y_quad(w + 1. / (2 * Tw), npts)
    , hermwx(std::make_shared<HermiteNW<double> >(npts))
    , hermwy(std::make_shared<HermiteNW<double> >(npts))
// 256 digits (mpfr accuracy)
{
  hermwx->compute(x_quad.pts());

  std::vector<double> y(npts);
  for (int i = 0; i < npts; ++i) {
    y[i] = -y_quad.pts(i);
  }

  hermwy->compute(y);
}
}  // end namespace local_

// -------------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
class BoundaryConditions : public BD_FACES_MANAGER
{
 private:
  typedef BD_FACES_MANAGER bd_faces_manager_t;
  typedef dealii::types::global_dof_index size_type;
  typedef dealii::DoFHandler<2> dh_t;
  typedef typename METHOD::spectral_basis_t spectral_basis_t;
  typedef typename traits::DoFMapper<APP::bc_type>::type mapper_t;
  typedef Indexer<mapper_t> indexer_t;
  typedef dealii::TrilinosWrappers::MPI::Vector deal_vector_t;

  // boundary id
  typedef unsigned int bid_t;

 public:
  BoundaryConditions(double dt,
                     const dh_t& dh,
                     const spectral_basis_t& spectral_basis,
                     const indexer_t& indexer,
                     const YAML::Node& config);

  void apply(deal_vector_t& out, const deal_vector_t& in) const;

  void apply(Epetra_MultiVector& out, const Epetra_MultiVector& in) const;

  /**
   *  @brief Assemble inflow type boundary conditions into the right hand side
   *
   */
  template <typename VECTOR>
  void assemble_rhs(VECTOR& dst) const;

 private:
  typedef SpectralBasisFactoryHN::basis_type hermite_basis_t;
  typedef std::shared_ptr<impl::flux_worker> ptr_flux_worker;

 private:
  double dt_;
  const dh_t& dh_;
  const spectral_basis_t& spectral_basis_;
  const indexer_t& indexer_;
  const YAML::Node& config_;

  //@{
  /// spectral basis transformation / rotation operators
  RotateBasis<spectral_basis_t> R_;
  hermite_basis_t hermite_basis_;
  typedef Polar2Hermite<spectral_basis_t, hermite_basis_t> P2H_t;
  typedef Hermite2Nodal<hermite_basis_t> H2N_t;
  std::shared_ptr<P2H_t> ptr_P2H_;
  std::shared_ptr<H2N_t> ptr_H2N_;
  //@}

  std::map<bid_t, ptr_flux_worker> flux_workers_;
  std::set<bid_t> periodic_;

  //@{
  /*  Buffers for Lagrange coefficients
   *  1p: output from flux_worker for vertex 1
   *  2p: output from flux_worker for vertex 2
   */
  mutable Eigen::MatrixXd L1p_;  // output f (1. trial) for flux worker
  mutable Eigen::MatrixXd L1_;   // input f  (1. tiral function) ..
  mutable Eigen::MatrixXd L2p_;  // output f  (2. trial function) ..
  mutable Eigen::MatrixXd L2_;
  mutable Eigen::MatrixXd L_;
  //@}

  /// buffer for hermite coefficients
  /// ybuffer
  mutable Eigen::VectorXd ybuf_;

  // polynomial degree
  int K_ = -1;

  // TODO Epetra Multi vector, Import, Export, Maps
  // imports from base class
  using bd_faces_manager_t::relevant_dofs_;
  using bd_faces_manager_t::get_faces_list;
  // epetra vectors
  mutable std::shared_ptr<Epetra_Vector> x_ghosted_;
  mutable std::shared_ptr<Epetra_Vector> y_ghosted_;
  mutable std::shared_ptr<Epetra_Import> importer_;
  mutable std::shared_ptr<Epetra_Export> exporter_;
};

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::BoundaryConditions(
    double dt,
    const dh_t& dh,
    const spectral_basis_t& spectral_basis,
    const indexer_t& indexer,
    const YAML::Node& config)
    : bd_faces_manager_t(dh, spectral_basis, indexer)
    , dt_(dt)
    , dh_(dh)
    , spectral_basis_(spectral_basis)
    , indexer_(indexer)
    , config_(config)
    , R_(spectral_basis)
{
  int K_ = config_["SpectralBasis"]["deg"].as<int>();

  // initialize hermite basis
  SpectralBasisFactoryHN::create(hermite_basis_, K_, 2);
  static QHermiteW hermite_quad(1.0, K_);

  Eigen::VectorXd hw = hermite_quad.vwts<1>();
  Eigen::VectorXd hx = hermite_quad.vpts<1>();

  // resize buffers
  L1_.resize(K_, K_);
  L2_.resize(K_, K_);
  L1p_.resize(K_, K_);
  L2p_.resize(K_, K_);
  L_.resize(K_, K_);
  ybuf_.resize(spectral_basis_.n_dofs());

  typedef Eigen::MatrixXd mat_t;
  ptr_P2H_ = std::make_shared<P2H_t>(spectral_basis_, hermite_basis_);
  ptr_H2N_ = std::make_shared<H2N_t>(
      hermite_basis_, K_, [K_](mat_t& m1, mat_t& m2) { H2N_1d<>::create(m1, m2, K_); });

  auto node = config_["BoundaryDescriptors"];
  // TODO: process config
  for (YAML::const_iterator it = node.begin(); it != node.end(); ++it) {
    auto entry = it->second;
    id_t id = it->first.as<id_t>();
    std::string type = entry["type"].as<std::string>();

    typedef bc_traits<METHOD::lsq_type> bc_traits_t;
    if (std::strcmp(type.c_str(), "diffusive reflection") == 0) {
      double T = entry["T"].as<double>();
      double rho = 1.0;
      if (entry["rho"]) {
        rho = entry["rho"].as<double>();
      }
      if (entry["vt"]) {
        double vt = entry["vt"].as<double>();
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflection>(hw, hx, vt, T, rho);
      } else {
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflection>(hw, hx, 0, T, rho);
      }
    } else if (std::strcmp(type.c_str(), "diffusive reflection x") == 0) {
      std::string Tx = entry["Tx"].as<std::string>();
      if (entry["vt"]) {
        double vt = entry["vt"].as<double>();
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflectionX>(hw, hx, vt, Tx);
      } else {
        flux_workers_[id] =
            std::make_shared<typename bc_traits_t::DiffusiveReflectionX>(hw, hx, 0, Tx);
      }
    }
    else if (std::strcmp(type.c_str(), "specular reflection") == 0) {
      flux_workers_[id] = std::make_shared<typename bc_traits_t::SpecularReflection>(hw, hx);
    } else if (std::strcmp(type.c_str(), "inflow") == 0) {
      flux_workers_[id] = std::make_shared<typename bc_traits_t::Inflow>(hw, hx);

    } else if (std::strcmp(type.c_str(), "periodic") == 0) {
      periodic_.insert(id);
    } else {
      AssertThrow(false, dealii::ExcMessage("Unkown boundary condition entry"));
    }
  }

  auto epetra_map = relevant_dofs_.make_trilinos_map(MPI_COMM_WORLD, true);

  x_ghosted_ = std::make_shared<Epetra_Vector>(epetra_map);
  y_ghosted_ = std::make_shared<Epetra_Vector>(epetra_map);
}

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
template <typename VECTOR>
void
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::assemble_rhs(VECTOR& dst) const
{
  /* inflow boundary conditions need to be assembled into the rhs vector. this is done in this
     routine */
  const int K = spectral::get_max_k(spectral_basis_);
  std::map<double, local_::inflow_bc_quad> inflow_bc_quad_map;
  auto& bd_conf = config_["BoundaryDescriptors"];
  const int npts = 2 * K;

  // --------------------------------------------------
  // prepare quadrature rules
  for (YAML::const_iterator it = bd_conf.begin(); it != bd_conf.end(); ++it) {
    auto entry = it->second;
    id_t id = it->first.as<id_t>();

    std::string type = entry["type"].as<std::string>();
    if (std::strcmp(type.c_str(), "inflow") == 0) {
      // currently inflow function g(v), can be of Maxwellian type only
      if (std::strcmp(entry["func"].as<std::string>().c_str(), "maxwellian") == 0) {
        double Tw = entry["T"].as<double>();
        // basis exp weight
        const double basis_alpha = 0.5;
        inflow_bc_quad_map[Tw] = local_::inflow_bc_quad(npts, Tw, basis_alpha);
      }
    }
  }

  // --------------------------------------------------
  // prepare deal.II stuff
  const int dimX = 2;
  unsigned int N = spectral_basis_.n_dofs();
  dealii::UpdateFlags update_flags = dealii::update_values | dealii::update_JxW_values |
                                     dealii::update_quadrature_points |
                                     dealii::update_normal_vectors;

  dealii::QGauss<dimX - 1> quad(2);
  int n_qpoints = quad.size();

  auto& fe = dh_.get_fe();
  const int dofs_per_cell = fe.dofs_per_cell;
  dealii::FEFaceValues<2> fe_face_values(fe, quad, update_flags);
  std::vector<size_type> local_dof_indices(fe.dofs_per_cell);

  const auto& faces_list = this->get_faces_list();

  typedef typename VECTOR::size_type size_type;
  std::vector<size_type> global_indices(N);

  Eigen::VectorXd II(N);
  Eigen::VectorXd IIp(N);    // buffer to add into trilinos vector
  Eigen::VectorXd IIloc(N);  // buffer to add into trilinos vector

  // iterate over faces at boundary
  for (const auto& mypair : faces_list) {
    const auto& cell = std::get<0>(mypair);
    int face_idx = std::get<1>(mypair);
    id_t bd_ind = cell.face(face_idx)->boundary_id();

    // check if bc is periodic.
    std::string type = config_["BoundaryDescriptors"][bd_ind]["type"].as<std::string>();
    if (periodic_.find(bd_ind) != periodic_.end() || std::strcmp(type.c_str(), "inflow") != 0)
      // bc is periodic: nothing to do
      continue;
    // --------------------------------------------------
    // load parameter
    auto params = config_["BoundaryDescriptors"][bd_ind];

    std::string func = params["func"].as<std::string>();
    if (std::strcmp(func.c_str(), "zero") == 0) {
      // nothing to do
      continue;
    }
    // check the inflow function
    BAssertThrow(std::strcmp(func.c_str(), "maxwellian") == 0,
                 "BoundaryConditions::assemble_rhs, do not know how to handle boundary type.\n" +
                     "Type was: " + func);

    double Tw = params["T"].as<double>();

    auto iter = inflow_bc_quad_map.find(Tw);
    assert(iter != inflow_bc_quad_map.end());
    const auto& quad_helper = iter->second;
    Eigen::Vector2d v0 = {0, 0};
    if (params["v"]) {
      double vx = params["v"][0].as<double>();
      double vy = params["v"][1].as<double>();
      v0 = {vx, vy};
    }

    double rho;
    if (params["rho"])
      rho = params["rho"].as<double>();
    else
      rho = 1;

    typedef dealii::DoFCellAccessor<dealii::DoFHandler<2>, false> accessor_t;
    typedef dealii::TriaIterator<accessor_t> tria_iterator_t;

    cell.get_dof_indices(local_dof_indices);
    fe_face_values.reinit(tria_iterator_t(cell), face_idx);

    const double nx = fe_face_values.normal_vector(0)[0];
    const double ny = fe_face_values.normal_vector(0)[1];
    // winkel zwischen y-achse und n
    const double alpha = numbers::PI / 2 - std::atan2(ny, nx);

    std::array<double, 4> B;
    B.fill(0);
    for (unsigned int l1 = 0; l1 < fe.dofs_per_cell; ++l1) {
      double val = 0;
      for (int q = 0; q < n_qpoints; ++q) {
        val += fe_face_values.shape_value(l1, q) * fe_face_values.JxW(q);
      }
      B[l1] = val;
    }

    const double v02 = v0.squaredNorm();
    // hermite basis accessors
    typedef typename hermite_basis_t::elem_t elem_t;
    typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type hx_t;
    typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type hy_t;
    typename elem_t::Acc::template get<hx_t> get_hx;
    typename elem_t::Acc::template get<hy_t> get_hy;

    auto hermwx = quad_helper.hermwx;
    auto hermwy = quad_helper.hermwy;
    const auto& xpts = quad_helper.x_quad.pts();
    const auto& xwts = quad_helper.x_quad.wts();
    const auto& ypts = quad_helper.y_quad.pts();
    const auto& ywts = quad_helper.y_quad.wts();

    Eigen::Rotation2D<double> rot2d(alpha);
    Eigen::Matrix2d rotm = rot2d.toRotationMatrix();
    auto v0h = rotm * v0;
    /*
     *  HINT: about Hermite quad. rule: (used in x-direction) The weights are
     *     multiplied by e^(x^2/2), which accounts for the factor e^(-x^2/2)
     *     which is included in the evaluation of the Hermite polynomials (e.g.
     *     Hermite functions in this case)
     */
    for (unsigned int j = 0; j < N; ++j) {
      // iterate over test functions
      const auto& elem = hermite_basis_.get_elem(j);
      const unsigned int jx = get_hx(elem).get_id().k;
      const unsigned int jy = get_hy(elem).get_id().k;
      double val = 0;
      // 2d quadrature
      for (int qx = 0; qx < npts; ++qx) {
        for (int qy = 0; qy < npts; ++qy) {
          Eigen::Vector2d vh = {xpts[qx], -1.0 * ypts[qy]};
          // inflow => -1.0
          val -= hermwx->get(jx)[qx] * hermwy->get(jy)[qy] * std::exp(v0h.dot(vh) / Tw) *
                 (std::exp(-xpts[qx] * xpts[qx] / 2 / Tw) * xwts[qx]) *
                 (std::exp(vh[1] * vh[1] / 2) * ywts[qy]);
        }
      }
      II(j) = std::exp(-v02 / 2 / Tw) * val * rho / (2 * numbers::PI * Tw);
    }
    // transform to polar
    ptr_P2H_->to_hermite_T(IIp, II);
    // rotate back
    R_.apply(IIloc.data(), IIp.data(), -alpha);

    for (unsigned int l1 = 0; l1 < fe.dofs_per_cell; ++l1) {
      if (std::abs(B[l1]) > 1e-15) {
        for (unsigned int j = 0; j < N; ++j) {
          global_indices[j] = indexer_.to_global(local_dof_indices[l1], j);
        }
        II = IIloc * B[l1];
        dst.add(N, global_indices.data(), II.data());
      }
    }
  }

  dst.compress(dealii::VectorOperation::add);
}

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
void
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::apply(deal_vector_t& out,
                                                         const deal_vector_t& in) const
{
  this->apply(out.trilinos_vector(), in.trilinos_vector());
}

// --------------------------------------------------------------------------------
template <typename METHOD, typename APP, typename BD_FACES_MANAGER>
void
BoundaryConditions<METHOD, APP, BD_FACES_MANAGER>::apply(Epetra_MultiVector& out,
                                                         const Epetra_MultiVector& in) const
{
#ifdef DEBUG
  out.SetTracebackMode(2);
  in.SetTracebackMode(2);
  x_ghosted_->SetTracebackMode(2);
  y_ghosted_->SetTracebackMode(2);
#endif

  const int dimX = 2;
  // ATTENTION: this makes assumptions on deal.II internals
  static size_type faces_vertex[4][2] = {{0, 2}, {1, 3}, {0, 1}, {2, 3}};

  static dealii::Tensor<2, 2> B;

#ifdef DEBUG
  const unsigned int pid = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  dealii::ConditionalOStream pcout(std::cout, pid == 0);
#endif

  if (importer_.use_count() == 0) {
#ifdef DEBUG
    pcout << "Importer not set. Initializing\n";
#endif
    auto& ghosted_map = x_ghosted_->Map();
    auto& src_map = out.Map();
    importer_ = std::make_shared<Epetra_Import>(ghosted_map, src_map);
  }

  if (exporter_.use_count() == 0) {
#ifdef DEBUG
    pcout << "Exporter not set. Initializing\n";
#endif
    auto& ghosted_map = x_ghosted_->Map();
    auto& src_map = out.Map();
    exporter_ = std::make_shared<Epetra_Export>(ghosted_map, src_map);
  }

  // Import nonlocal elements
  int ier;
  ier = x_ghosted_->Import(in, *importer_.get(), Insert);
  AssertThrow(ier == 0, dealii::ExcTrilinosError(ier));
  // get pointer to x_ghosted_
  const double* data_in = x_ghosted_->Values();
  // reset y_ghosted_
  std::fill(y_ghosted_->Values(), y_ghosted_->Values() + y_ghosted_->MyLength(), 0);

  double* data_out = y_ghosted_->Values();

  unsigned int N = spectral_basis_.n_dofs();
  dealii::UpdateFlags update_flags = dealii::update_values | dealii::update_JxW_values |
                                     dealii::update_quadrature_points |
                                     dealii::update_normal_vectors;

  dealii::QGauss<dimX - 1> quad(2);
  int n_qpoints = quad.size();

  auto& fe = dh_.get_fe();
  dealii::FEFaceValues<2> fe_face_values(fe, quad, update_flags);
  std::vector<size_type> local_dof_indices(fe.dofs_per_face);

  // transform to lagrange coefficients
  unsigned int local_size = in.MyLength();
  const auto& faces_list = this->get_faces_list();
  // rotated Polar coeffs
  Eigen::VectorXd cP_rot(N);
  // Hermite coeffs
  Eigen::VectorXd cH(N);

  for (const auto& mypair : faces_list) {
    const auto& cell = std::get<0>(mypair);
    int face_idx = std::get<1>(mypair);
    id_t bd_ind = cell.face(face_idx)->boundary_id();

    // this is a periodic boundary and thus requires no further computations
    // (since it is built into the enumeration of DoFs already)
    if (periodic_.find(bd_ind) != periodic_.end()) continue;

    auto fentry = flux_workers_.find(bd_ind);
    if (fentry == flux_workers_.end()) {
      AssertThrow(false, dealii::ExcMessage("unknown boundary type"));
    }

    typedef dealii::DoFCellAccessor<dealii::DoFHandler<2>, false> accessor_t;
    typedef dealii::TriaIterator<accessor_t> tria_iterator_t;

    cell.face(face_idx)->get_dof_indices(local_dof_indices);

    fe_face_values.reinit(tria_iterator_t(cell), face_idx);

#ifdef DEBUG
    // checking dof indexing
    std::vector<size_type> ldof_indices(fe.dofs_per_cell);
    cell.get_dof_indices(ldof_indices);
    size_type l0 = cell.face(face_idx)->vertex_dof_index(0, 0);
    size_type l1 = cell.face(face_idx)->vertex_dof_index(1, 0);

    AssertThrow(l0 == ldof_indices[faces_vertex[face_idx][0]],
                dealii::ExcMessage("something with the local dofs is wrong"));
    AssertThrow(l1 == ldof_indices[faces_vertex[face_idx][1]],
                dealii::ExcMessage("something with the local dofs is wrong"));
    AssertThrow(l0 == local_dof_indices[0],
                dealii::ExcMessage("something with local dofs is wrong"));
    AssertThrow(l1 == local_dof_indices[1],
                dealii::ExcMessage("something with local dofs is wrong"));
#endif
    B *= 0;
    for (int i = 0; i < dimX; ++i) {
      int l1 = faces_vertex[face_idx][i];
      for (int j = 0; j < dimX; ++j) {
        int l2 = faces_vertex[face_idx][j];
        for (unsigned int q = 0; q < quad.size(); ++q) {
          B[i][j] += (fe_face_values.shape_value(l1, q) * fe_face_values.shape_value(l2, q) *
                      fe_face_values.JxW(q));
        }
      }
    }

    Assert(std::abs(B[0][1]) > 0, dealii::ExcMessage("something went wrong"));
    Assert(std::abs(B[1][1]) > 0, dealii::ExcMessage("something went wrong"));
    Assert(std::abs(B[0][0]) > 0, dealii::ExcMessage("something went wrong"));

    const double nx = fe_face_values.normal_vector(0)[0];
    const double ny = fe_face_values.normal_vector(0)[1];
    // winkel zwischen y-achse und n
    const double alpha = numbers::PI / 2 - std::atan2(ny, nx);

    typedef dealii::TrilinosWrappers::types::int_type index_t;
    index_t ig1 =
        x_ghosted_->Map().LID(static_cast<index_t>(indexer_.to_global(local_dof_indices[0], 0)));

    Assert(ig1 != -1, dealii::ExcMessage("global index not found in x_ghosted_!"));

    index_t ig2 =
        x_ghosted_->Map().LID(static_cast<index_t>(indexer_.to_global(local_dof_indices[1], 0)));
    Assert(ig2 != -1, dealii::ExcMessage("global index not found in x_ghosted_!"));

    // *** flux ***
    R_.apply(cP_rot.data(), data_in + ig1, alpha);
    ptr_P2H_->to_hermite(cH, cP_rot);
    ptr_H2N_->to_nodal(L1_, cH);
    fentry->second->apply(L1p_, L1_, cell.face(face_idx)->vertex(0));

    R_.apply(cP_rot.data(), data_in + ig2, alpha);
    ptr_P2H_->to_hermite(cH, cP_rot);
    ptr_H2N_->to_nodal(L2_, cH);
    fentry->second->apply(L2p_, L2_, cell.face(face_idx)->vertex(1));

    // *** Assemble into y_ghosted_ ***
    // lfe_idx1-test function => global index ig1, ig1+N
    L_ = B[0][0] * L1p_ + B[0][1] * L2p_;
    ptr_H2N_->to_hermite(cH.data(), L_);               // to_hermite = to_nodal^T
    ptr_P2H_->to_hermite_T(cP_rot, cH);  // to_hermite_T = to_hermite^T
    // rotate back
    R_.apply(ybuf_.data(), cP_rot.data(), -alpha);
    // write to ghosted vector
    Eigen::Map<Eigen::VectorXd> vout1(data_out + ig1, N);
    vout1 += ybuf_;

    // lfe_idx2-test function => global index ig2, ig2+N
    L_ = B[1][0] * L1p_ + B[1][1] * L2p_;
    ptr_H2N_->to_hermite(cH, L_);               // to_hermite = to_nodal^T
    ptr_P2H_->to_hermite_T(cP_rot, cH);  // to_hermite_T = to_hermite^T
    // rotate back
    R_.apply(ybuf_.data(), cP_rot.data(), -alpha);

    // write to ghosted vector
    Eigen::Map<Eigen::VectorXd> vout2(data_out + ig2, N);
    vout2 += ybuf_;
  }

  // scale y_ghosted_
  y_ghosted_->Scale(dt_);
  // *** export y_ghosted_ into global solution vector ***
  ier = out.Export(*y_ghosted_.get(), *exporter_.get(), Epetra_CombineMode::Epetra_AddLocalAlso);
  Assert(ier == 0, dealii::ExcMessage("Export to global solution vector failed."));
}

}  // end namespace boltzmann


/**
 * @file esx.hpp
 * @author Leonardo Arcari (leonardo1.arcari@gmail.com)
 * @version 1.0.0
 * @date 2018-10-28
 *
 * @copyright Copyright (c) 2018 Leonardo Arcari
 *
 * MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#ifndef BOOST_ESX_ALTERNATIVE_ROUTING_HPP
#define BOOST_ESX_ALTERNATIVE_ROUTING_HPP

#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>

#include <arlib/terminators.hpp>
#include <arlib/type_traits.hpp>

#include <arlib/details/esx_impl.hpp>

#include <queue>
#include <unordered_set>
#include <vector>

/**
 * An Alternative-Routing library for Boost.Graph
 */
namespace arlib {
/**
 * An implementation of ESX k-shortest path with limited overlap for
 * Boost::Graph.
 *
 * This implementation refers to the following publication:
 *
 * Theodoros Chondrogiannis, Panagiotis Bouros, Johann Gamper and Ulf Leser,
 * Exact and Approximate Algorithms for Finding k-Shortest Paths with Limited
 * Overlap , In Proc. of the 20th Int. Conf. on Extending Database Technology
 * (EDBT) (2017)
 *
 * @tparam Graph A Boost::VertexAndEdgeListGraph
 * @tparam WeightMap The weight or "length" of each edge in the graph. The
 *         weights must all be non-negative, and the algorithm will throw a
 *         negative_edge exception is one of the edges is negative. The type
 *         WeightMap must be a model of Readable Property Map. The edge
 *         descriptor type of the graph needs to be usable as the key type for
 *         the weight map. The value type for this map must be the same as the
 *         value type of the distance map.
 * @tparam MultiPredecessorMap The multi predecessor map records the edges in
 *         the alternative paths from @p s to @p t. Upon completion of the
 *         algorithm, for any vertex `v` on any alternative path `p`, multi
 *         predecessor map stores the predecessor of node `v` on path `p`. If no
 *         predecessor for a node `v'` is reported, then `v'` is not part of any
 *         alternative path or it is the source node @p s. The type of
 *         `MultiPredecessorMap` must be a model of `Read Property Map`. The
 *         vertex descriptor of the input graph @p G must be usable as a key
 *         type for the multi predecessor map. Whereas the value type must
 *         satisfy the `UnorderedAssociativeContainer` concept, where its key is
 *         an `int`, the index/number of alternative path for which it exists a
 *         predecessor of `v`, and where the value type is a vertex descriptor
 *         of input graph @p G.
 * @tparam Vertex The vertex descriptor.
 * @param G The input graph.
 * @param weight The weight map of @p G.
 * @param predecessors The multi predecessor map of @p G.
 * @param s The source node.
 * @param t The target node.
 * @param k The number of alternative paths to compute.
 * @param theta The similarity threshold.
 * @param algorithm The routing kernel to employ. The default is
 *        routing_kernels::astar
 *
 */
template <typename Graph, typename WeightMap, typename MultiPredecessorMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<Graph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const Graph &G, WeightMap const &weight,
         MultiPredecessorMap &predecessors, Vertex s, Vertex t, int k,
         double theta, routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  details::esx_dispatch(G, weight, predecessors, s, t, k, theta, algorithm,
                        std::forward<Terminator>(terminator));
}

template <typename Graph, typename WeightMap, typename MultiPredecessorMap,
          typename EdgeCentralityMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<Graph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const Graph &G, WeightMap const &weight,
         MultiPredecessorMap &predecessors,
         EdgeCentralityMap const &edge_centrality, Vertex s, Vertex t, int k,
         double theta, routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  details::esx_dispatch(G, weight, predecessors, edge_centrality, s, t, k,
                        theta, algorithm, std::forward<Terminator>(terminator));
}

/**
 * An implementation of `ESX` k-shortest path with limited overlap for
 * `Boost::Graph`.
 *
 * This overload takes an input graph modeling `PropertyGraph` concept having at
 * least one edge property with tag `boost::edge_weight_t`. Moreover it does not
 * require an explicit `WeightMap` parameter, because it is directly gathered
 * from the `PropertyGraph`.
 *
 * @see esx(const Graph &G, WeightMap const &weight,
 *          MultiPredecessorMap &predecessors, Vertex s, Vertex t, int k,
 *          double theta,
 *          routing_kernels algorithm)
 *
 * @tparam PropertyGraph A Boost::PropertyGraph having at least one edge
 *         property with tag boost::edge_weight_t.
 *
 */
template <typename PropertyGraph, typename MultiPredecessorMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<PropertyGraph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const PropertyGraph &G, MultiPredecessorMap &predecessors, Vertex s,
         Vertex t, int k, double theta,
         routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  using namespace boost;
  using Edge = typename graph_traits<PropertyGraph>::edge_descriptor;

  BOOST_CONCEPT_ASSERT(
      (PropertyGraphConcept<PropertyGraph, Edge, edge_weight_t>));

  auto weight = get(edge_weight, G);
  details::esx_dispatch(G, weight, predecessors, s, t, k, theta, algorithm,
                        std::forward<Terminator>(terminator));
}

template <typename PropertyGraph, typename MultiPredecessorMap,
          typename EdgeCentralityMap,
          typename Terminator = arlib::always_continue,
          typename Vertex = vertex_of_t<PropertyGraph>,
          typename = std::enable_if_t<std::is_same_v<
              typename boost::property_traits<MultiPredecessorMap>::key_type,
              Vertex>>>
void esx(const PropertyGraph &G, MultiPredecessorMap &predecessors,
         EdgeCentralityMap const &edge_centrality, Vertex s, Vertex t, int k,
         double theta, routing_kernels algorithm = routing_kernels::astar,
         Terminator &&terminator = Terminator{}) {
  using namespace boost;
  using Edge = typename graph_traits<PropertyGraph>::edge_descriptor;

  BOOST_CONCEPT_ASSERT(
      (PropertyGraphConcept<PropertyGraph, Edge, edge_weight_t>));

  auto weight = get(edge_weight, G);
  esx(G, weight, predecessors, edge_centrality, s, t, k, theta, algorithm,
      std::forward<Terminator>(terminator));
}
} // namespace arlib

#endif


#include <iostream>
#include <boost/compute/core.hpp>
#include <vector>
#include <boost/concept_check.hpp>
#include "../pointsets/Pointset.hpp"
#include "../io/fileIO.hpp"
#include "../io/histogramIO.hpp"

#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/algorithm/max_element.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/math.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/function.hpp>

namespace compute = boost::compute;

const char source2[] = BOOST_COMPUTE_STRINGIZE_SOURCE(__kernel void computeScoreMinDist(__global const unsigned int* permut_lsb,
																				__global const unsigned int* permut_msb,
																				__global const unsigned int* pattern,
																				__global const float* pcf,
																				__global float* min_dist,
                                                                                __global float* score
																				//__global unsigned int* debug
                                                                              )
													{
														uint gid = get_global_id(0);

														unsigned int permut_x_lsb = permut_lsb[2*gid];
														unsigned int permut_y_lsb = permut_lsb[2*gid+1];
														unsigned int permut_x_msb = permut_msb[2*gid];
														unsigned int permut_y_msb = permut_msb[2*gid+1];

														unsigned int permutN_lsb[2];
														unsigned int permutN_msb[2];
														permutN_lsb[0] = permut_x_lsb;
														permutN_lsb[1] = permut_y_lsb;
														permutN_msb[0] = permut_x_msb;
														permutN_msb[1] = permut_y_msb;

														unsigned int permutM_lsb[2];
														unsigned int permutM_msb[2];
														permutM_lsb[0] = 0;
														permutM_lsb[1] = 0;
														permutM_msb[0] = 0;
														permutM_msb[1] = 0;

														uint tilesize = 64;
														uint LOGK=6;
														uint D=2;

														uint x[5][2];
														for(uint d=0; d<D; d++)
														{
															int nb_flags = 2;
															int orig_lvl = tilesize-2 -1;
															for(uint i=1; i<LOGK; i++)
															{
																x[i-1][d] = orig_lvl - (pattern[d]/(tilesize/nb_flags));
																orig_lvl -= nb_flags;
																nb_flags*=2;
															}
														}

														uint xcount[2];
														for(uint d=0; d<D; d++)
															xcount[d] = tilesize-LOGK-2; // bitset indexing: 0==least significant bit

														for(uint i=1; i<tilesize-1; i++) { // start form the second i because xpermuts15bits[0].x = xpermuts15bits[0].y = 0;
															uint ind = tilesize-2-i;

															for(uint d=0; d<D; d++)
															{
																bool isFlag=true;
																for(uint i=0; i<(uint)(LOGK-1); i++)
																	isFlag &= (ind != x[i][d]);

																if( isFlag )
																{
																	if(xcount[d] < 32)
																	{
																		int digit_value = ((permutN_lsb[d] >> xcount[d]) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	else
																	{
																		int digit_value = ((permutN_msb[d] >> (xcount[d]-32)) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	xcount[d]--;
																}
															}
														}

														permut_x_lsb = permutM_lsb[0];
														permut_y_lsb = permutM_lsb[1];
														permut_x_msb = permutM_msb[0];
														permut_y_msb = permutM_msb[1];

														unsigned int permutted_pattern[128];

														int ind;
														unsigned int xpermuts_lsb[2];
														xpermuts_lsb[0] = permut_x_lsb;
														xpermuts_lsb[1] = permut_y_lsb;

														unsigned int xpermuts_msb[2];
														xpermuts_msb[0] = permut_x_msb;
														xpermuts_msb[1] = permut_y_msb;

														uint NBITS = 6;
														uint PERMUTS_NBITS = tilesize-1;

														for(uint ipt=0; ipt<tilesize; ipt++)
														{
															unsigned int xresdigits[2];
															unsigned int thisSobolPt[2];
															for(uint d=0; d<2; d++)
																thisSobolPt[d] = pattern[2*ipt+d];
															xresdigits[0] = 0;
															xresdigits[1] = 0;

															for(uint ilevel=0; ilevel<NBITS; ilevel++) {

																for(uint d=0; d<2; d++)
																{
																	int power=(1 << ilevel);
																	ind = (pattern[2*ipt+d] >> (NBITS-ilevel)) + (power-1);
																	int digit_value_1 = 0;
																	if(PERMUTS_NBITS-ind-1 < 32)
																		digit_value_1 =  (xpermuts_lsb[d] >> (PERMUTS_NBITS-ind-1)) & 1;
																	else
																		digit_value_1 =  (xpermuts_msb[d] >> (PERMUTS_NBITS-ind-1-32)) & 1;
																	int digit_value_2 =  (thisSobolPt[d] >> (NBITS-ilevel-1)) & 1;
																	int digit_value = (digit_value_1 ^ digit_value_2);
																	int digit_pos = NBITS-ilevel-1;
																	unsigned int mask = (unsigned int)((unsigned int)1 << digit_pos);
																	if(digit_value)
																		xresdigits[d] |= mask;
																	else
																		xresdigits[d] &= (unsigned int)~mask;
																}
															}

															for(uint d=0; d<2; d++)
																permutted_pattern[2*ipt+d] = xresdigits[d];
														}

														float mindist = 10000.0;
														for (uint ipts = 0; ipts < tilesize; ipts++) {
															for (uint ipts_to_compare = ipts+1; ipts_to_compare < tilesize; ipts_to_compare++) {

																float dist_sq = 0.0;
																float dx = (float)permutted_pattern[2*ipts]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare]/(float)tilesize;
																dist_sq += dx*dx;
																float dy = (float)permutted_pattern[2*ipts+1]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare+1]/(float)tilesize;
																dist_sq += dy*dy;

																if (dist_sq < mindist) mindist = dist_sq;
															}
														}
														min_dist[gid] = sqrt(mindist);
														score[gid]= min_dist[gid];
													}
                                                    );

const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(__kernel void computeScorePCF(__global const unsigned int* permut_lsb,
																				__global const unsigned int* permut_msb,
																				__global const unsigned int* pattern,
																				__global const float* pcf,
																				__global float* min_dist,
                                                                                __global float* score
																				//__global unsigned int* debug
                                                                              )
													{
														uint gid = get_global_id(0);

														unsigned int permut_x_lsb = permut_lsb[2*gid];
														unsigned int permut_y_lsb = permut_lsb[2*gid+1];
														unsigned int permut_x_msb = permut_msb[2*gid];
														unsigned int permut_y_msb = permut_msb[2*gid+1];

														unsigned int permutN_lsb[2];
														unsigned int permutN_msb[2];
														permutN_lsb[0] = permut_x_lsb;
														permutN_lsb[1] = permut_y_lsb;
														permutN_msb[0] = permut_x_msb;
														permutN_msb[1] = permut_y_msb;

														unsigned int permutM_lsb[2];
														unsigned int permutM_msb[2];
														permutM_lsb[0] = 0;
														permutM_lsb[1] = 0;
														permutM_msb[0] = 0;
														permutM_msb[1] = 0;

														uint tilesize = 64;

														uint LOGK=6;
														uint D=2;

														uint x[5][2];
														for(uint d=0; d<D; d++)
														{
															int nb_flags = 2;
															int orig_lvl = tilesize-2 -1;
															for(uint i=1; i<LOGK; i++)
															{
																x[i-1][d] = orig_lvl - (pattern[d]/(tilesize/nb_flags));
																orig_lvl -= nb_flags;
																nb_flags*=2;
															}
														}

														uint xcount[2];
														for(uint d=0; d<D; d++)
															xcount[d] = tilesize-LOGK-2; // bitset indexing: 0==least significant bit

														for(uint i=1; i<tilesize-1; i++) { // start form the second i because xpermuts15bits[0].x = xpermuts15bits[0].y = 0;
															uint ind = tilesize-2-i;

															for(uint d=0; d<D; d++)
															{
																bool isFlag=true;
																for(uint i=0; i<(uint)(LOGK-1); i++)
																	isFlag &= (ind != x[i][d]);

																if( isFlag )
																{
																	if(xcount[d] < 32)
																	{
																		int digit_value = ((permutN_lsb[d] >> xcount[d]) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	else
																	{
																		int digit_value = ((permutN_msb[d] >> (xcount[d]-32)) & 1);
																		if(ind < 32)
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << ind);
																			if(digit_value)
																				permutM_lsb[d] |= mask;
																			else
																				permutM_lsb[d] &= (unsigned int)~mask;
																		}
																		else
																		{
																			unsigned int mask = (unsigned int)((unsigned int)1 << (ind-32));
																			if(digit_value)
																				permutM_msb[d] |= mask;
																			else
																				permutM_msb[d] &= (unsigned int)~mask;
																		}
																	}
																	xcount[d]--;
																}
															}
														}

														permut_x_lsb = permutM_lsb[0];
														permut_y_lsb = permutM_lsb[1];
														permut_x_msb = permutM_msb[0];
														permut_y_msb = permutM_msb[1];

														unsigned int permutted_pattern[128];

														int ind;
														unsigned int xpermuts_lsb[2];
														xpermuts_lsb[0] = permut_x_lsb;
														xpermuts_lsb[1] = permut_y_lsb;

														unsigned int xpermuts_msb[2];
														xpermuts_msb[0] = permut_x_msb;
														xpermuts_msb[1] = permut_y_msb;

														uint NBITS = 6;
														uint PERMUTS_NBITS = tilesize-1;

														for(uint ipt=0; ipt<tilesize; ipt++)
														{
															unsigned int xresdigits[2];
															unsigned int thisSobolPt[2];
															for(uint d=0; d<2; d++)
																thisSobolPt[d] = pattern[2*ipt+d];
															xresdigits[0] = 0;
															xresdigits[1] = 0;

															for(uint ilevel=0; ilevel<NBITS; ilevel++) {

																for(uint d=0; d<2; d++)
																{
																	int power=(1 << ilevel);
																	ind = (pattern[2*ipt+d] >> (NBITS-ilevel)) + (power-1);
																	int digit_value_1 = 0;
																	if(PERMUTS_NBITS-ind-1 < 32)
																		digit_value_1 =  (xpermuts_lsb[d] >> (PERMUTS_NBITS-ind-1)) & 1;
																	else
																		digit_value_1 =  (xpermuts_msb[d] >> (PERMUTS_NBITS-ind-1-32)) & 1;
																	int digit_value_2 =  (thisSobolPt[d] >> (NBITS-ilevel-1)) & 1;
																	int digit_value = (digit_value_1 ^ digit_value_2);
																	int digit_pos = NBITS-ilevel-1;
																	unsigned int mask = (unsigned int)((unsigned int)1 << digit_pos);
																	if(digit_value)
																		xresdigits[d] |= mask;
																	else
																		xresdigits[d] &= (unsigned int)~mask;
																}
															}

															for(uint d=0; d<2; d++)
																permutted_pattern[2*ipt+d] = xresdigits[d];
														}

														float mindist = 10000.0;
														for (uint ipts = 0; ipts < tilesize; ipts++) {
															for (uint ipts_to_compare = ipts+1; ipts_to_compare < tilesize; ipts_to_compare++) {

																float dist_sq = 0.0;
																float dx = (float)permutted_pattern[2*ipts]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare]/(float)tilesize;
																dist_sq += dx*dx;
																float dy = (float)permutted_pattern[2*ipts+1]/(float)tilesize - (float)permutted_pattern[2*ipts_to_compare+1]/(float)tilesize;
																dist_sq += dy*dy;

																if (dist_sq < mindist) mindist = dist_sq;
															}
														}
														min_dist[gid] = sqrt(mindist);

														if(min_dist[gid] < 0.07)
														{
															score[gid] = 0.01;
														}
														else
														{
															float norm_dist = 2.0 * sqrt( 1.0 / (2.0*sqrt(3.0)*tilesize) );
															float smoothing = 0.1 * norm_dist;
															float ra = 1.5*smoothing;
															float rb = 2.5*norm_dist;

															float ani_smoothing = 0.01;
															float aa = 0;
															float ab = 2*M_PI;

															uint nbbins = 20;
															float pcf_pattern[20];
															float anisotropy_pattern[20];
															for(uint pcfid = 0; pcfid < nbbins; ++pcfid)
															{
																pcf_pattern[pcfid] = 0;
																anisotropy_pattern[pcfid] = 0;
															}

															float bin_size = (rb-ra)/(float)nbbins;
															uint covering = ((2.0*smoothing)/bin_size)+1;
															if(covering > nbbins)
																covering = nbbins;

															float ani_bin_size = (ab-aa)/(float)nbbins;
															uint ani_covering = ((2.0*ani_smoothing)/ani_bin_size)+1;
															if(ani_covering > nbbins)
																ani_covering = nbbins;

															for(uint i=0; i<tilesize; i++)
															{
																for(uint j=(i+1); j<tilesize; j++)
																{
																	float dist_sq = 0;
																	float dx = (float)permutted_pattern[2*i]/(float)tilesize - (float)permutted_pattern[2*j]/(float)tilesize;
																	dx*=dx;
																	dist_sq += dx;
																	float dy = (float)permutted_pattern[2*i+1]/(float)tilesize - (float)permutted_pattern[2*j+1]/(float)tilesize;
																	dy*=dy;
																	dist_sq += dy;
																	float dxy = dist_sq;
																	dx=sqrt(dx);
																	dy=sqrt(dy);

																	float vector_angle = atan(dy/dx);
																	int ani_init_bin = vector_angle/bin_size;
																	anisotropy_pattern[ani_init_bin] += 1;

																	int init_bin = sqrt(dxy)/bin_size;
																	int ida = init_bin - covering;
																	int idb = init_bin + covering;
																	if(ida < 0) ida = 0;
																	if(idb > nbbins) idb = nbbins;
																	for(uint pcfid = ida; pcfid < idb; ++pcfid)
																	{
																		float r = ra + pcfid*(rb - ra) / (float) nbbins;
																		float dist = r - sqrt(dxy);
																		float val = ( 1.0/(sqrt(2*M_PI)*smoothing) ) * exp( -0.5*(dist*dist)/(smoothing*smoothing) );
																		pcf_pattern[pcfid] += 2*val;
																	}
																}
															}

															for(uint pcfid = 0; pcfid < nbbins; ++pcfid)
															{
																float r = ra + pcfid*(rb - ra) / (float) nbbins;

																float cov = 1.0 - ((2.0*r)/M_PI)*2.0 + ((r*r)/M_PI);
																float factor = 2.0*M_PI*r*cov;
																factor = 1.0/factor;

																pcf_pattern[pcfid] *= factor;
																pcf_pattern[pcfid] /= (tilesize*(tilesize-1));
																anisotropy_pattern[pcfid] /= 20.0;
															}

															float linf = 0;
															float ani_linf = 0;
															for(uint i=0; i<nbbins; i++)
															{
																if(pcf_pattern[i] != 0 && fabs(pcf[i] - pcf_pattern[i]) > linf )
																	linf = fabs(pcf[i] - pcf_pattern[i]);

																if(anisotropy_pattern[i] != 0 && fabs(1 - anisotropy_pattern[i]) > ani_linf )
																	ani_linf = fabs(1 - anisotropy_pattern[i]);
															}

															score[gid] = 10 -linf -ani_linf;
														}
													}
                                                    );

bool permutFromNbitsToMbits(uint N, uint M, uint fixed_point[2], unsigned long long int permutN_x, unsigned long long int permutN_y,
							unsigned long long int& permutM_x, unsigned long long int& permutM_y)
{
	int tilesize = M+1;
	int LOGK=log2(tilesize);
	uint D=2;

	std::vector< std::array<uint, 2> > x;
	x.resize(LOGK-1);
	for(uint d=0; d<D; d++)
	{
		int nb_flags = 2;
		int orig_lvl = tilesize-2 -1;
		for(int i=1; i<LOGK; i++)
		{
			x[i-1][d] = orig_lvl - (fixed_point[d]/(tilesize/nb_flags));
			orig_lvl -= nb_flags;
			nb_flags*=2;
		}
	}

	std::bitset<64> xpermutsNbits[2];
	xpermutsNbits[0] = permutN_x;
	xpermutsNbits[1] = permutN_y;

	std::bitset<64> xpermutsMbits[2];
	int xcount[2];
	for(uint d=0; d<D; d++)
		xcount[d] = tilesize-LOGK-2; // bitset indexing: 0==least significant bit
	for(uint i=1; i<(uint)(tilesize-1); i++) { // start form the second i because xpermuts15bits[0].x = xpermuts15bits[0].y = 0;
		uint ind = tilesize-2-i;

		for(uint d=0; d<D; d++)
		{
			bool isFlag=true;
			for(int i=0; i<LOGK-1; i++)
				isFlag &= (ind != x[i][d]);

			if( isFlag ) {
				xpermutsMbits[d][ind] = xpermutsNbits[d][xcount[d]];
				xcount[d]--;
			}
		}
	}

	permutM_x = xpermutsMbits[0].to_ullong();
	permutM_y = xpermutsMbits[1].to_ullong();

	return true;
}

/*void computeScoreCPU(const unsigned int* permut_lsb,
						const unsigned int* permut_msb,
						const unsigned int* pattern,
						const float* pcf,
						float* min_dist,
                        float* score, uint gid)
{
	//Copy OpenCL Kernel Here
}*/

bool optimizeGPU(utk::Pointset<2, unsigned int, utk::Point<2, unsigned int> > pts, utk::Histogram1dd pcf_data, utk::Vector<2, unsigned long long int>& resulting_permut)
{
	assert(pts.size() == 64);

	srand48(time(NULL));
	float max_score= 0;
	float max_dist = 0;
	int max_id=0;

	int t = time(NULL);

	//get default device and setup context
	compute::device device = compute::system::default_device();
	compute::context context(device);
	compute::command_queue queue(context, device);

	int nbbins = 20;
	assert(pcf_data.size() == (uint)nbbins);
	std::vector<float> local_pcf;
	local_pcf.resize(nbbins);
	for(int i=0; i<nbbins; i++)
		local_pcf[i] = pcf_data[i].second;

	std::cout << "Local PCF Vector OK" << std::endl;

	//Send PCF to the device
	compute::vector<float> device_pcf(local_pcf.size(), context);
	compute::copy(local_pcf.begin(), local_pcf.end(), device_pcf.begin(), queue);
	std::cout << "Device PCF Vector OK" << std::endl;

	std::vector<unsigned int> local_pattern(pts.size()*2, 0);
	for(uint i=0; i<pts.size(); i++)
	{
		local_pattern[2*i] = pts[i].pos()[0];
		local_pattern[2*i+1] = pts[i].pos()[1];
	}
	std::cout << "Local Pattern Vector OK" << std::endl;

	//Send pattern to the device
	compute::vector<unsigned int> device_pattern(pts.size()*2, context);
	compute::copy(local_pattern.begin(), local_pattern.end(), device_pattern.begin(), queue);
	std::cout << "Device Pattern Vector OK" << std::endl;

	uint tilesize=64;
	uint N, M;
	M = tilesize-1;
	N = M - log2(tilesize);
	uint zero_pt[2]; zero_pt[0] = local_pattern[0]; zero_pt[1] = local_pattern[1];
	int nbfreedigits = 11;
	int npermut = (1 << nbfreedigits);

	int nbbatches=2048;
	int batch_size = (npermut*npermut)/nbbatches;

	//Create result vector
	compute::vector<float> device_mindist(batch_size, context);
	std::fill(device_mindist.begin(), device_mindist.end(), 0);
	std::cout << "Device MinDist Vector OK" << std::endl;

	compute::vector<float> device_score(batch_size, context);
	std::fill(device_score.begin(), device_score.end(), 0);
	std::cout << "Device Result Vector OK" << std::endl;

	compute::vector<unsigned int> device_permutations_lsb(batch_size*2, context);
	compute::vector<unsigned int> device_permutations_msb(batch_size*2, context);
	std::vector<float> local_mindist(batch_size);
	std::vector<float> local_score(batch_size);

	std::cout << "Building Kernel ..." << std::endl;

	//Create and build the kernels

	//Create and build the kernels
	compute::program m_program;
	m_program = compute::program::create_with_source(source, context);
	try
	{
		m_program.build();
	}
	catch(boost::compute::opencl_error &e)
	{
		std::cout << m_program.build_log() << std::endl;
	}
	compute::kernel score_kernel;
	score_kernel = m_program.create_kernel("computeScorePCF");
	score_kernel.set_arg(0, device_permutations_lsb.get_buffer());
	score_kernel.set_arg(1, device_permutations_msb.get_buffer());
	score_kernel.set_arg(2, device_pattern.get_buffer());
	score_kernel.set_arg(3, device_pcf.get_buffer());
	score_kernel.set_arg(4, device_mindist.get_buffer());
	score_kernel.set_arg(5, device_score.get_buffer());
	std::cout << "OK" << std::endl;

	std::cout << "Optimizing first " << nbfreedigits << " digits ..." << std::endl;

	std::vector<unsigned long long int> local_permutations_full(npermut*npermut*2, 0);
	for(int i=0; i<npermut; i++)
	{
		for(int j=0; j<npermut; j++)
		{
			local_permutations_full[2*(i*npermut+j)] = (unsigned long long int)((unsigned long long int)i << (unsigned long long int)(N-nbfreedigits));
			local_permutations_full[2*(i*npermut+j)+1] = (unsigned long long int)((unsigned long long int)j << (unsigned long long int)(N-nbfreedigits));
		}
	}
	std::vector<unsigned int> local_permutations_lsb(npermut*npermut*2, 0);
	std::vector<unsigned int> local_permutations_msb(npermut*npermut*2, 0);
	for(int i=0; i<npermut; i++)
	for(int j=0; j<npermut; j++)
	{
		local_permutations_lsb[2*(i*npermut+j)] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)]) & (unsigned long long int)0xffffff);
		local_permutations_lsb[2*(i*npermut+j)+1] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)+1]) & (unsigned long long int)0xffffff);

		local_permutations_msb[2*(i*npermut+j)] = ((local_permutations_full[2*(i*npermut+j)]) >> 32);
		local_permutations_msb[2*(i*npermut+j)+1] = ((local_permutations_full[2*(i*npermut+j)+1]) >> 32);
	}
	std::cout << "Local Permut Vector OK" << std::endl;

	for(int batch=0; batch<nbbatches; batch++)
	{
		compute::copy(local_permutations_lsb.begin()+(2*batch*batch_size), local_permutations_lsb.begin()+(2*(batch+1)*batch_size), device_permutations_lsb.begin(), queue);
		compute::copy(local_permutations_msb.begin()+(2*batch*batch_size), local_permutations_msb.begin()+(2*(batch+1)*batch_size), device_permutations_msb.begin(), queue);

		//Go Go go !
		//std::cout << batch << "/" << nbbatches << "\r" << std::flush;
		queue.enqueue_1d_range_kernel(score_kernel, 0, batch_size, 0);

		compute::copy(device_mindist.begin(), device_mindist.end(), local_mindist.begin(), queue);
		compute::copy(device_score.begin(), device_score.end(), local_score.begin(), queue);

		for(int i=0; i<batch_size; i++)
		{
			if( max_score < local_score[i])
			{
				max_dist = local_mindist[i];
				max_id = batch*batch_size+i;
				max_score = local_score[i];
			}
		}
	}

	/*unsigned long long int final_perm_x, final_perm_y;
	permutFromNbitsToMbits(N, M, zero_pt, local_permutations_full[2*max_id], local_permutations_full[2*max_id+1], final_perm_x, final_perm_y);
	std::cout << "Done [" << time(NULL) - t << "s]" << std::endl;
	std::cout<< "[DEVICE] Current Best Permut " << final_perm_x << "," << final_perm_y << " (score " << max_score << ")" << "(dist " << max_dist << ")" << std::endl;*/

	std::cout << "Optimizing last " << (N-nbfreedigits) << " digits ..." << std::endl;
	for(int i=0; i<npermut; i++)
	{
		for(int j=0; j<npermut; j++)
		{
			unsigned long long int randval = (unsigned long long int)(drand48()*((unsigned long long int)1 << (unsigned long long int)(N-nbfreedigits)));
			local_permutations_full[2*(i*npermut+j)] = local_permutations_full[2*max_id] + randval;
			randval = drand48()*(1 << (N-nbfreedigits));
			local_permutations_full[2*(i*npermut+j)+1] = local_permutations_full[2*max_id+1] + randval;
		}
	}
	for(int i=0; i<npermut; i++)
	for(int j=0; j<npermut; j++)
	{
		local_permutations_lsb[2*(i*npermut+j)] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)]) & (unsigned long long int)0xffffff);
		local_permutations_lsb[2*(i*npermut+j)+1] = (unsigned long long int)((local_permutations_full[2*(i*npermut+j)+1]) & (unsigned long long int)0xffffff);

		local_permutations_msb[2*(i*npermut+j)] = ((local_permutations_full[2*(i*npermut+j)]) >> 32);
		local_permutations_msb[2*(i*npermut+j)+1] = ((local_permutations_full[2*(i*npermut+j)+1]) >> 32);
	}
	std::cout << "Local Permut Vector OK" << std::endl;

	for(int batch=0; batch<nbbatches; batch++)
	{
		compute::copy(local_permutations_lsb.begin()+(2*batch*batch_size), local_permutations_lsb.begin()+(2*(batch+1)*batch_size), device_permutations_lsb.begin(), queue);
		compute::copy(local_permutations_msb.begin()+(2*batch*batch_size), local_permutations_msb.begin()+(2*(batch+1)*batch_size), device_permutations_msb.begin(), queue);

		//Go Go go !
		//std::cout << batch << "/" << nbbatches << "\r" << std::flush;
		queue.enqueue_1d_range_kernel(score_kernel, 0, batch_size, 0);
		compute::copy(device_mindist.begin(), device_mindist.end(), local_mindist.begin(), queue);
		compute::copy(device_score.begin(), device_score.end(), local_score.begin(), queue);

		for(int i=0; i<batch_size; i++)
		{
			//computeScoreCPU(&local_permutations_lsb[2*batch*batch_size], &local_permutations_msb[2*batch*batch_size], &local_pattern[0], &local_pcf[0], &local_mindist[0], &local_score[0], i);
			//exit(1);
			if( max_score < local_score[i])
			{
				max_dist = local_mindist[i];
				max_id = batch*batch_size+i;
				max_score = local_score[i];
			}
		}
	}

	resulting_permut[0] = 0; resulting_permut[1] = 0;
	permutFromNbitsToMbits(N, M, zero_pt, local_permutations_full[2*max_id], local_permutations_full[2*max_id+1], resulting_permut[0], resulting_permut[1]);
	std::cout << "Done [" << time(NULL) - t << "s]" << std::endl;
	std::cout<< "[DEVICE] Best Permut " << resulting_permut[0] << "," << resulting_permut[1] << " (score " << max_score << ")" << "(dist " << max_dist << ")" << std::endl;

	return 0;
}