#include <ros/package.h>
#include <signal.h>
#include <boost/filesystem.hpp>
#include <chrono>
#include <iostream>
#include <memory>
#include <vector>
#include <ctime>
#include <ratio>
#include "kacanopen/core/canopen_error.h"
#include "kacanopen/core/core.h"
#include "kacanopen/core/logger.h"
#include "kacanopen/master/device.h"
#include "kacanopen/master/master.h"
#include "kacanopen/tools/device_rpdo.h"
#include "kacanopen/tools/device_tpdo.h"
#include "kacanopen/core/logger.h"
#include "kacanopen/ros_bridge/bridge.h"
#include "kacanopen/ros_bridge/entry_publisher.h"
#include "kacanopen/ros_bridge/entry_subscriber.h"
#include "kacanopen/ros_bridge/joint_state_publisher.h"
#include "kacanopen/ros_bridge/joint_state_subscriber.h"

#include "hexa_package/drivesFeedBack.h"
#include "hexa_package/drivesAction.h"
#include "hexa_package/sensor.h"
#include "hexa_package/setting.h"

#include <chrono>
#include <memory>
#include <thread>
#include <string>

/* For setting the process's priority (setpriority) */
#include <sys/resource.h>
/* For pid_t and getpid() */
#include <unistd.h>
#include <sys/types.h>
/* For locking the program in RAM (mlockall) to prevent swapping */
#include <sys/mman.h>
/* clock_gettime, struct timespec, etc. */
#include <time.h>

using namespace std;

void control();

static volatile int keepRunning = 1;

void intHandler(int dummy) {
    (void) dummy;
    keepRunning = 0;

    printf("trying to stop process %d\n", getpid());
    std::this_thread::sleep_for(std::chrono::microseconds(1000));
    /*do something*/
    printf("killing process %d\n", getpid());
    ros::shutdown();

    //TODO check device available and then set current
    // Currentleft = 0;
    // Currentright = 0;
    // device_1->set_entry("current_mode_setting_value", Currentleft, kaco::WriteAccessMethod::pdo);
    // device->set_entry("current_mode_setting_value", Currentright, kaco::WriteAccessMethod::pdo);

    exit(0);
}

bool printDeviceInfo(std::shared_ptr <kaco::Device> device) {
    try {
        std::string display_device_type;
        auto device_type =
                device->get_entry(0x1000, 0x0, kaco::ReadAccessMethod::sdo);
        if (131474 == static_cast<uint32_t>(device_type))
            display_device_type = "DS402";  // 131474 is the type number for DS-402;
        auto device_name =
                device->get_entry(0x1008, 0x0, kaco::ReadAccessMethod::sdo);
        auto vendor_id =
                device->get_entry(0x1018, 0x01, kaco::ReadAccessMethod::sdo);
        auto product_id =
                device->get_entry(0x1018, 0x02, kaco::ReadAccessMethod::sdo);
        auto revision =
                device->get_entry(0x1018, 0x03, kaco::ReadAccessMethod::sdo);

        auto serial_no =
                device->get_entry(0x1018, 0x04, kaco::ReadAccessMethod::sdo);

        std::cout << "" << std::endl;
        std::cout << "" << std::endl;
        std::cout << "*************************************************************"
                  << std::endl;
        std::cout << "*************************************************************"
                  << std::endl;
        std::cout << "* Device Name found as '" << device_name << "'" << std::endl;
        std::cout << "* Device Type found as CiA-" << display_device_type << "'"
                  << std::endl;
        std::cout << "* Vendor ID=" << vendor_id << std::endl;
        std::cout << "* Product ID=" << product_id << std::endl;
        std::cout << "* Serial Number=" << serial_no << std::endl;
        std::cout << "* Revision Number=" << revision << std::endl;
        std::cout << "*************************************************************"
                  << std::endl;
        std::cout << "*************************************************************"
                  << std::endl;
        std::cout << "" << std::endl;
        std::cout << "" << std::endl;
        return true;
    } catch (...) {
        std::cout << "Exception occured while acquiring device information.!"
                  << std::endl;
        return false;
    }
}

void initializeDevice(std::shared_ptr <kaco::Device> device,
                      uint16_t heartbeat_interval, uint8_t node_id) {
    // set the our desired heartbeat_interval time
    device->set_entry(0x1017, 0x0, heartbeat_interval,
                      kaco::WriteAccessMethod::sdo);

    // Mater side Periodic Tranmit pdo1 value initialization
    device->set_entry("current_mode_setting_value", static_cast<int16_t>(0x0), kaco::WriteAccessMethod::sdo);

    device->set_entry("controlword", static_cast<uint16_t>(0x0006),
                      kaco::WriteAccessMethod::sdo);

    // Master side tpdo1 mapping
    device->add_transmit_pdo_mapping(
            0x200 + node_id, {{"current_mode_setting_value", 0},
                              {"controlword",                2}},
            kaco::TransmissionType::ON_CHANGE,
            std::chrono::milliseconds(5)); //kaco::TransmissionType::PERIODIC - ON_CHANGE

    // Master side RPDO mapping starts here; This must be in line with device
    // side TPDOs

    // Master side rpdo1 mapping

    device->add_receive_pdo_mapping(0x180 + node_id, "position_actual_value",
                                    0);                                 // 32bit

    device->add_receive_pdo_mapping(0x180 + node_id, "statusword", 4);  // 16bit

    device->add_receive_pdo_mapping(0x180 + node_id, "current_actual_value",
                                    6);                                 // 16bit

    // Master side RPDO mapping ends here

    /***************** TPDO MAPPING in DEVICE *****************/
    /// Device side TPDO mapping starts here. This must be in line with the
    /// master side RPDOs.

    // Device side tpdo1 mapping entries and mapping
    std::vector <uint32_t> tpdo1_entries_to_be_mapped = {0x60640020, 0x60410010, 0x60780010};

    map_tpdo_in_device(TPDO_1, tpdo1_entries_to_be_mapped, 255, device);

    // Device side tpdo2 mapping entries and mapping
    std::vector <uint32_t> tpdo2_entries_to_be_mapped = {0x60640020, 0x60410010, 0x60780010};

    map_tpdo_in_device(TPDO_2, tpdo2_entries_to_be_mapped, 255, device);

    /// Device side TPDO mapping ends here;

    /***************** RPDO MAPPING in DEVICE *****************/
    /// Device side RPDO mapping starts here.This must be in line with the
    /// master side TPDOs.
    std::vector <uint32_t> rpdo1_entries_to_be_mapped = {
            0x20300010, 0x60400010,
    };
    map_rpdo_in_device(RPDO_1, rpdo1_entries_to_be_mapped, 255, device);
    /// Device side RPDO mapping ends here

    // Try to clear all possible errors in the CANOpen device
    device->set_entry("controlword", static_cast<uint16_t>(0x0080),
                      kaco::WriteAccessMethod::sdo);

}

using namespace std::chrono;

// Create device pointer
std::shared_ptr <kaco::Device> device;
std::shared_ptr <kaco::Device> device_1;

volatile bool found_node = false;
volatile bool device_connected = false;
std::mutex device_mutex;
volatile bool found_node_1 = false;
volatile bool device_connected_1 = false;
std::mutex device_mutex_1;

bool messageRecievedFromController = false;

high_resolution_clock::time_point lastTime = high_resolution_clock::now();

// void chatterCallback0(const std_msgs::Int16::ConstPtr& msg){
//   duration<double, std::milli> time_take_to_this = high_resolution_clock::now() - lastTime;
//   double currentTime = time_take_to_this.count() ;
//   lastTime = high_resolution_clock::now();
//   messageRecievedFromController = true;
//   if((device_connected){
//     cout << msg->data << "\t" << currentTime << "\n" ;
//     device->set_entry("current_mode_setting_value", msg->data,
//                             kaco::WriteAccessMethod::pdo);
//     // device->set_entry("controlword", static_cast<uint16_t>(0x000F),
//     //                         kaco::WriteAccessMethod::pdo);
//   }
// }

// void chatterCallback1(const std_msgs::Int16::ConstPtr& msg){
//   messageRecievedFromController = true;
//   if((device_connected_1){
//     device_1->set_entry("current_mode_setting_value", msg->data,
//                             kaco::WriteAccessMethod::pdo);
//   }
// }

// void actionSubscriberCallback(const hexa_package::drivesAction::ConstPtr& msg){
//   duration<double, std::milli> time_take_to_this = high_resolution_clock::now() - lastTime;
//   double currentTime = time_take_to_this.count() ;
//   lastTime = high_resolution_clock::now();
//   messageRecievedFromController = true;
//   if((currentTime > 6 || currentTime < 1)
//   cout << currentTime << "\n" ;
//   if((device_connected && device_connected_1) {
//     // cout << msg->current0;
//     device->set_entry("current_mode_setting_value", msg->current0, kaco::WriteAccessMethod::pdo);
//     device_1->set_entry("current_mode_setting_value", msg->current1, kaco::WriteAccessMethod::pdo);
//   }

// }

#define MAX_SAFE_STACK (8 * 1024)

void stack_prefault(void) {
    unsigned char dummy[MAX_SAFE_STACK];
    memset(dummy, 0, MAX_SAFE_STACK);
}

int16_t Currentleft = 0;
int16_t Currentright = 0;
int32_t LoadcellRightHip = 0;
int32_t LoadcellLeftHip = 0;

int16_t actual_curr_0 = 0;
int16_t actual_curr_1 = 0;
int32_t actual_position_0 = 0;
int32_t actual_position_1 = 0;
int32_t P_RH = 50;//30;
int32_t P_LH = 50;//30;
long double Force_RH = 0;
long double Force_LH = 0;
int32_t timer_right = 100;
int32_t timer_left = 100;
int32_t Assist_Right = 2000;
int32_t Assist_Left = 2000;
int32_t delay_time = 0;
int32_t Assist_Delay = 0;
string control_algorithm = "assist_as_needed";
string control_algorithm_right = "zero_impedance";
string control_algorithm_left = "zero_impedance";

void getSensorData(const hexa_package::sensor::ConstPtr &msg) {
    //cout  << endl << msg->loadcell1 << endl;

    duration<double, std::milli> time_take_to_this = high_resolution_clock::now() - lastTime;
    double currentTime = time_take_to_this.count();
    lastTime = high_resolution_clock::now();
    //Todo print time for debug
    // cout << currentTime << "\n" ;
    LoadcellRightHip = msg->loadcell1;
    LoadcellLeftHip = msg->loadcell0;
}

void getSettingData(const hexa_package::setting::ConstPtr &msg) {

    control_algorithm = msg->assist_algorithm;
    control_algorithm_right = msg->right_leg_algorithm;
    control_algorithm_left = msg->left_leg_algorithm;
    Assist_Right = msg->right_assistive_force * 30;
    Assist_Left = msg->left_assistive_force * 30;
    timer_right = msg->right_assistive_time;
    timer_left = msg->left_assistive_time;

    // In time based assist always one of the legs is zero impedance mode
    if (control_algorithm == "time_based_assist") {
        if (control_algorithm_left == "assist_as_needed") {
            delay_time = msg->assist_delay_left;
            Assist_Delay = msg->left_assistive_force * 30;
        }
        if (control_algorithm_right == "assist_as_needed") {
            delay_time = msg->assist_delay_right;
            Assist_Delay = msg->right_assistive_force * 30;
        }
    }
    cout << "GUI Settings" << endl << msg->assist_algorithm << endl << msg->left_leg_algorithm << endl
         << msg->right_leg_algorithm << endl << Assist_Right << endl << Assist_Left
         << endl << timer_right << endl << timer_left << endl << delay_time << endl;

}

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

    cpu_set_t set;
    //CPU_ZERO(&set);
    //CPU_SET(3, &set);

    //if (sched_setaffinity(0, sizeof(set), &set))
    //{
    //       printf("Setting CPU affinity failed!\n");
    //      return -1;
    //}
    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    printf("Using priority %i.\n", param.sched_priority);
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
        perror("sched_setscheduler failed\n");
    }

    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        printf("mlockall failed\n");
        return -1;
    }

    stack_prefault();

    // Create bridge
    ros::init(argc, argv, "hexa_node4");
    kaco::Bridge bridge;
    ros::NodeHandle n;

    //ros::AsyncSpinner spinner(4); // Use 2 threads
    //spinner.start();
    //ros::waitForShutdown();

    ros::Rate loop_rate(200);

    // Signal handleing
    signal(SIGINT, intHandler);

    // Define Publisher
    // ros::Publisher chatter_pub0 = n.advertise<std_msgs::Int16>("actualCurrent0", 1000);
    // ros::Publisher chatter_pub1 = n.advertise<std_msgs::Int16>("actualCurrent1", 1000);
    // ros::Publisher postion_message_publisher_0 = n.advertise<std_msgs::Int32>("actualPosition0", 1000);
    // ros::Publisher postion_message_publisher_1 = n.advertise<std_msgs::Int32>("actualPosition1", 1000);
    ros::Publisher drivesFeedBack_publisher = n.advertise<hexa_package::drivesFeedBack>("drivesFeedBack", 10000);

    // ----------- //
    // Preferences //
    // ----------- //

    // The node ID of the slave we want to communicate with.
    const uint8_t node_id_0 = 7;
    const uint8_t node_id_1 = 1;

    const std::string busname = "can0";

    const std::string baudrate = "1M";

    const uint16_t heartbeat_interval = 1000;

    // Set the heartbeat time out, after which the system should detect slave
    // disconnection; values can be "250", "500", "1000" and "2000" millisecond.
    // Temporary disabled the timeout parameters; A gloabl 2 second time is now
    // used in device_alive and device_dead callback
    // const uint16_t heartbeat_timeout = heartbeat_interval * 3;

    // -------------- //
    // Initialization //
    // -------------- //

    // Create core.
    kaco::Core core;

    std::cout << "Starting Core (connect to the driver and start the receiver "
                 "thread)...Hi"
              << std::endl;
    if (!core.start(busname, baudrate)) {
        std::cout << "Starting core failed." << std::endl;
        return EXIT_FAILURE;
    }

    // This will be set to true by the callback below.

    std::cout << "Registering a callback which is called when a device is "
                 "detected via NMT..."
              << std::endl;
    // make sure that the node is reset and goes back to NMT preoperational
    core.nmt.send_nmt_message(node_id_0, kaco::NMT::Command::reset_node);
    core.nmt.send_nmt_message(node_id_1, kaco::NMT::Command::reset_node);
    core.nmt.register_device_alive_callback([&](const uint8_t new_node_id) {
//        cout << "node found \n";
        // Check if this is the node we are looking for.
        if (new_node_id == node_id_0) {
            // lock
            if (!found_node) {
                found_node = true;
                // Lock device mutex
                //TODO check why?
                std::lock_guard <std::mutex> lock(device_mutex);
                try {
                    // Initialize the device
                    device.reset(new kaco::Device(core, node_id_0));
                    std::string path = ros::package::getPath("kacanopen");

                    boost::filesystem::path full_path = "/home/pi/hexa_workspace/src/hexa_package/edsFiles/epos2_347717.eds";

                    device->load_dictionary_from_eds(full_path.string());
                    //std::cout << "Printing Device Object Dictionary" << std::endl;
                    //device->print_dictionary(); // this print all eds dictionary
                    core.nmt.send_nmt_message(node_id_0,
                                              kaco::NMT::Command::enter_preoperational);

                    initializeDevice(device, heartbeat_interval, node_id_0);
                    int8_t set_mode_of_operation = -3;

                    std::this_thread::sleep_for(std::chrono::microseconds(5000));

                    device->set_entry(0x6060, 0x00, set_mode_of_operation,
                                      kaco::WriteAccessMethod::sdo);

                    device->start();
                    //printDeviceInfo(device);
                    device_connected = true;

                } catch (const std::exception &exc) {
                    std::cout << "Exception in device alive!" << std::endl;
                    std::cerr << exc.what();
                    found_node = false;
                    device_connected = false;
                }
            }
        }
        if (new_node_id == node_id_1) {
            // lock
            if (!found_node_1) {
                found_node_1 = true;
                // Lock device mutex
                std::lock_guard <std::mutex> lock(device_mutex_1);
                try {
                    // Initialize the device
                    device_1.reset(new kaco::Device(core, node_id_1));
                    std::string path = ros::package::getPath("kacanopen");

                    boost::filesystem::path full_path = "/home/pi/hexa_workspace/src/hexa_package/edsFiles/epos2_347717.eds";

                    device_1->load_dictionary_from_eds(full_path.string());
                    core.nmt.send_nmt_message(node_id_1,
                                              kaco::NMT::Command::enter_preoperational);

                    initializeDevice(device_1, heartbeat_interval, node_id_1);

                    int8_t set_mode_of_operation = -3;

                    std::this_thread::sleep_for(std::chrono::microseconds(5000));

                    device_1->set_entry(0x6060, 0x00, set_mode_of_operation,
                                        kaco::WriteAccessMethod::sdo);

                    device_1->start();
                    //printDeviceInfo(device_1);
                    device_connected_1 = true;

                } catch (const std::exception &exc) {
                    std::cout << "Exception in device alive!" << std::endl;
                    std::cerr << exc.what();
                    found_node_1 = false;
                    device_connected_1 = false;
                }
            }
        }
    });

    int loopCounter = 0;
    high_resolution_clock::time_point lastTime = high_resolution_clock::now();

    high_resolution_clock::time_point startprogramm = high_resolution_clock::now();

    std::cout << "start subscring .. .. . ." << endl;
    ros::Subscriber loadCell1Sub = n.subscribe("sensor", 1000, getSensorData);
    ros::Subscriber settingsSub = n.subscribe("setting", 1000, getSettingData);

    // Define a lamda expression
    auto f = []() {
        ros::spin();
        //or
        //ros::spinOnce();
        //std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    };

// Pass f and its parameters to thread
// object constructor as
    std::thread thread_object(f);

    while (!(device_connected && device_connected_1)) {
        cout << "waiting for drives\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    }

    //waite for sensor data

    while (!(LoadcellRightHip != 0 && LoadcellLeftHip != 0)) {
        cout << "waiting for sensor data\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    }

    //ros::Subscriber actionSubscriber = n.subscribe("drivesAction", 1000, actionSubscriberCallback);

    device->set_entry("controlword", static_cast<uint16_t>(0x000F),
                      kaco::WriteAccessMethod::pdo);

    device_1->set_entry("controlword", static_cast<uint16_t>(0x000F),
                        kaco::WriteAccessMethod::pdo);

    cout << "\noperational\n";
    while (keepRunning) {
        high_resolution_clock::time_point startloop = high_resolution_clock::now();
        if (device_connected && device_connected_1) {

            // Lock device mutex
            //todo check why?
            std::lock_guard <std::mutex> lock(device_mutex);

            try {

                // duration<double, std::milli> time_take_to_this = high_resolution_clock::now() - startprogramm;
                // double currentTime = time_take_to_this.count() ;
                actual_curr_0 =
                        device->get_entry("current_actual_value",
                                          kaco::ReadAccessMethod::cache);
                //pdo_request_and_wait
                actual_curr_1 =
                        device_1->get_entry("current_actual_value",
                                            kaco::ReadAccessMethod::cache);

                actual_position_0 =
                        device->get_entry("position_actual_value",
                                          kaco::ReadAccessMethod::cache);

                actual_position_1 =
                        device_1->get_entry("position_actual_value",
                                            kaco::ReadAccessMethod::cache);

                //cout << actual_curr_0 << " " << actual_curr_1 << "\n";
                //cout << actual_position_0 << " " << actual_position_1 << "\n";
                duration<double, std::milli> time_span = high_resolution_clock::now() - lastTime;

                lastTime = high_resolution_clock::now();

                // device_1->set_entry("current_mode_setting_value", static_cast<int16_t>(1000),
                //                   kaco::WriteAccessMethod::pdo);

                // device_1->set_entry("current_mode_setting_value", desiredCurrent,
                //                   kaco::WriteAccessMethod::pdo);

                // device_1->set_entry("current_mode_setting_value", static_cast<int16_t>(1000),
                //                   kaco::WriteAccessMethod::pdo);

                // device->set_entry("controlword", static_cast<uint16_t>(0x000F),
                //                   kaco::WriteAccessMethod::pdo);

                // device_1->set_entry("controlword", static_cast<uint16_t>(0x000F),
                //                   kaco::WriteAccessMethod::pdo);

                loopCounter = loopCounter + 1;

                // Publish This Message
                // std_msgs::Int16 msg0;
                // msg0.data = actual_curr_0;
                // chatter_pub0.publish(msg0);

                // std_msgs::Int16 msg1;
                // msg1.data = actual_curr_1;
                // chatter_pub1.publish(msg1);

                // static int32_t actual_position_0_old = -970;
                // static int32_t actual_position_1_old = -970;
                // // Publish postion message
                // if(messageRecievedFromController && actual_position_0_old != actual_position_0 ){
                //   actual_position_0_old = actual_position_0;
                //   std_msgs::Int32 postionMessage0;
                //   postionMessage0.data = actual_position_0;
                //   postion_message_publisher_0.publish(postionMessage0);
                // }

                // if(messageRecievedFromController && actual_position_1_old != actual_position_1){
                //   actual_position_1_old = actual_position_1;
                //   std_msgs::Int32 postionMessage1;
                //   postionMessage1.data = actual_position_1;
                //   postion_message_publisher_1.publish(postionMessage1);
                // }

                hexa_package::drivesFeedBack drivesFeedBack;
                drivesFeedBack.actualPosition0 = actual_position_0;
                drivesFeedBack.actualPosition1 = actual_position_1;
                drivesFeedBack.loadcell0 = Force_LH;
                drivesFeedBack.loadcell1 = Force_RH;
                drivesFeedBack_publisher.publish(drivesFeedBack);

                control();

                device->set_entry("current_mode_setting_value", Currentleft, kaco::WriteAccessMethod::pdo);
                device_1->set_entry("current_mode_setting_value", Currentright, kaco::WriteAccessMethod::pdo);

            } catch (const std::exception &exc) {

                std::cout << "Exception in main!" << std::endl;
                std::cerr << exc.what();

                //turn off device
                Currentleft = 0;
                Currentright = 0;
                device_1->set_entry("current_mode_setting_value", Currentleft, kaco::WriteAccessMethod::pdo);
                device->set_entry("current_mode_setting_value", Currentright, kaco::WriteAccessMethod::pdo);
            }
        }
        // duration<double, std::milli> loop_calculation_time_span = high_resolution_clock::now() - startloop;
        // // Sleep
        // if((unsigned int)loop_calculation_time_span.count() < 5 ){
        //   std::this_thread::sleep_for(std::chrono::milliseconds(5 - (unsigned int)loop_calculation_time_span.count() ));
        // }else{
        //   std::this_thread::sleep_for(std::chrono::microseconds(100));
        // }
        // ros::spinOnce();
        loop_rate.sleep();
        //ros::Rate.sleep();
    }

    std::cout << "Finished." << std::endl;
    return EXIT_SUCCESS;
};

void control() {

    static long double a_filter = 0.01;
    static long double pi = 3.14;
    static long double D_Time;
    static long double D_time = D_Time = 0.005;
    static long double Current_Limit = 3000;

    static long double Assist_R = 0;
    static long double Assist_L = 0;

//# Feedback
// long double LoadcellRightHip = 0;
// long double LoadcellLeftHip = 0;
    static long double PositionActualValueright = actual_position_1;
    PositionActualValueright = actual_position_1;
    static long double PositionActualValueleft = actual_position_0;
    PositionActualValueleft = actual_position_0;
    static long double VelocityActualValueright = 0;
    static long double VelocityActualValueleft = 0;

    static long double CurrentActualValueright = actual_curr_1;
    CurrentActualValueright = actual_curr_1;
    static long double CurrentActualValueleft = actual_curr_0;
    CurrentActualValueleft = actual_curr_0;

    static long double Zero_Impedance_Gain = 1;

    static long double I_R = 0.1;
    static long double C_R = 1.5;
    static long double FK_R = 1.5;
    static long double MgL_R = 0.5;
    static long double PID_Gain_R = 0.3;
    static long double N_RH = 100;

    static long double I_L = 0.1;
    static long double C_L = 1.5;
    static long double FK_L = 1.5;
    static long double MgL_L = 0.5;
    static long double PID_Gain_L = 0.5;
    static long double N_LH = 100;

    //# initial value

    static long double Flt_Vel_RH_old = 0;
    static long double Flt_Vel_LH_old = 0;
    static long double L_Assist = 755;
    static long double R_Assist = 750;
    static long double R_o = 0;
    static long double L_o = 0;
    static long double Flt_Acc_RH_old = 0;
    static long double Flt_Acc_LH_old = 0;
    static long double ef_RH_old = 0;
    static long double ef_LH_old = 0;
    static long double PD_RH_old = 0;
    static long double PD_LH_old = 0;

    static long double D_RH = 0.1;
    static long double D_LH = 0.1;
    static long double timer_r = 0;
    static long double timer_l = 0;
    static long double Moving_Average_RH[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    static long double Moving_Average_LH[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    static long double Moving_Average_FR[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    static long double Moving_Average_FL[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    static long double Velocity_RH = 0;
    static long double Velocity_LH = 0;
    static long double Theta_RH = 0;
    static long double Theta_LH = 0;
    static long double Flt_Vel_RH = 0;
    static long double Flt_Vel_LH = 0;
    static long double Flt_Acc_RH = 0;
    static long double Flt_Acc_LH = 0;
    static long double ACC_RH = 0;
    static long double ACC_LH = 0;
    static long double SUM_Vel_R = 0;
    static long double SUM_Vel_L = 0;
    static long double SUM_Tau_R = 0;
    static long double SUM_Tau_L = 0;
    static long double ef_RH = 0;
    static long double ef_LH = 0;
    static long double SIGN_Theta_dt_RH = 0;
    static long double SIGN_Theta_dt_LH = 0;
    static long double ef_RH_dt = 0;
    static long double ef_LH_dt = 0;
    static long double PD_RH = 0;
    static long double PD_LH = 0;
    static long double Trq_RH_Zero_Impedance = 0;
    static long double Trq_LH_Zero_Impedance = 0;
    static long double R_i = 0;
    static long double L_i = 0;
    static long double timer_delay = 0;
    static long double R_Assist_Delay = 0;
    static long double L_Assist_Delay = 0;
    static long double Theta_dt_RH = 0;
    static long double Theta_dt_LH = 0;

    Force_RH = 0.07 * (-0.023306 * (LoadcellRightHip) + 458.15 - 6.0) - 12.75;
    Force_LH = 0.07 * (-0.023359 * (LoadcellLeftHip) + 756.79 - 6.0) - 1;

    Moving_Average_RH[1] = Moving_Average_RH[2];
    Moving_Average_RH[2] = Moving_Average_RH[3];
    Moving_Average_RH[3] = Moving_Average_RH[4];
    Moving_Average_RH[4] = Moving_Average_RH[5];
    Moving_Average_RH[5] = Moving_Average_RH[6];
    Moving_Average_RH[6] = Moving_Average_RH[7];
    Moving_Average_RH[7] = Moving_Average_RH[8];
    Moving_Average_RH[8] = Velocity_RH;

    Moving_Average_LH[1] = Moving_Average_LH[2];
    Moving_Average_LH[2] = Moving_Average_LH[3];
    Moving_Average_LH[3] = Moving_Average_LH[4];
    Moving_Average_LH[4] = Moving_Average_LH[5];
    Moving_Average_LH[5] = Moving_Average_LH[6];
    Moving_Average_LH[6] = Moving_Average_LH[7];
    Moving_Average_LH[7] = Moving_Average_LH[8];
    Moving_Average_LH[8] = Velocity_LH;

    Moving_Average_FR[1] = Moving_Average_FR[2];
    Moving_Average_FR[2] = Moving_Average_FR[3];
    Moving_Average_FR[3] = Moving_Average_FR[4];
    Moving_Average_FR[4] = Moving_Average_FR[5];
    Moving_Average_FR[5] = Moving_Average_FR[6];
    Moving_Average_FR[6] = Moving_Average_FR[7];
    Moving_Average_FR[7] = Moving_Average_FR[8];
    Moving_Average_FR[8] = Force_RH;

    Moving_Average_FL[1] = Moving_Average_FL[2];
    Moving_Average_FL[2] = Moving_Average_FL[3];
    Moving_Average_FL[3] = Moving_Average_FL[4];
    Moving_Average_FL[4] = Moving_Average_FL[5];
    Moving_Average_FL[5] = Moving_Average_FL[6];
    Moving_Average_FL[6] = Moving_Average_FL[7];
    Moving_Average_FL[7] = Moving_Average_FL[8];
    Moving_Average_FL[8] = Force_LH;

    // Theta
    Theta_RH = -(2 * pi / (4800)) * PositionActualValueright;
    Theta_LH = +(2 * pi / (4800)) * PositionActualValueleft;

    // Velocity With Filter
    Flt_Vel_RH = (1 - D_Time / a_filter) * Flt_Vel_RH_old + (Theta_RH) * D_Time / a_filter;
    Flt_Vel_LH = (1 - D_Time / a_filter) * Flt_Vel_LH_old + (Theta_LH) * D_Time / a_filter;

    Velocity_RH = (Flt_Vel_RH - Flt_Vel_RH_old) / D_Time;
    Velocity_LH = (Flt_Vel_LH - Flt_Vel_LH_old) / D_Time;

    Flt_Vel_RH_old = Flt_Vel_RH;
    Flt_Vel_LH_old = Flt_Vel_LH;

    // Velocity Filter
    Flt_Acc_RH = (1 - D_Time / a_filter) * Flt_Acc_RH_old + (Velocity_RH) * D_Time / a_filter;
    Flt_Acc_LH = (1 - D_Time / a_filter) * Flt_Acc_LH_old + (Velocity_LH) * D_Time / a_filter;

    // Acceleration
    ACC_RH = (Flt_Acc_RH - Flt_Acc_RH_old) / D_Time;
    ACC_LH = (Flt_Acc_LH - Flt_Acc_LH_old) / D_Time;

    Flt_Acc_RH_old = Flt_Acc_RH;
    Flt_Acc_LH_old = Flt_Acc_LH;

// Moving Average
    SUM_Vel_R = (Moving_Average_RH[1] + Moving_Average_RH[2] + Moving_Average_RH[3] + Moving_Average_RH[4] +
                 Moving_Average_RH[5] + Moving_Average_RH[6] + Moving_Average_RH[7] + Moving_Average_RH[8]) / 8;
    SUM_Vel_L = (Moving_Average_LH[1] + Moving_Average_LH[2] + Moving_Average_LH[3] + Moving_Average_LH[4] +
                 Moving_Average_LH[5] + Moving_Average_LH[6] + Moving_Average_LH[7] + Moving_Average_LH[8]) / 8;

    SUM_Tau_R = -(Moving_Average_FR[1] + Moving_Average_FR[2] + Moving_Average_FR[3] + Moving_Average_FR[4] +
                  Moving_Average_FR[5] + Moving_Average_FR[6] + Moving_Average_FR[7] + Moving_Average_FR[8]) / 8;
    SUM_Tau_L = -(Moving_Average_FL[1] + Moving_Average_FL[2] + Moving_Average_FL[3] + Moving_Average_FL[4] +
                  Moving_Average_FL[5] + Moving_Average_FL[6] + Moving_Average_FL[7] + Moving_Average_FL[8]) / 8;

    // Identification
    if (Force_RH > 0) {
        SIGN_Theta_dt_RH = -1;
    } else if (Force_RH < 0) {
        SIGN_Theta_dt_RH = +1;
    } else {
        SIGN_Theta_dt_RH = 0;
    }

    if (Force_LH > 0) {
        SIGN_Theta_dt_LH = -1;
    } else if (Force_LH < 0) {
        SIGN_Theta_dt_LH = +1;
    } else {
        SIGN_Theta_dt_LH = 0;
    }

    // PD Code
    ef_RH = 0 - Force_RH;
    ef_LH = 0 - Force_LH;

    ef_RH_dt = (ef_RH - ef_RH_old) / D_time;
    ef_LH_dt = (ef_LH - ef_LH_old) / D_time;

    PD_RH = (1 - N_RH * D_time) * PD_RH_old + N_RH * P_RH * D_time * (ef_RH) + (P_RH + N_RH * D_RH) * D_time * ef_RH_dt;
    PD_LH = (1 - N_LH * D_time) * PD_LH_old + N_LH * P_LH * D_time * (ef_LH) + (P_LH + N_LH * D_LH) * D_time * ef_LH_dt;

    ef_RH_dt = (ef_RH - ef_RH_old) / D_time;
    ef_LH_dt = (ef_LH - ef_LH_old) / D_time;

    PD_RH = (1 - N_RH * D_time) * PD_RH_old + N_RH * P_RH * D_time * (ef_RH) + (P_RH + N_RH * D_RH) * D_time * ef_RH_dt;
    PD_LH = (1 - N_LH * D_time) * PD_LH_old + N_LH * P_LH * D_time * (ef_LH) + (P_LH + N_LH * D_LH) * D_time * ef_LH_dt;

    ef_RH_old = ef_RH;
    ef_LH_old = ef_LH;

    PD_RH_old = PD_RH;
    PD_LH_old = PD_LH;

    //Zero Impedance
    Trq_RH_Zero_Impedance =
            -(I_R * ACC_RH + C_R * Theta_dt_RH + FK_R * SIGN_Theta_dt_RH + MgL_R * sin(Theta_RH) +
              PID_Gain_R * PD_RH) / 0.0369;
    Trq_LH_Zero_Impedance =
            (I_L * ACC_LH + C_L * Theta_dt_LH + FK_L * SIGN_Theta_dt_LH + MgL_L * sin(Theta_LH) +
             PID_Gain_L * PD_LH) / 0.0369;

    // Assist as Needed
    if (control_algorithm == "assist_as_needed") {
        if (timer_r > timer_right) {
            R_o = 0;
        } else if (Velocity_RH > 0.2 && Velocity_LH <= 0.0 && R_i == 0) {
            R_o = 1;
            R_i = 1;
            L_i = 0;
        }

        if (timer_l > timer_left) {
            L_o = 0;
        } else if (Velocity_LH > 0.2 && Velocity_RH <= 0.0 && L_i == 0) {
            L_o = 1;
            R_i = 0;
            L_i = 1;
        }

        if (R_o == 1) {
            timer_r = timer_r + 1;
            R_Assist = -Assist_Right; // this value came from GUI;
        } else if (R_o == 0) {
            timer_r = 0;
            R_Assist = 0;
        }

        if (L_o == 1) {
            timer_l = timer_l + 1;
            L_Assist = Assist_Left;
        } else if (L_o == 0) {
            timer_l = 0;
            L_Assist = 0;
        }
    }

    if (control_algorithm == "time_based_assist") {
        // Time based assist for right leg
        if (control_algorithm_right == "assist_as_needed") {
            if (timer_r > timer_right) {
                R_o = 0;
            } else if (timer_delay >= delay_time && R_i == 0) {
                R_o = 1;
                R_i = 1;
                L_i = 0;
                timer_delay = 0;
            }

            if (timer_l > timer_left) {
                L_o = 0;
            } else if (SUM_Vel_L >= 0.1 && SUM_Vel_R <= 0.0 && L_i == 0) {
                L_o = 1;
                R_i = 0;
                L_i = 1;
            }

            if (L_i == 1) {
                timer_delay = timer_delay + 1;
            }

            if (R_o == 1) {
                timer_r = timer_r + 1;
                R_Assist_Delay = -Assist_Delay;
            } else if (R_o == 0) {
                timer_r = 0;
                R_Assist_Delay = 0;
            }

            if (L_o == 1) {
                timer_l = timer_l + 1;
                L_Assist_Delay = Trq_LH_Zero_Impedance;
            } else if (L_o == 0) {
                timer_l = 0;
                L_Assist = Trq_LH_Zero_Impedance;
            }
        }
        // Time based assist for left leg
//        cout<<"control_algorithm_left: "<<control_algorithm_left<<endl;
        if (control_algorithm_left == "assist_as_needed") {
            if (timer_r > timer_right) {
                R_o = 0;
            } else if (SUM_Vel_R >= 0.1 && SUM_Vel_L <= 0.0 && R_i == 0) {
                R_o = 1;
                R_i = 1;
                L_i = 0;
            }

            if (timer_l > timer_left) {
                L_o = 0;
            } else if (timer_delay >= delay_time && L_i == 0) {
                L_o = 1;
                R_i = 0;
                L_i = 1;
                timer_delay = 0;
            }

            if (R_i == 1) {
                timer_delay = timer_delay + 1;
            }

            if (R_o == 1) {
                timer_r = timer_r + 1;
                R_Assist_Delay = Trq_RH_Zero_Impedance;
            } else if (R_o == 0) {
                timer_r = 0;
                R_Assist_Delay = Trq_RH_Zero_Impedance;
            }

            if (L_o == 1) {
                timer_l = timer_l + 1;
                L_Assist_Delay = Assist_Delay;
            } else if (L_o == 0) {
                timer_l = 0;
                L_Assist_Delay = 0;
            }
        }
    }

    // Send TO Derive

    // Right Left Control
    if (control_algorithm_right == "zero_impedance") {
        Currentright = int(Trq_RH_Zero_Impedance);
    }
    if (control_algorithm_right == "assist_as_needed") {
        Currentright = int(R_Assist);
    }
    if (control_algorithm == "time_based_assist") {
        Currentright = int(R_Assist_Delay);
//        cout << "R Assist: " << Currentright << endl;
    }

    // Left Leg Control
    if (control_algorithm_left == "zero_impedance") {
        Currentleft = int(Trq_LH_Zero_Impedance);
    }
    if (control_algorithm_left == "assist_as_needed") {
        Currentleft = int(L_Assist);
    }

    if (control_algorithm == "time_based_assist") {
        Currentleft = int(L_Assist_Delay);
//        cout<<"Timer : "<<timer_delay<<endl;
    }

    static int loopCounter;
    loopCounter++;
    //Currentright = 200 * sin(loopCounter / 200.00);
    //Currentleft = 200 * sin(loopCounter / 200.00);

    // # desiredCurrentRight.publish(int(Currentright))
    // # desiredCurrentLeft.publish(int(Currentleft))

    // # drivesFeedBack df
    // # df.actualCurrent0 = Currentleft
    // # df.actualCurrent1 = Currentright
    // drives_action.publish(drivesAction(int(Currentleft), int(Currentright)))

    // #desiredCurrentRight.publish(int(500))
    // #desiredCurrentLeft.publish(int(500))
    // # if((loopCounter % 1 == 0):
    // #     print("time:" + str(time.time()) + "\tForce_LH:" + str(Force_LH) + "\tForce_RH:" + str(Force_RH) +"\tCurrentleft:" + str(Currentleft) + "\tCurrentright:" + str(Currentright) + "\tCurrentActualValueright:" + str(CurrentActualValueright) + "\tCurrentActualValueleft" + str(CurrentActualValueleft) + "\tLoadcellLeftHip:" + str(LoadcellLeftHip) + "\tLoadcellRightHip:" + str(LoadcellRightHip) + "\tPositionActualValueleft:" + str(PositionActualValueleft)  + "\tPositionActualValueright:" + str(PositionActualValueright))
    // loopCounter = loopCounter + 1
    // #print("time:" + str(time.time()) )
    //rate.sleep()
    //  if((loopCounter % 40)
    //  cout << "time:" << "str(time.time())" << "\tForce_LH:" << (Force_LH) << "\tForce_RH:" << (Force_RH) << "\tCurrentleft:" <<(Currentleft) << "\tCurrentright:" << (Currentright) << "\tCurrentActualValueright:" << (CurrentActualValueright) << "\tCurrentActualValueleft" << (CurrentActualValueleft) << "\tLoadcellLeftHip:" << (LoadcellLeftHip) << "\tLoadcellRightHip:" << (LoadcellRightHip) << "\tPositionActualValueleft:" << (PositionActualValueleft)  << "\tPositionActualValueright:" << (PositionActualValueright) << endl ;
    //cout << Force_LH << "\tForce_RH:" << (Force_RH) << "\n";
// cout << LoadcellRightHip;

    // duration<double, std::milli> time_take_to_this = high_resolution_clock::now() - lastTime;
    // double currentTime = time_take_to_this.count() ;
    // lastTime = high_resolution_clock::now();
    // cout << currentTime << "\n" ;

}


//=============================================================================================================
/**
 * @file     hpifit.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>;
 *           Ruben Dörfel <doerfelruben@aol.com>;
 *           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
 * @since    0.1.0
 * @date     March, 2017
 *
 * @section  LICENSE
 *
 * Copyright (C) 2017, Lorenz Esch, Matti Hamalainen. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    HPIFit class defintion.
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "hpifit.h"
#include "hpifitdata.h"
#include "sensorset.h"
#include "hpidataupdater.h"
#include "signalmodel.h"
#include "hpimodelparameters.h"

#include <utils/ioutils.h>
#include <utils/mnemath.h>

#include <iostream>
#include <vector>
#include <numeric>
#include <fiff/fiff_cov.h>
#include <fiff/fiff_dig_point_set.h>
#include <fstream>

#include <fwd/fwd_coil_set.h>

//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================

#include <Eigen/Dense>

//=============================================================================================================
// QT INCLUDES
//=============================================================================================================

#include <QVector>
#include <QFuture>
#include <QtConcurrent/QtConcurrent>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace INVERSELIB;
using namespace FIFFLIB;
using namespace FWDLIB;

//=============================================================================================================
// DEFINE GLOBAL METHODS
//=============================================================================================================

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

//=============================================================================================================

HPIFit::HPIFit(const SensorSet& sensorSet)
    : m_sensors(sensorSet),
      m_signalModel(SignalModel())
{

}

//=============================================================================================================

void HPIFit::checkForUpdate(const SensorSet &sensorSet)
{
    if(m_sensors != sensorSet) {
        m_sensors = sensorSet;
    }
}

//=============================================================================================================

void HPIFit::fit(const MatrixXd& matProjectedData,
                 const MatrixXd& matProjectors,
                 const HpiModelParameters& hpiModelParameters,
                 const MatrixXd& matCoilsHead,
                 HpiFitResult& hpiFitResult)
{
    fit(matProjectedData,matProjectors,hpiModelParameters,matCoilsHead,false,hpiFitResult);
}

//=============================================================================================================

void HPIFit::fit(const MatrixXd& matProjectedData,
                 const MatrixXd& matProjectors,
                 const HpiModelParameters& hpiModelParameters,
                 const MatrixXd& matCoilsHead,
                 const bool bOrderFrequencies,
                 HpiFitResult& hpiFitResult)
{
    if(matProjectedData.rows() != matProjectors.rows()) {
        std::cout<< "HPIFit::fit - Projector and data dimensions do not match. Returning."<<std::endl;
        return;
    } else if(hpiModelParameters.iNHpiCoils()!= matCoilsHead.rows()) {
        std::cout<< "HPIFit::fit - Number of coils and hpi digitizers do not match. Returning."<<std::endl;
        return;
    } else if(matProjectedData.rows()==0 || matProjectors.rows()==0) {
        std::cout<< "HPIFit::fit - No data or Projectors passed. Returning."<<std::endl;
        return;
    } else if(m_sensors.ncoils() != matProjectedData.rows()) {
        std::cout<< "HPIFit::fit - Number of channels in sensors and data do not match. Returning."<<std::endl;
        return;
    }

    const MatrixXd matAmplitudes = computeAmplitudes(matProjectedData,
                                                     hpiModelParameters);

    const MatrixXd matCoilsSeed = computeSeedPoints(matAmplitudes,
                                                    hpiFitResult.devHeadTrans,
                                                    hpiFitResult.errorDistances,
                                                    matCoilsHead);

    CoilParam fittedCoilParams = dipfit(matCoilsSeed,
                                        m_sensors,
                                        matAmplitudes,
                                        hpiModelParameters.iNHpiCoils(),
                                        matProjectors,
                                        500,
                                        1e-9f);

    if(bOrderFrequencies) {
        const std::vector<int> vecOrder = findCoilOrder(fittedCoilParams.pos,
                                                        matCoilsHead);

        fittedCoilParams.pos = order(vecOrder,fittedCoilParams.pos);
        hpiFitResult.hpiFreqs = order(vecOrder,hpiModelParameters.vecHpiFreqs());
    }

    hpiFitResult.GoF = computeGoF(fittedCoilParams.dpfiterror);

    hpiFitResult.fittedCoils = getFittedPointSet(fittedCoilParams.pos);

    hpiFitResult.devHeadTrans = computeDeviceHeadTransformation(fittedCoilParams.pos,
                                                                matCoilsHead);

    hpiFitResult.errorDistances = computeEstimationError(fittedCoilParams.pos,
                                                         matCoilsHead,
                                                         hpiFitResult.devHeadTrans);
}

//=============================================================================================================

Eigen::MatrixXd HPIFit::computeAmplitudes(const Eigen::MatrixXd& matProjectedData,
                                          const HpiModelParameters& hpiModelParameters)
{
    // fit model
    MatrixXd matTopo = m_signalModel.fitData(hpiModelParameters,matProjectedData);
    matTopo.transposeInPlace();

    // split into sine and cosine amplitudes
    const int iNumCoils = hpiModelParameters.iNHpiCoils();

    MatrixXd matAmpSine(matProjectedData.cols(), iNumCoils);
    MatrixXd matAmpCosine(matProjectedData.cols(), iNumCoils);

    matAmpSine = matTopo.leftCols(iNumCoils);
    matAmpCosine = matTopo.middleCols(iNumCoils,iNumCoils);

    // Select sine or cosine component depending on their contributions to the amplitudes
    for(int j = 0; j < iNumCoils; ++j) {
        float fNS = 0.0;
        float fNC = 0.0;
        fNS = matAmpSine.col(j).array().square().sum();
        fNC = matAmpCosine.col(j).array().square().sum();
        if(fNC > fNS) {
            matAmpSine.col(j) = matAmpCosine.col(j);
        }
    }

    return matAmpSine;
}

//=============================================================================================================

Eigen::MatrixXd HPIFit::computeSeedPoints(const Eigen::MatrixXd& matAmplitudes,
                                          const FIFFLIB::FiffCoordTrans& transDevHead,
                                          const QVector<double>& vecError,
                                          const Eigen::MatrixXd& matCoilsHead)
{
    const int iNumCoils = matCoilsHead.rows();
    MatrixXd matCoilsSeed = MatrixXd::Zero(iNumCoils,3);

    const double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();

    if(transDevHead.trans != MatrixXd::Identity(4,4).cast<float>() && dError < 0.010) {
        // if good last fit, use old trafo
        matCoilsSeed = transDevHead.apply_inverse_trans(matCoilsHead.cast<float>()).cast<double>();
    } else {
        // if not, find max amplitudes in channels
        VectorXi vecChIdcs(iNumCoils);

        for (int j = 0; j < iNumCoils; j++) {
            int iChIdx = 0;
            VectorXd::Index indMax;
            matAmplitudes.col(j).maxCoeff(&indMax);
            if(indMax < m_sensors.ncoils()) {
                iChIdx = indMax;
            }
            vecChIdcs(j) = iChIdx;
        }
        // and go 3 cm inwards from max channels
        for (int j = 0; j < vecChIdcs.rows(); ++j) {
            if(vecChIdcs(j) < m_sensors.ncoils()) {
                Vector3d r0 = m_sensors.r0(vecChIdcs(j));
                Vector3d ez = m_sensors.ez(vecChIdcs(j));
                matCoilsSeed.row(j) = (-1 * ez * 0.03 + r0);
            }
        }
    }
    return matCoilsSeed;
}

//=============================================================================================================

CoilParam HPIFit::dipfit(const MatrixXd matCoilsSeed,
                         const SensorSet& sensors,
                         const MatrixXd& matData,
                         const int iNumCoils,
                         const MatrixXd& matProjectors,
                         const int iMaxIterations,
                         const float fAbortError)
{
    //Do this in conncurrent mode
    //Generate QList structure which can be handled by the QConcurrent framework
    QList<HPIFitData> lCoilData;

    for(qint32 i = 0; i < iNumCoils; ++i) {
        HPIFitData coilData;
        coilData.m_coilPos = matCoilsSeed.row(i);
        coilData.m_sensorData = matData.col(i);
        coilData.m_sensors = sensors;
        coilData.m_matProjector = matProjectors;
        coilData.m_iMaxIterations = iMaxIterations;
        coilData.m_fAbortError = fAbortError;

        lCoilData.append(coilData);
    }
    //Do the concurrent filtering
    CoilParam coil(iNumCoils);

    if(!lCoilData.isEmpty()) {
        //        //Do sequential
        //        for(int l = 0; l < lCoilData.size(); ++l) {
        //            doDipfitConcurrent(lCoilData[l]);
        //        }

        //Do concurrent
        QFuture<void> future = QtConcurrent::map(lCoilData,
                                                 &HPIFitData::doDipfitConcurrent);
        future.waitForFinished();

        //Transform results to final coil information
        for(qint32 i = 0; i < lCoilData.size(); ++i) {
            coil.pos.row(i) = lCoilData.at(i).m_coilPos;
            coil.mom = lCoilData.at(i).m_errorInfo.moment.transpose();
            coil.dpfiterror(i) = lCoilData.at(i).m_errorInfo.error;
            coil.dpfitnumitr(i) = lCoilData.at(i).m_errorInfo.numIterations;

            //std::cout<<std::endl<< "HPIFit::dipfit - Itr steps for coil " << i << " =" <<coil.dpfitnumitr(i);
        }
    }
    return coil;
}

//=============================================================================================================

std::vector<int> HPIFit::findCoilOrder(const MatrixXd& matCoilsDev,
                                       const MatrixXd& matCoilsHead)
{
    // extract digitized and fitted coils
    MatrixXd matCoilTemp = matCoilsDev;
    const int iNumCoils = matCoilsDev.rows();

    std::vector<int> vecOrder(iNumCoils);
    std::iota(vecOrder.begin(), vecOrder.end(), 0);;

    // maximum 10 mm mean error
    const double dErrorMin = 0.010;
    double dErrorActual = 0.0;
    double dErrorBest = dErrorMin;

    MatrixXd matTrans(4,4);
    std::vector<int> vecOrderBest = vecOrder;

    bool bSuccess = false;
    // permutation
    do {
        for(int i = 0; i < iNumCoils; i++) {
            matCoilTemp.row(i) =  matCoilsDev.row(vecOrder[i]);
        }
        matTrans = computeTransformation(matCoilsHead,matCoilTemp);
        dErrorActual = objectTrans(matCoilsHead,matCoilTemp,matTrans);
        if(dErrorActual < dErrorMin && dErrorActual < dErrorBest) {
            // exit
            dErrorBest = dErrorActual;
            vecOrderBest = vecOrder;
            bSuccess = true;
        }
    } while (std::next_permutation(vecOrder.begin(), vecOrder.end()));
    return vecOrderBest;
}

//=============================================================================================================

double HPIFit::objectTrans(const MatrixXd& matHeadCoil,
                           const MatrixXd& matCoil,
                           const MatrixXd& matTrans)
{
    // Compute the fiducial registration error - the lower, the better.
    const int iNumCoils = matHeadCoil.rows();
    MatrixXd matTemp = matCoil;

    // homogeneous coordinates
    matTemp.conservativeResize(matCoil.rows(),matCoil.cols()+1);
    matTemp.block(0,3,iNumCoils,1).setOnes();
    matTemp.transposeInPlace();

    // apply transformation
    MatrixXd matTestPos = matTrans * matTemp;

    // remove
    MatrixXd matDiff = matTestPos.block(0,0,3,iNumCoils) - matHeadCoil.transpose();
    VectorXd vecError = matDiff.colwise().norm();

    // compute error
    double dError = matDiff.colwise().norm().mean();;

    return dError;
}

//=============================================================================================================

Eigen::MatrixXd HPIFit::order(const std::vector<int>& vecOrder,
                              const Eigen::MatrixXd& matToOrder)
{
    const int iNumCoils = vecOrder.size();
    MatrixXd matToOrderTemp = matToOrder;

    for(int i = 0; i < iNumCoils; i++) {
        matToOrderTemp.row(i) =  matToOrder.row(vecOrder[i]);
    }
    return matToOrderTemp;
}

//=============================================================================================================

QVector<int> HPIFit::order(const std::vector<int>& vecOrder,
                           const QVector<int>& vecToOrder)
{
    const int iNumCoils = vecOrder.size();
    QVector<int> vecToOrderTemp = vecToOrder;

    for(int i = 0; i < iNumCoils; i++) {
        vecToOrderTemp[i] =  vecToOrder[vecOrder[i]];
    }
    return vecToOrderTemp;
}

//=============================================================================================================

Eigen::VectorXd HPIFit::computeGoF(const Eigen::VectorXd& vecDipFitError)
{
    VectorXd vecGoF(vecDipFitError.size());
    for(int i = 0; i < vecDipFitError.size(); ++i) {
        vecGoF(i) = 1 - vecDipFitError(i);
    }
    return vecGoF;
}

//=============================================================================================================

FIFFLIB::FiffCoordTrans HPIFit::computeDeviceHeadTransformation(const Eigen::MatrixXd& matCoilsDev,
                                                                const Eigen::MatrixXd& matCoilsHead)
{
    const MatrixXd matTrans = computeTransformation(matCoilsHead,matCoilsDev);
    return FiffCoordTrans::make(1,4,matTrans.cast<float>(),true);
}

//=============================================================================================================

Eigen::Matrix4d HPIFit::computeTransformation(Eigen::MatrixXd matNH, MatrixXd matBT)
{
    MatrixXd matXdiff, matYdiff, matZdiff, matC, matQ;
    Matrix4d matTransFinal = Matrix4d::Identity(4,4);
    Matrix4d matRot = Matrix4d::Zero(4,4);
    Matrix4d matTrans = Matrix4d::Identity(4,4);
    double dMeanX,dMeanY,dMeanZ,dNormf;

    for(int i = 0; i < 15; ++i) {
        // Calculate mean translation for all points -> centroid of both data sets
        matXdiff = matNH.col(0) - matBT.col(0);
        matYdiff = matNH.col(1) - matBT.col(1);
        matZdiff = matNH.col(2) - matBT.col(2);

        dMeanX = matXdiff.mean();
        dMeanY = matYdiff.mean();
        dMeanZ = matZdiff.mean();

        // Apply translation -> bring both data sets to the same center location
        for (int j = 0; j < matBT.rows(); ++j) {
            matBT(j,0) = matBT(j,0) + dMeanX;
            matBT(j,1) = matBT(j,1) + dMeanY;
            matBT(j,2) = matBT(j,2) + dMeanZ;
        }

        // Estimate rotation component
        matC = matBT.transpose() * matNH;

        JacobiSVD< MatrixXd > svd(matC ,Eigen::ComputeThinU | ComputeThinV);

        matQ = svd.matrixU() * svd.matrixV().transpose();

        //Handle special reflection case
        if(matQ.determinant() < 0) {
            matQ(0,2) = matQ(0,2) * -1;
            matQ(1,2) = matQ(1,2) * -1;
            matQ(2,2) = matQ(2,2) * -1;
        }

        // Apply rotation on translated points
        matBT = matBT * matQ;

        // Calculate GOF
        dNormf = (matNH.transpose()-matBT.transpose()).norm();

        // Store rotation part to transformation matrix
        matRot(3,3) = 1;
        for(int j = 0; j < 3; ++j) {
            for(int k = 0; k < 3; ++k) {
                matRot(j,k) = matQ(k,j);
            }
        }

        // Store translation part to transformation matrix
        matTrans(0,3) = dMeanX;
        matTrans(1,3) = dMeanY;
        matTrans(2,3) = dMeanZ;

        // Safe rotation and translation to final matrix for next iteration step
        // This step is safe to do since we change one of the input point sets (matBT)
        // ToDo: Replace this for loop with a least square solution process
        matTransFinal = matRot * matTrans * matTransFinal;
    }
    return matTransFinal;
}

//=============================================================================================================

QVector<double> HPIFit::computeEstimationError(const Eigen::MatrixXd& matCoilsDev,
                                               const Eigen::MatrixXd& matCoilsHead,
                                               const FIFFLIB::FiffCoordTrans& transDevHead)
{
    //Calculate Error
    MatrixXd matTemp = matCoilsDev;
    MatrixXd matTestPos = transDevHead.apply_trans(matTemp.cast<float>()).cast<double>();
    MatrixXd matDiffPos = matTestPos - matCoilsHead;

    // compute error
    int iNumCoils = matCoilsDev.rows();
    QVector<double> vecError(iNumCoils);
    for(int i = 0; i < matDiffPos.rows(); ++i) {
        vecError[i] = matDiffPos.row(i).norm();
    }
    return vecError;
}

//=============================================================================================================

FIFFLIB::FiffDigPointSet HPIFit::getFittedPointSet(const Eigen::MatrixXd& matCoilsDev)
{
    FiffDigPointSet fittedPointSet;
    const int iNumCoils = matCoilsDev.rows();

    for(int i = 0; i < iNumCoils; ++i) {
        FiffDigPoint digPoint;
        digPoint.kind = FIFFV_POINT_EEG; //Store as EEG so they have a different color
        digPoint.ident = i;
        digPoint.r[0] = matCoilsDev(i,0);
        digPoint.r[1] = matCoilsDev(i,1);
        digPoint.r[2] = matCoilsDev(i,2);

        fittedPointSet << digPoint;
    }
    return fittedPointSet;
}

//=============================================================================================================

void HPIFit::storeHeadPosition(float fTime,
                               const Eigen::MatrixXf& transDevHead,
                               Eigen::MatrixXd& matPosition,
                               const Eigen::VectorXd& vecGoF,
                               const QVector<double>& vecError)

{
    Matrix3f matRot = transDevHead.block(0,0,3,3);

    Eigen::Quaternionf quatHPI(matRot);
    double dError = std::accumulate(vecError.begin(), vecError.end(), .0) / vecError.size();     // HPI estimation Error

    matPosition.conservativeResize(matPosition.rows()+1, 10);
    matPosition(matPosition.rows()-1,0) = fTime;
    matPosition(matPosition.rows()-1,1) = quatHPI.x();
    matPosition(matPosition.rows()-1,2) = quatHPI.y();
    matPosition(matPosition.rows()-1,3) = quatHPI.z();
    matPosition(matPosition.rows()-1,4) = transDevHead(0,3);
    matPosition(matPosition.rows()-1,5) = transDevHead(1,3);
    matPosition(matPosition.rows()-1,6) = transDevHead(2,3);
    matPosition(matPosition.rows()-1,7) = vecGoF.mean();
    matPosition(matPosition.rows()-1,8) = dError;
    matPosition(matPosition.rows()-1,9) = 0;
}


///// \file   python_module_walras.cpp
/////
///// \brief
/////
///// \authors    Maarten P. Scholl
///// \date       2020-10-11
///// \copyright  Copyright 2017-2020 The Institute for New Economic Thinking,
/////             Oxford Martin School, University of Oxford
/////
/////             Licensed under the Apache License, Version 2.0 (the "License");
/////             you may not use this file except in compliance with the License.
/////             You may obtain a copy of the License at
/////
/////                 http://www.apache.org/licenses/LICENSE-2.0
/////
/////             Unless required by applicable law or agreed to in writing,
/////             software distributed under the License is distributed on an "AS
/////             IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
/////             express or implied. See the License for the specific language
/////             governing permissions and limitations under the License.
/////
/////             You may obtain instructions to fulfill the attribution
/////             requirements in CITATION.cff
/////
//#include <esl/economics/markets/walras/python_module_walras.hpp>
//#include <esl/economics/markets/walras/price_setter.hpp>
//#include <esl/economics/markets/walras/tatonnement.hpp>
//
//#include <esl/law/property_collection.hpp>
//
//
//
//#ifdef WITH_PYTHON
//
//#include <esl/simulation/python_module_simulation.hpp>
//
//#include <utility>
//
//#define BOOST_BIND_GLOBAL_PLACEHOLDERS
//#include <boost/python.hpp>
//#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
//using boost::python::init;
//using boost::python::no_init;
//using boost::python::object;
//using boost::python::enum_;
//using boost::python::class_;
//using boost::python::dict;
//using boost::python::list;
//using boost::python::len;
//using boost::python::make_tuple;
//using boost::python::extract;
//using boost::python::wrapper;
//using boost::python::vector_indexing_suite;
//
//using namespace esl::economics::markets;
//using namespace esl::economics::markets::tatonnement;
//
//
//
//template<typename object_t_>
//void do_release(typename boost::shared_ptr<object_t_> const&, object_t_*)
//{
//
//}
//
//template<typename object_t_>
//typename std::shared_ptr<object_t_> to_std(typename boost::shared_ptr<object_t_> const& p)
//{
//    return std::shared_ptr<object_t_>(p.get(), [p](auto &&PH1)
//        {
//          return do_release<object_t_>(p, std::forward<decltype(PH1)>(PH1));
//        });
//
//}
//
//template<typename object_t_>
//boost::shared_ptr<object_t_> to_boost(std::shared_ptr<object_t_>& ptr)
//{
//    return boost::shared_ptr<object_t_>(ptr.get(), [ptr](object_t_ *) mutable
//                                        {
//                                            ptr.reset();
//                                        });
//}
//
//
//class python_differentiable_order_message
//    : public esl::economics::markets::walras::differentiable_order_message
//        , public wrapper<esl::economics::markets::walras::differentiable_order_message>
//{
//public:
//    python_differentiable_order_message(
//        const esl::simulation::python_module::python_identity &sender       = esl::simulation::python_module::python_identity(),
//        const esl::simulation::python_module::python_identity& recipient     = esl::simulation::python_module::python_identity(),
//        esl::simulation::time_point sent         = esl::simulation::time_point(),
//        esl::simulation::time_point received     = esl::simulation::time_point())
//        : esl::economics::markets::walras::differentiable_order_message
//              ( esl::reinterpret_identity_cast<esl::agent>(sender)
//                  , esl::reinterpret_identity_cast<esl::agent>(recipient)
//                  , sent
//                  , received)
//        , wrapper<esl::economics::markets::walras::differentiable_order_message>()
//    {
//
//    }
//
//    // the default implementation is to demand zero for all
//    [[nodiscard]] std::map<esl::identity<esl::law::property>, esl::variable> excess_demand(const std::map<
//        esl::identity<esl::law::property>
//        , std::tuple<esl::economics::markets::quote, esl::variable>> &quotes) const override
//    {
//        dict quotes_;
//
//        for(const auto &[i, v]: quotes){
//            auto t = make_tuple(std::get<0>(v), esl::value(std::get<1>(v)));
//            quotes_.setdefault(i, t);
//        }
//
//        // specify the type, so that the return value is converted to python::object
//        object return_value_ = get_override("excess_demand")(quotes_);
//        dict excess_ = extract<dict>(return_value_);
//        auto keys = list(excess_.keys());
//        auto values = list(excess_.values());
//
//        std::map<esl::identity<esl::law::property>, esl::variable> result_;
//
//        for(int i = 0; i < len(keys); ++i) {
//            extract<esl::identity<esl::law::property>> extractor(keys[i]);
//            extract<double> value_extractor(values[i]);
//            if(extractor.check() && value_extractor.check()) {
//                auto key   = extractor();
//                auto value = value_extractor();
//
//                result_.emplace(key, esl::variable(value));
//            }
//        }
//
//        return result_;
//    }
//};
//
//
/////
///// \brief  Excess demand model wrapper
/////
//class python_excess_demand_model
//: public excess_demand_model
//, public wrapper<excess_demand_model>
//{
//public:
//    explicit python_excess_demand_model(esl::law::property_map<quote> initial_quotes)
//        : excess_demand_model(std::move(initial_quotes))
//        , wrapper<excess_demand_model>()
//    {
//
//    }
//};
//
/////
///// \brief  Helper funciton
/////
///// \param e
///// \return
//dict clear_market(python_excess_demand_model *e)
//{
//    auto quotes_ = e->compute_clearing_quotes();
//    dict result_;
//
//    if(quotes_.has_value()){
//        //LOG(trace) << "clear_market has_value" << std::endl;
//        for(const auto &[k,v]: quotes_.value()){
//            //LOG(trace) << k <<" = " << v << std::endl;
//            result_.setdefault(k, v);
//        }
//    }
//
//    return result_;
//}
//
//
/////
///// \brief  Constructor wrapper that deals with the Python-supplied mapping
/////
///// \param  Python dict with property base pointers as keys, and quote s as value
///// \return
//boost::shared_ptr<python_excess_demand_model> excess_demand_model_python_constructor(const dict &d)
//{
//    esl::law::property_map<quote> pm;
//
//    list keys = list(d.keys());
//    list values = list(d.values());
//
//    for (int i = 0; i < len(keys); ++i) {
//        extract<boost::shared_ptr<esl::law::property>> extractor(keys[i]);
//        extract<quote> value_extractor(values[i]);
//        if (extractor.check() && value_extractor.check()){
//            auto key = extractor();
//            auto value = value_extractor();
//
//            pm.try_emplace(to_std(key), value);
//        }
//    }
//
//    auto model_ = boost::make_shared<python_excess_demand_model>(pm);
//    model_->methods =
//        { python_excess_demand_model::derivative_free_root
//        , python_excess_demand_model::derivative_free_minimization};
//
//    return model_;
//}
//
///*
//list get_differentiable_order_message(const excess_demand_model* e)
//{
//
//    boost::shared_ptr<walras::differentiable_order_message>
//    e->excess_demand_functions_
//}*/
//
////
//typedef std::vector<boost::shared_ptr<walras::differentiable_order_message>> messages_t;
//
/////
///// \brief  converts the list of demand messages to Python
/////
///// \param e
///// \return
//messages_t get_excess_demand_functions(const excess_demand_model &e)
//{
//    std::vector<boost::shared_ptr<walras::differentiable_order_message>> result_;
//    for(auto m: e.excess_demand_functions_){
//        result_.push_back(to_boost(m));
//    }
//    return result_;
//}
//
/////
///// \brief  accepts Python list back to
/////
///// \param e
///// \param l
//void set_excess_demand_functions(excess_demand_model &e, const list& l)
//{
//    std::vector<boost::shared_ptr<walras::differentiable_order_message>> result_;
//    e.excess_demand_functions_.clear();
//    for(int i = 0; i < len(l); ++i){
//        extract<boost::shared_ptr<python_differentiable_order_message>> extractor(l[i]);
//        e.excess_demand_functions_.push_back(to_std(extractor()));
//    }
//}
//
//BOOST_PYTHON_MODULE(_walras)
//{
//    enum_<excess_demand_model::solver>("solver")
//        // TODO: implement automatic differentiation for Python
//        //.value("root", excess_demand_model::root)
//        //.value("minimization", excess_demand_model::minimization)
//        .value("derivative_free_root", excess_demand_model::derivative_free_root)
//        .value("derivative_free_minimization", excess_demand_model::derivative_free_minimization)
//        .export_values()
//        ;
//
//    class_< python_differentiable_order_message, boost::noncopyable>
//        ("differentiable_order_message",
//         init< esl::simulation::python_module::python_identity
//              ,esl::simulation::python_module::python_identity
//              , esl::simulation::time_point
//              ,esl::simulation::time_point
//              >())
//        .add_property("supply", &walras::differentiable_order_message::supply)
//        ;
//
//    // expose vector of messages to Python
//    class_<messages_t>("messages_t").def(vector_indexing_suite<messages_t>() );
//
//    class_<python_excess_demand_model, boost::noncopyable>("excess_demand_model", no_init) // non-trivial constructor
//        .def("__init__", make_constructor(&excess_demand_model_python_constructor))
//        .def_readwrite("circuit_breaker", &excess_demand_model::circuit_breaker)
//        .def_readwrite("methods", &excess_demand_model::methods)
//        .def_readwrite("quotes", &excess_demand_model::quotes)
//        .def("compute_clearing_quotes", &clear_market)
//        .add_property("excess_demand_functions", &get_excess_demand_functions, &set_excess_demand_functions)
//        ;
//}
//
//#endif


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_CollisionHandlerFactory.hpp
//! \author Alex Robinson
//! \brief  Collision handler factory class declaration.
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/unordered_set.hpp>

// FRENSIE Includes
#include "MonteCarlo_CollisionHandlerFactory.hpp"
#include "MonteCarlo_AtomicRelaxationModelFactory.hpp"
#include "MonteCarlo_IncoherentModelType.hpp"
#include "MonteCarlo_BremsstrahlungAngularDistributionType.hpp"
#include "MonteCarlo_NuclideFactory.hpp"
#include "MonteCarlo_PhotoatomFactory.hpp"
#include "MonteCarlo_ElectroatomFactory.hpp"
#include "MonteCarlo_AtomicRelaxationModelFactory.hpp"
#include "MonteCarlo_SimulationGeneralProperties.hpp"
#include "MonteCarlo_SimulationNeutronProperties.hpp"
#include "MonteCarlo_SimulationPhotonProperties.hpp"
#include "MonteCarlo_SimulationElectronProperties.hpp"
#include "Utility_ArrayString.hpp"
#include "Utility_ExceptionTestMacros.hpp"
#include "Utility_ExceptionCatchMacros.hpp"
#include "Utility_ContractException.hpp"

namespace MonteCarlo{

// Constructor
CollisionHandlerFactory::CollisionHandlerFactory( std::ostream* os_warn )
  : d_os_warn( os_warn )
{
  // Make sure the output stream is valid
  testPrecondition( os_warn != NULL );
}

// Initialize the collision handler
/*! \details Make sure the simulation properties have been set
 * (in MonteCarlo::SimulationGeneralProperties etc) before running this factory
 * method. The properties can influence how this factory method behaves.
 */
void CollisionHandlerFactory::initializeHandler(
		     const Teuchos::ParameterList& material_reps,
		     const Teuchos::ParameterList& cross_sections_table_info,
		     const std::string& cross_sections_xml_directory )
{
  // Validate the materials
  Teuchos::ParameterList::ConstIterator it = material_reps.begin();

  boost::unordered_set<Geometry::ModuleTraits::InternalCellHandle>
    material_ids;

  while( it != material_reps.end() )
  {
    const Teuchos::ParameterList& material_rep =
      Teuchos::any_cast<Teuchos::ParameterList>( it->second.getAny() );

    CollisionHandlerFactory::validateMaterialRep( material_rep,
						  material_ids );

    ++it;
  }

  material_ids.clear();

  // Validate the material ids
  this->validateMaterialIds( material_reps );

  // Extract the cross section table alias map
  Teuchos::ParameterList alias_map_list;

  try{
    alias_map_list = cross_sections_table_info.sublist( "alias map" );
  }
  EXCEPTION_CATCH_AND_EXIT( std::exception,
			    "Error: The cross_sections.xml file in "
			    << cross_sections_xml_directory <<
			    " is invalid - the 'alias_map' ParameterList "
			    "is not defined!" );

  // Create the set of all nuclides/atoms needed to construct materials
  boost::unordered_set<std::string> aliases;

  CollisionHandlerFactory::createAliasSet( material_reps,
					   alias_map_list,
					   aliases );

  // Create the material id data maps
  boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                       Teuchos::Array<double> > material_id_fraction_map;
  boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                       Teuchos::Array<std::string> > material_id_component_map;

  CollisionHandlerFactory::createMaterialIdDataMaps(
						   material_reps,
						   material_id_fraction_map,
						   material_id_component_map );

  // Create the cell id data maps
  boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
		       std::vector<std::string> > cell_id_mat_id_map;

  boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
		       std::vector<std::string> > cell_id_density_map;

  this->createCellIdDataMaps( cell_id_mat_id_map, cell_id_density_map );

  // Initialize an atomic relaxation model factory
  Teuchos::RCP<AtomicRelaxationModelFactory> atomic_relaxation_model_factory(
					    new AtomicRelaxationModelFactory );

  // Load the cross section data
  switch( SimulationGeneralProperties::getParticleMode() )
  {
  case NEUTRON_MODE:
  {
    this->createNeutronMaterials( cross_sections_table_info,
				  cross_sections_xml_directory,
				  material_id_fraction_map,
				  material_id_component_map,
				  aliases,
				  cell_id_mat_id_map,
				  cell_id_density_map,
				  false,
				  false );
    break;
  }
  case PHOTON_MODE:
  {
    this->createPhotonMaterials(
		     cross_sections_table_info,
		     cross_sections_xml_directory,
		     material_id_fraction_map,
		     material_id_component_map,
		     aliases,
		     cell_id_mat_id_map,
		     cell_id_density_map,
		     atomic_relaxation_model_factory,
		     SimulationPhotonProperties::getNumberOfPhotonHashGridBins(),
		     SimulationPhotonProperties::getIncoherentModelType(),
		     SimulationPhotonProperties::getKahnSamplingCutoffEnergy(),
		     SimulationPhotonProperties::isDetailedPairProductionModeOn(),
		     SimulationPhotonProperties::isAtomicRelaxationModeOn(),
		     SimulationPhotonProperties::isPhotonuclearInteractionModeOn() );
    break;
  }
  case NEUTRON_PHOTON_MODE:
  {
    *d_os_warn << "Warning: Neutron-Photon mode is not fully supported!"
	      << std::endl;

    this->createNeutronMaterials( cross_sections_table_info,
				  cross_sections_xml_directory,
				  material_id_fraction_map,
				  material_id_component_map,
				  aliases,
				  cell_id_mat_id_map,
				  cell_id_density_map,
				  false,
				  true );

    this->createPhotonMaterials(
		     cross_sections_table_info,
		     cross_sections_xml_directory,
		     material_id_fraction_map,
		     material_id_component_map,
		     aliases,
		     cell_id_mat_id_map,
		     cell_id_density_map,
		     atomic_relaxation_model_factory,
		     SimulationPhotonProperties::getNumberOfPhotonHashGridBins(),
		     SimulationPhotonProperties::getIncoherentModelType(),
		     SimulationPhotonProperties::getKahnSamplingCutoffEnergy(),
		     SimulationPhotonProperties::isDetailedPairProductionModeOn(),
		     SimulationPhotonProperties::isAtomicRelaxationModeOn(),
		     SimulationPhotonProperties::isPhotonuclearInteractionModeOn() );
    break;
  }
  case ELECTRON_MODE:
  {
    this->createElectronMaterials(
		     cross_sections_table_info,
		     cross_sections_xml_directory,
		     material_id_fraction_map,
		     material_id_component_map,
		     aliases,
		     cell_id_mat_id_map,
		     cell_id_density_map,
		     atomic_relaxation_model_factory,
		     SimulationElectronProperties::getNumberOfElectronHashGridBins(),
		     SimulationElectronProperties::getBremsstrahlungAngularDistributionFunction(),
		     SimulationElectronProperties::isAtomicRelaxationModeOn(),
		     SimulationElectronProperties::getElasticCutoffAngle() );
    break;
  }
  default:
    THROW_EXCEPTION( std::logic_error,
		     "Error: " << SimulationGeneralProperties::getParticleMode() <<
		     " is not currently supported!" );
  }
}

// Validate a material representation
void CollisionHandlerFactory::validateMaterialRep(
	      const Teuchos::ParameterList& material_rep,
	      boost::unordered_set<Geometry::ModuleTraits::InternalCellHandle>&
	      material_ids )
{
  // Make sure the id is present
  TEST_FOR_EXCEPTION( !material_rep.isParameter( "Id" ),
		      InvalidMaterialRepresentation,
		      "Error: a material must have an id specified!" );

  // Make sure the id is unique
  TEST_FOR_EXCEPTION( material_ids.find( material_rep.get<unsigned>( "Id" ) )!=
		      material_ids.end(),
		      InvalidMaterialRepresentation,
		      "Error: a materials id must be unique (material id "
		      << material_rep.get<unsigned>( "Id" ) <<
		      " appears more than once)!" );

  material_ids.insert( material_rep.get<unsigned>( "Id" ) );

  // Make sure the isotopes that make up the material are specified
  TEST_FOR_EXCEPTION( !material_rep.isParameter( "Isotopes" ),
		      InvalidMaterialRepresentation,
		      "Error: a material must have isotopes specified!" );

  // Make sure the isotope fractions are specified
  TEST_FOR_EXCEPTION( !material_rep.isParameter( "Fractions" ),
		      InvalidMaterialRepresentation,
		      "Error: a material must have isotope fractions "
		      "specified!" );
}

// Create the set of all nuclides/atoms needed to construct materials
void CollisionHandlerFactory::createAliasSet(
		       const Teuchos::ParameterList& material_reps,
		       const Teuchos::ParameterList& cross_sections_alias_map,
		       boost::unordered_set<std::string>& nuclides )
{
  Teuchos::ParameterList::ConstIterator it = material_reps.begin();

  while( it != material_reps.end() )
  {
    const Teuchos::ParameterList& material_rep =
      Teuchos::any_cast<Teuchos::ParameterList>( it->second.getAny() );

    const Teuchos::Array<std::string>& material_isotopes =
      material_rep.get<Teuchos::Array<std::string> >( "Isotopes" );

    for( unsigned i = 0; i < material_isotopes.size(); ++i )
    {
      // The name is a key - store the mapped name
      if( cross_sections_alias_map.isParameter( material_isotopes[i] ) )
      {
	std::string mapped_alias;
	try{
	  mapped_alias =
	    cross_sections_alias_map.get<std::string>( material_isotopes[i] );
        }
	EXCEPTION_CATCH_AND_EXIT( Teuchos::Exceptions::InvalidParameter,
				  "Error: cross section alias map entry "
				  << material_isotopes[i] <<
				  "is invalid! Please fix this entry." );

	nuclides.insert( mapped_alias );
      }
      // The name is not a key - store the name
      else
	nuclides.insert( material_isotopes[i] );
    }

    ++it;
  }
}

// Create the material id data maps
void CollisionHandlerFactory::createMaterialIdDataMaps(
     const Teuchos::ParameterList& material_reps,
     boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                          Teuchos::Array<double> >& material_id_fraction_map,
     boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                       Teuchos::Array<std::string> >& material_id_component_map )
{
  Teuchos::ParameterList::ConstIterator it = material_reps.begin();

  while( it != material_reps.end() )
  {
    const Teuchos::ParameterList& material_rep =
      Teuchos::any_cast<Teuchos::ParameterList>( it->second.getAny() );

    const Utility::ArrayString& array_string =
      material_rep.get<Utility::ArrayString>( "Fractions" );

    Teuchos::Array<double> material_fractions;

    try{
      material_fractions = array_string.getConcreteArray<double>();
    }
    EXCEPTION_CATCH_RETHROW_AS( Teuchos::InvalidArrayStringRepresentation,
				InvalidMaterialRepresentation,
				"Error: The fractions requested for "
				"material " << material_rep.name() <<
				" are not valid!" );

    const Teuchos::Array<std::string>& material_isotopes =
      material_rep.get<Teuchos::Array<std::string> >( "Isotopes" );

    TEST_FOR_EXCEPTION( material_fractions.size() != material_isotopes.size(),
			InvalidMaterialRepresentation,
			"Error: The number of fractions does not "
			"equal the number of isotopes in material "
			<< material_rep.name() << "!" );

    material_id_fraction_map[material_rep.get<unsigned>( "Id" )] =
      material_fractions;

    material_id_component_map[material_rep.get<unsigned>( "Id" )] =
      material_isotopes;

    ++it;
  }
}

// Create the neutron materials
void CollisionHandlerFactory::createNeutronMaterials(
   const Teuchos::ParameterList& cross_sections_table_info,
   const std::string& cross_sections_xml_directory,
   const boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                            Teuchos::Array<double> >& material_id_fraction_map,
   const boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                      Teuchos::Array<std::string> >& material_id_component_map,
   const boost::unordered_set<std::string>& nuclide_aliases,
   const boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
                              std::vector<std::string> >& cell_id_mat_id_map,
   const boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
                               std::vector<std::string> >& cell_id_density_map,
   const bool use_unresolved_resonance_data,
   const bool use_photon_production_data )
{
  // Load the nuclides of interest
  NuclideFactory nuclide_factory( cross_sections_xml_directory,
				  cross_sections_table_info,
				  nuclide_aliases,
				  use_unresolved_resonance_data,
				  use_photon_production_data,
				  d_os_warn );

  boost::unordered_map<std::string,Teuchos::RCP<Nuclide> > nuclide_map;

  nuclide_factory.createNuclideMap( nuclide_map );

  // Create the material name data maps
  boost::unordered_map<std::string,Teuchos::RCP<NeutronMaterial> >
    material_name_pointer_map;

  boost::unordered_map<std::string,
                   Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle> >
    material_name_cell_ids_map;

  CollisionHandlerFactory::createMaterialNameDataMaps(
						  material_id_fraction_map,
						  material_id_component_map,
						  nuclide_map,
						  cell_id_mat_id_map,
						  cell_id_density_map,
						  material_name_pointer_map,
						  material_name_cell_ids_map );

  // Register materials with the collision handler
  CollisionHandlerFactory::registerMaterials( material_name_pointer_map,
					      material_name_cell_ids_map );
}

// Create the photon materials
void CollisionHandlerFactory::createPhotonMaterials(
   const Teuchos::ParameterList& cross_sections_table_info,
   const std::string& cross_sections_xml_directory,
   const boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                            Teuchos::Array<double> >& material_id_fraction_map,
   const boost::unordered_map<ModuleTraits::InternalMaterialHandle,
                      Teuchos::Array<std::string> >& material_id_component_map,
   const boost::unordered_set<std::string>& photoatom_aliases,
   const boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
                              std::vector<std::string> >& cell_id_mat_id_map,
   const boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
                               std::vector<std::string> >& cell_id_density_map,
   const Teuchos::RCP<AtomicRelaxationModelFactory>&
   atomic_relaxation_model_factory,
   const unsigned hash_grid_bins,
   const IncoherentModelType incoherent_model,
   const double kahn_sampling_cutoff_energy,
   const bool use_detailed_pair_production_data,
   const bool use_atomic_relaxation_data,
   const bool use_photonuclear_data )
{
  boost::unordered_map<std::string,Teuchos::RCP<Photoatom> > photoatom_map;

  // Load the photonuclides of interest
  if( use_photonuclear_data )
  {
    THROW_EXCEPTION( std::logic_error,
		     "Error: Photonuclear data is not currently supported!" );
  }
  // Load the photoatoms of interest
  else
  {
    PhotoatomFactory photoatom_factory( cross_sections_xml_directory,
					cross_sections_table_info,
					photoatom_aliases,
					atomic_relaxation_model_factory,
					hash_grid_bins,
					incoherent_model,
					kahn_sampling_cutoff_energy,
					use_detailed_pair_production_data,
					use_atomic_relaxation_data,
					d_os_warn );

    photoatom_factory.createPhotoatomMap( photoatom_map );
  }

  // Create the material name data maps
  boost::unordered_map<std::string,Teuchos::RCP<PhotonMaterial> >
    material_name_pointer_map;

  boost::unordered_map<std::string,
                   Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle> >
    material_name_cell_ids_map;

  CollisionHandlerFactory::createMaterialNameDataMaps(
						  material_id_fraction_map,
						  material_id_component_map,
						  photoatom_map,
						  cell_id_mat_id_map,
						  cell_id_density_map,
						  material_name_pointer_map,
						  material_name_cell_ids_map );

  // Register materials with the collision handler
  CollisionHandlerFactory::registerMaterials( material_name_pointer_map,
					      material_name_cell_ids_map );
}

// Create the electron materials
void CollisionHandlerFactory::createElectronMaterials(
   const Teuchos::ParameterList& cross_sections_table_info,
   const std::string& cross_sections_xml_directory,
   const boost::unordered_map<ModuleTraits::InternalMaterialHandle,
   Teuchos::Array<double> >& material_id_fraction_map,
   const boost::unordered_map<ModuleTraits::InternalMaterialHandle,
   Teuchos::Array<std::string> >& material_id_component_map,
   const boost::unordered_set<std::string>& electroatom_aliases,
   const boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
   std::vector<std::string> >& cell_id_mat_id_map,
   const boost::unordered_map<Geometry::ModuleTraits::InternalCellHandle,
   std::vector<std::string> >& cell_id_density_map,
   const Teuchos::RCP<AtomicRelaxationModelFactory>&
    atomic_relaxation_model_factory,
   const unsigned hash_grid_bins,
   const BremsstrahlungAngularDistributionType photon_distribution_function,
   const bool use_atomic_relaxation_data,
   const double cutoff_angle )
{
  boost::unordered_map<std::string,Teuchos::RCP<Electroatom> > electroatom_map;

  ElectroatomFactory electroatom_factory( cross_sections_xml_directory,
                                          cross_sections_table_info,
					                      electroatom_aliases,
                                          atomic_relaxation_model_factory,
                                          hash_grid_bins,
                                          photon_distribution_function,
                                          use_atomic_relaxation_data,
                                          cutoff_angle,
					  d_os_warn );

  electroatom_factory.createElectroatomMap( electroatom_map );

  // Create the material name data maps
  boost::unordered_map<std::string,Teuchos::RCP<ElectronMaterial> >
    material_name_pointer_map;

  boost::unordered_map<std::string,
                   Teuchos::Array<Geometry::ModuleTraits::InternalCellHandle> >
    material_name_cell_ids_map;

  CollisionHandlerFactory::createMaterialNameDataMaps(
						  material_id_fraction_map,
						  material_id_component_map,
						  electroatom_map,
						  cell_id_mat_id_map,
						  cell_id_density_map,
						  material_name_pointer_map,
						  material_name_cell_ids_map );

  // Register materials with the collision handler
  CollisionHandlerFactory::registerMaterials( material_name_pointer_map,
                                              material_name_cell_ids_map );
}

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// end MonteCarlo_CollisionHandlerFactory.cpp
//---------------------------------------------------------------------------//