#include "constants.h"
#include "pluginHeader.h"

#include <boost/algorithm/string.hpp> // starts_with
#include <cstdlib>                    // atoi
#include <cstring>                    /* strcpy  */
#include <fstream>                    // ifstream, ofstream
#include <iomanip>                    // setw
#include <iostream>

struct Matrix
{
    int width;
    int height;
    float* elements;
};

// Matrix functions
Matrix* readMatrixFile(const char* filename);
int writeMatrixFile(const char* filename, const Matrix* matrix);
void displayMatrix(const Matrix* matrix);
void deleteMatrix(Matrix*& matrix);

// matrixMulOnDevice is defined in "matrix.cu"
extern void matrixMulOnDevice(const Matrix* A, const Matrix* B, Matrix* C);
void matrixMulOnHost(const Matrix* A, const Matrix* B, Matrix* C);

enum inputs
{
    INPUTA,
    INPUTB,
    OUTPUT,
    DISPLAY
};

/* globals */
char params[][256] = {"inputFileMatrixA.txt", "inputFileMatrixB.txt", "OutputFileMatrixC.txt", "0"};
const int NUM_ARGS = sizeof params / sizeof params[0];
clock_t total_t = 0;
const char* PARAM_INFO = "inputFileMatrixA,inputFileMatrixB,OutputFileMatrixC,displayResult";

/******************************************************************************
 * main()
 * - this function is not used by the plugin, but this can be built as
 *   a standalone executable
 ******************************************************************************/
int main(int argc, char** argv)
{
    if (argc == 1)
    {
        std::cout << "Usage: " << argv[0] << " inputFileA inputFileB outputFileC displayResult\n";
        return 0;
    }

    if (argc > 1)
    {
        if (strcmp(argv[1], "--help") == 0)
        {
            displayPluginInfo();
            return 0;
        }
        strcpy(params[INPUTA], argv[1]);
    }

    if (argc > 2)
        strcpy(params[INPUTB], argv[2]);

    if (argc > 3)
        strcpy(params[OUTPUT], argv[3]);

    if (argc > 4)
        strcpy(params[DISPLAY], argv[4]);

    return run();
}

///////////////////////////// MATRIX FUNCTIONS ////////////////////////////////

/******************************************************************************
 * TODO: implement me
 * matrixMulOnHost
 * - does the computation in serial, using the CPU
 ******************************************************************************/
void matrixMulOnHost(const Matrix* A, const Matrix* B, Matrix* C) {}

/******************************************************************************
 * readMatrixFile()
 * - opens the file for reading
 * - allocates the matrix, read the elements, return the allocated matrix
 ******************************************************************************/
Matrix* readMatrixFile(const char* filename)
{
    // open the file
    std::ifstream fin(filename);
    if (!fin)
        return nullptr;

    int rows = 0;
    int cols = 0;
    float* elements = nullptr;
    Matrix* matrix = nullptr;
    std::string line;

    // read the 1st 2 lines ~ should contain "rows=" and "cols="
    for (int i = 0; i < 2; i++)
    {
        getline(fin, line);
        if (boost::starts_with(line, "rows="))
            rows = atoi(line.c_str() + 5);
        else if (boost::starts_with(line, "cols="))
            cols = atoi(line.c_str() + 5);
    }

    if (rows <= 0 || cols <= 0)
        return nullptr;

    elements = new float[rows * cols](); // allocate the array of elements

    // loop, reading al the elements into the array
    int i = 0;
    while (fin >> elements[i++] && (i < (rows * cols)))
        ;

    fin.close();

    // allocate and assign the matrix items
    matrix = new Matrix;
    matrix->width = cols;
    matrix->height = rows;
    matrix->elements = elements;

    return matrix;
}

/******************************************************************************
 * writeMatrix()
 * - writes the matrix to the file
 ******************************************************************************/
int writeMatrixFile(const char* filename, const Matrix* matrix)
{
    // open the file
    std::ofstream fout(filename);
    if (!fout || !matrix)
    {
        return ERROR;
    }

    // write the 1st couple of header lines
    fout << "rows=" << matrix->height << std::endl << "cols=" << matrix->width << std::endl << std::endl;

    int padding = 4; // TODO: dynamically figure out the padding size

    for (int row = 0; row < matrix->height; ++row)
    {
        for (int col = 0; col < matrix->width; ++col)
            fout << std::setw(padding) << matrix->elements[row * matrix->width + col] << " ";
        fout << std::endl;
    }
    fout.close();
    return OK;
}

/******************************************************************************
 * displays the Matrix elements
 ******************************************************************************/
void displayMatrix(const Matrix* matrix)
{
    if (matrix)
    {
        int row;
        int col;
        for (row = 0; row < matrix->height; ++row)
        {
            for (col = 0; col < matrix->width; ++col)
                std::cout << matrix->elements[row * matrix->width + col] << " ";
            std::cout << std::endl;
        }
    }
}

/******************************************************************************
 * deallocates the matrix elements
 ******************************************************************************/
void deleteMatrix(Matrix*& matrix)
{
    if (matrix)
    {
        if (matrix->elements)
        {
            delete[] matrix->elements;
            matrix->elements = nullptr;
        }
        matrix = nullptr;
    }
}

///////////////////////////// PLUGIN FUNCTIONS ////////////////////////////////

/******************************************************************************
 * run()
 * - computes the matrix multiplication: C = A * B
 * - reads the input files from the parameters: A = params[0], B = params[1]
 * - writes the output result C to the file set in params[2]
 * - if params[3] is set, then display the result to the screen
 ******************************************************************************/
int run()
{
    clock_t start_t;
    clock_t end_t;
    total_t = 0; // reset the clock counter

    // open and allocate the input Matrix Files
    Matrix* A = readMatrixFile(params[INPUTA]);
    Matrix* B = readMatrixFile(params[INPUTB]);
    if (!A || !B)
    {
        std::cout << "index: " << params[!A ? 0 : 1] << std::endl;
        std::cout << "Failed to open Matrix file: \"" << params[!A ? 0 : 1] << "\"\n";
        return ERROR;
    }

    if (A->width != B->height) // C = A * B = (example): 3x4 * 4x5 = 3x5
    {
        std::cout << "Invalid matrix dimensions!\n"
                  << "Can't multiply " << A->height << 'x' << A->width << " * " << B->height << 'x' << B->width
                  << std::endl;
        return ERROR;
    }

    // allocate the output Matrix
    Matrix* C = nullptr;
    C = new Matrix;
    C->height = A->height;
    C->width = B->width;
    C->elements = new float[C->height * C->width]();

#ifdef DEBUG
    std::cout << "Multiplying C = A * B\n";
    std::cout << "A:\n";
    displayMatrix(A);
    std::cout << "B:\n";
    displayMatrix(B);
#endif

    // compute the result using the GPU
    start_t = clock();
    matrixMulOnDevice(A, B, C); // using CUDA
    end_t = clock();
    total_t = end_t - start_t;

    // write the result to the file
    if (writeMatrixFile(params[OUTPUT], C) == ERROR)
    {
        std::cout << "Failed to write Matrix to: \"" << params[OUTPUT] << "\"\n";
    }

    // check if the result should be displayed
    if (atoi(params[DISPLAY])) // params[DISPLAY] => "0" or "1"
    {
        std::cout << "result: \n";
        displayMatrix(C);
    }

    // no memory leaks
    deleteMatrix(A);
    deleteMatrix(B);
    deleteMatrix(C);

    return OK;
}

/******************************************************************************
 * setParams()
 * - input: DELIM separated buffer
 * - splits the buffer and sets the "params" to the new passed in parameters
 * - this function only works if all the parameters are passed in to be set
 ******************************************************************************/
int setParams(const char* buffer)
{
    int bufferSize = strlen(buffer) + 1; // +1 for '\0'

    // if the buffer is empty, then just return
    if (bufferSize == 1 && NUM_ARGS == 0)
        return OK;

    // check if the passed in buffer will fit in our params
    if (bufferSize > NUM_ARGS * BUFFER_SIZE)
        return ERROR;

    // count the number of arguments by counting the number of delimiters
    int commas = 0;
    const char* p;
    for (p = buffer; *p; p++)
        if (*p == DELIM)
            commas++;

    if (commas + 1 == NUM_ARGS)
    {
        // make a copy of the input buffer (so strtok doesnt change the original)
        char buf[bufferSize];
        strcpy(buf, buffer);
        char* arg;
        int i;

        // copy the arguments to the params array
        arg = strtok(buf, DELIM_STR);
        for (i = 0; i < NUM_ARGS && arg; ++i)
        {
            strcpy(params[i], arg);
            arg = strtok(nullptr, DELIM_STR);
        }

        return OK; // OK
    }
    return ERROR; // NOT_OK
}

/******************************************************************************
 * getParams()
 * - builds a DELIM delimited string of the current parameter values
 * - sets the "buffer" input to the newly built cstring
 ******************************************************************************/
int getParams(char* buffer, int bufferSize)
{
    int i;

    // count the size needed
    int size = 0;
    for (i = 0; i < NUM_ARGS; i++)
        size += strlen(params[i]);
    size += NUM_ARGS - 1;

    // the size of the array must be big enough
    if (bufferSize < size || bufferSize < 1)
        return ERROR; // ERROR

    // join the arguments into a DELIM separated list
    buffer[0] = '\0';
    for (i = 0; i < NUM_ARGS; i++)
    {
        strcat(buffer, params[i]);
        strcat(buffer, DELIM_STR); // NOTE: could use DC1 as delimeter
    }
    buffer[size] = '\0'; // delete the last inserted comma
    return OK;           // OK
}

/******************************************************************************
 * returns info / help on how to use this plugin
 ******************************************************************************/
void* displayPluginInfo()
{
    std::cout << "The Matrix plugin is intended to quickly compute the multiplication of matrices.\n"
              << "The paramaters that can be set are as follows:\n"
              << "\t* inputFileMatrixA - the (left) matrix file operand\n"
              << "\t* inputFileMatrixB - the (right) matrix file operand\n"
              << "\t* inputFileMatrixC - the resulting matrix file (C = A * B)\n"
              << "\t* displayResult - this option can be set to '0' or '1'.\n "
              << "                         if set to true, the result of the operation will be\n"
              << "                          displayed on the screen when done computing.\n"
              << "Note: the input files follow a very specific type of format.\n"
              << "They must list the number of rows and cols, followed by a blank line.\n"
              << "Following that, each matrix row should be located in a single line.\n";

    return nullptr;
}

/******************************************************************************
 * returns a comma-separated list of the parameter names
 ******************************************************************************/
const char* getParamInfo()
{
    return PARAM_INFO;
}

/******************************************************************************
 * returns the number of arguments this plugin contains
 ******************************************************************************/
const int getNumArgs()
{
    return NUM_ARGS;
}

/******************************************************************************
 * returns how many milliseconds it took to run the main program
 ******************************************************************************/
clock_t getRunTime()
{
    return total_t;
}


#include <CAN/PCANDevice.h>
#include <CAN/Registers.h>
#include <CAN/RealTimeTask.hpp>
#include <string.h>
#include <iostream>
#include <unistd.h>
#include <cmath>

#include <NomadBLDC/NomadBLDC.h>
#include <nlohmann/json.hpp>
#include <zmq.hpp>

// Dart
#include <dart/dynamics/Skeleton.hpp>
#include <dart/dynamics/FreeJoint.hpp>
#include <dart/dynamics/WeldJoint.hpp>
#include <dart/dynamics/BallJoint.hpp>
#include <dart/dynamics/BoxShape.hpp>
#include <dart/simulation/World.hpp>
#include <dart/utils/urdf/DartLoader.hpp>

#include <dart/gui/osg/osg.hpp>

#define DEVICE "/dev/pcanusbfd32"

PCANDevice can;

double q1_ref = 0.0f;
double q2_ref = 0.0f;
double frequency = 1.0f;
double diameter = 0.05f;

double home = 0.0308f;
double home2 = -0.312458f;

Eigen::Vector2d foot_pos_des_;
Eigen::Vector2d foot_vel_des_;

Eigen::Vector2d k_P(2500, 2500);
Eigen::Vector2d k_D(20, 20);

typedef enum
{
    Zero = 0,
    Homing = 1,
    PreStand = 2,
    Stand = 3,
    Crouch = 4
} FSMState_e;

FSMState_e current_state_ = Zero;
int debounce = 0;

bool AlmostEquals(double A, double B, double epsilon = 0.005f)
{
    return (fabs(A - B) < epsilon);
}

#include <Eigen/Dense>

// Project Include Files
namespace Common
{
    class CubicPolynomialTrajectory
    {

    public:
        CubicPolynomialTrajectory(double q_f, double t_f);
        CubicPolynomialTrajectory(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f);
        CubicPolynomialTrajectory(); // Empty Trajectory

        void Generate(double q_f, double t_f);
        void Generate(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f);

        // TODO: Check for valid t between 0<->t_f
        double Position(double t);
        double Velocity(double t);
        double Acceleration(double t);

    protected:
        void ComputeCoeffs();

        Eigen::Vector4d a_; // Coefficients

        double q_0_;
        double v_0_;
        double t_0_;

        double q_f_;
        double v_f_;
        double t_f_;
    };
} // namespace Common

namespace Common
{
    CubicPolynomialTrajectory::CubicPolynomialTrajectory(double q_f, double t_f)
        : q_0_(0.0), q_f_(q_f), v_0_(0.0), v_f_(0.0), t_0_(0.0), t_f_(t_f)
    {
        // Compute Coefficients
        ComputeCoeffs();
    }
    CubicPolynomialTrajectory::CubicPolynomialTrajectory(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f)
        : q_0_(q_0), q_f_(q_f), v_0_(v_0), v_f_(v_f), t_0_(t_0), t_f_(t_f)
    {
        // Compute Coefficients
        ComputeCoeffs();
    }

    CubicPolynomialTrajectory::CubicPolynomialTrajectory() // Empty Trajectory
        : q_0_(0.0), q_f_(0.0), v_0_(0.0), v_f_(0.0), t_0_(0.0), t_f_(0.0)
    {
        // Compute Coefficients
        ComputeCoeffs();
    }

    void CubicPolynomialTrajectory::Generate(double q_f, double t_f)
    {
        q_0_ = 0.0;
        q_f_ = q_f;
        v_0_ = 0.0;
        v_f_ = 0.0;
        t_0_ = 0.0;
        t_f_ = t_f;

        ComputeCoeffs();
    }

    void CubicPolynomialTrajectory::Generate(double q_0, double q_f, double v_0, double v_f, double t_0, double t_f)
    {
        q_0_ = q_0;
        q_f_ = q_f;
        v_0_ = v_0;
        v_f_ = v_f;
        t_0_ = t_0;
        t_f_ = t_f;

        ComputeCoeffs();
    }

    // TODO: Check for valid t between 0<->t_f
    double CubicPolynomialTrajectory::Position(double t)
    {
        // Trim Position
        // TODO: Function for this trimming/mapping
        double t_eval = std::min(t, t_f_);
        t_eval = std::max(t_eval, t_0_);
        return a_(0) + a_(1) * t_eval + a_(2) * t_eval * t_eval + a_(3) * t_eval * t_eval * t_eval;
    }
    double CubicPolynomialTrajectory::Velocity(double t)
    {
        // Trim Position
        // TODO: Function for this trimming/mapping
        double t_eval = std::min(t, t_f_);
        t_eval = std::max(t_eval, t_0_);
        return a_(1) + 2 * a_(2) * t_eval + 3 * a_(3) * t_eval * t_eval;
    }
    double CubicPolynomialTrajectory::Acceleration(double t)
    {
        // Trim Position
        // TODO: Function for this trimming/mapping
        double t_eval = std::min(t, t_f_);
        t_eval = std::max(t_eval, t_0_);
        return 2 * a_(2) + 6 * a_(3) * t_eval;
    }

    void CubicPolynomialTrajectory::ComputeCoeffs()
    {

        Eigen::Matrix4d C; // Constraints
        C << 1, t_0_, t_0_ * t_0_, t_0_ * t_0_ * t_0_,
            0, 1, 2 * t_0_, 3 * t_0_ * t_0_,
            1, t_f_, t_f_ * t_f_, t_f_ * t_f_ * t_f_,
            0, 1, 2 * t_f_, 3 * t_f_ * t_f_;

        Eigen::Vector4d b;
        b << q_0_, v_0_, q_f_, v_f_;

        // Solve for Coefficients
        a_ = C.lu().solve(b);
    }
} // namespace Common

 Common::CubicPolynomialTrajectory com_traj_;

class LegTest : public Realtime::RealTimeTaskNode
{

public:
    // Block Diagram Class For Systems Task Node
    // name = Task Name
    // T_s = Sample Time (-1 for inherit)
    LegTest(const std::string &name, const double T_s = -1);
    virtual void Exit();

protected:
    // Overriden Run Function
    virtual void Run();

    // Pre-Run Setup Routine.  Setup any one time initialization here.
    virtual void Setup();

    // TODO: Load Dart URDF
    void SetupDART();

    // Time
    double time_;

    // NOMAD Servos
    NomadBLDC *servo1;
    NomadBLDC *servo2;

    // Zero MQ Context for PlotJuggler
    zmq::context_t ctx;
    zmq::socket_t *publisher;

    // DART Objects
    dart::dynamics::SkeletonPtr robot_;

    dart::dynamics::DegreeOfFreedomPtr hfe;
    dart::dynamics::DegreeOfFreedomPtr kfe;

    dart::dynamics::BodyNodePtr hip_body_;
    dart::dynamics::BodyNodePtr foot_body_;

    Eigen::Vector2d foot_pos_;
    Eigen::Vector2d foot_vel_;

    Eigen::Vector2d q_;
    Eigen::Vector2d qd_;
    Eigen::Vector2d qdd_;

    Eigen::MatrixXd J_; // Leg Jacobian

    Eigen::Matrix2d k_P_cartesian_; // Cartesian P Gains
    Eigen::Matrix2d k_D_cartesian_; // Cartesian D Gains

    Eigen::Vector2d force_output = Eigen::Vector2d::Zero();

    Eigen::VectorXd torque_output;
};

LegTest::LegTest(const std::string &name, const double T_s) : Realtime::RealTimeTaskNode(name, T_s, Realtime::Priority::HIGH, -1, PTHREAD_STACK_MIN), time_(0.0)
{
}

void LegTest::SetupDART()
{
    // Load URDF
    dart::utils::DartLoader loader;
    std::string urdf = std::getenv("NOMAD_RESOURCE_PATH");
    urdf.append("/LegTest/Leg.urdf");

    robot_= loader.parseSkeleton(urdf);
    robot_->getDof("j_hfe")->setPosition(0.0f);
    robot_->getDof("j_kfe")->setPosition(0.0f);

    //nomad->getJoint("j_kfe")->setPositionLimitEnforced(true);
    //robot_->getDof("j_kfe")->setDampingCoefficient(1.5f);
    //robot_->getDof("j_hfe")->setDampingCoefficient(1.5f);
    //nomad->getDof("j_kfe")->setPositionLimits(0.0f, 2.0);

    robot_->getDof("slider")->setPositionLimits(-0.0, 0.0);
    robot_->getJoint("slider")->setPositionLimitEnforced(true);

    hfe = robot_->getDof("j_hfe");
    kfe = robot_->getDof("j_kfe");

    hip_body_ = robot_->getBodyNode("HFE_Actuator1");
    foot_body_ = robot_->getBodyNode("Foot1");

    // Fix our base link (the stand)
    robot_->getRootBodyNode()->moveTo<dart::dynamics::WeldJoint>(nullptr);

    robot_->computeForwardKinematics();
    robot_->computeForwardDynamics();

    // Set Friction
    //std::cout << nomad->getBodyNode("foot")->getFrictionCoeff() << std::endl;
    //nomad->getBodyNode("foot")->setFrictionCoeff(10.8);
}
void LegTest::Run()
{
    auto start_time = std::chrono::high_resolution_clock::now();
    time_ += dt_actual_;

    q_[0] = servo1->GetPosition();// + q1_ref;
    qd_[0] = servo1->GetVelocity();
    qdd_[0]= servo1->GetTorque();

    q_[1] = servo2->GetPosition();// + q2_ref;
    qd_[1] = servo2->GetVelocity();
    qdd_[1]= servo2->GetTorque();

    robot_->setPosition(0, 0);
    robot_->setVelocity(0, 0);
    robot_->setForce(0, 0);

    robot_->setPosition(1, q_[0]);
    robot_->setVelocity(1, qd_[0]);
    robot_->setForce(1, qdd_[0]);

    robot_->setPosition(2, q_[1]);
    robot_->setVelocity(2, qd_[1]);
    robot_->setForce(2, qdd_[1]);

    robot_->computeForwardKinematics();
    robot_->computeForwardDynamics();

    J_ = robot_->getLinearJacobian(foot_body_, hip_body_);
    J_ = J_.bottomRows(2);

    foot_pos_ = foot_body_->getTransform(hip_body_).translation().tail(2);
    foot_vel_ = (J_ * robot_->getVelocities()).tail(2);

    Eigen::Vector3d torque;
    double home_target = 0.2f;

    switch (current_state_)
    {
    case FSMState_e::Homing:
        //std::cout << " STATE HOMING " << std::endl;
            servo1->ClosedLoopTorqueCommand(20.0f, 0.75f, home_target, 0.0f, 0.0f);
            servo2->ClosedLoopTorqueCommand(20.0f, 0.75f, 0.0f, 0.0f, 0.0f);

            if(AlmostEquals(q_[0], home_target, 0.1f))
            {
               // std::cout << "Arrived!" << std::endl;
                debounce++;

                if(debounce > 1000)
                {
                   // std::cout << "NEXT STATE!" << std::endl;
                   time_ = 0.0f;
                   com_traj_.Generate(foot_pos_[1], -0.42f, 0.0, 0.0, 0.0, 1.0f);
                   current_state_ = FSMState_e::Stand;
                }
            }
            else
            {
                //std::cout << "Test: " << q_[0] << std::endl;
                debounce = 0;
            }
            // Check Position then goto PRESTAND
        break;
    case FSMState_e::Stand:

        foot_pos_des_[1] = com_traj_.Position(time_);
        foot_vel_des_[1] = com_traj_.Velocity(time_);

        foot_pos_des_[0] = 0.01f;
        foot_vel_des_[0] = 0.0f;

        k_P_cartesian_ = k_P.asDiagonal();
        k_D_cartesian_ = k_D.asDiagonal();
        force_output = Eigen::Vector2d::Zero();
        force_output += k_P_cartesian_ * (foot_pos_des_ - foot_pos_);
        force_output += k_D_cartesian_ * (foot_vel_des_ - foot_vel_);

        torque_output = J_.transpose() * force_output + robot_->getGravityForces() * 1.1f;

        servo1->ClosedLoopTorqueCommand(0.0f, 0.0f, 0.0f, 0.0f, torque_output[1]);
        servo2->ClosedLoopTorqueCommand(0.0f, 0.0f, 0.0f, 0.0f, torque_output[2]);

        if (time_ > 1.0f)
        {
            time_ = 0.0f;
            com_traj_.Generate(foot_pos_[1], -0.27f, 0.0, 0.0, 0.0, 0.3f);
            current_state_ = FSMState_e::Crouch;

            std::cout << "GO TO CROUCH" << std::endl;
        }

        break;
    case FSMState_e::Crouch:

        foot_pos_des_[0] = 0.01f;
        foot_vel_des_[0] = 0.0f;

        foot_pos_des_[1] = com_traj_.Position(time_);
        foot_vel_des_[1] = com_traj_.Velocity(time_);

        k_P_cartesian_ = k_P.asDiagonal();
        k_D_cartesian_ = k_D.asDiagonal();
        force_output = Eigen::Vector2d::Zero();
        force_output += k_P_cartesian_ * (foot_pos_des_ - foot_pos_);
        force_output += k_D_cartesian_ * (foot_vel_des_ - foot_vel_);

        torque_output = J_.transpose() * force_output + robot_->getGravityForces() * 1.1f;

        servo1->ClosedLoopTorqueCommand(0.0f, 0.0f, 0.0f, 0.0f, torque_output[1]);
        servo2->ClosedLoopTorqueCommand(0.0f, 0.0f, 0.0f, 0.0f, torque_output[2]);

        if (time_ > 0.6f)
        {
            time_ = 0.0f;
            com_traj_.Generate(foot_pos_[1], -0.42f, 0.0, 0.0, 0.0, 0.27f);
            current_state_ = FSMState_e::Stand;

            std::cout << "GO TO STAND" << std::endl;
        }

        break;
    default:
        std::cout << "DEFAULT" << std::endl;
        break;
    }

   // foot_pos_des_[0] = home;// + diameter * std::cos(2 * M_PI * frequency * time_);
   // foot_pos_des_[1] = home2;// + diameter * std::sin(2 * M_PI * frequency * time_);

  /*  Eigen::Vector2d force_output = Eigen::Vector2d::Zero();
    Eigen::Vector2d tau_output;

    Eigen::Vector2d k_P(600, 600);
    Eigen::Vector2d k_D(2, 2);

    k_P_cartesian_ = k_P.asDiagonal();
    k_D_cartesian_ = k_D.asDiagonal();

    force_output += k_P_cartesian_ * (foot_pos_des_ - foot_pos_);
    force_output += k_D_cartesian_ * (foot_vel_des_ - foot_vel_);

    Eigen::VectorXd torque = J_.transpose() * force_output + robot_->getGravityForces()*1.3f;

    servo1->ClosedLoopTorqueCommand(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);//torque[1]);
    servo2->ClosedLoopTorqueCommand(0.0f, 0.0f, 0.0f, 0.0f, 0.0f);//torque[2]);*/

    // Update DART Here
    nlohmann::json test = {
        {"timestamp", time_},
        {"motor",
            {{"Foot_Pos_1", foot_pos_[0]},
            {"tau_ref_1", torque[1]},
            {"Foot_Des_1",foot_pos_des_[0]},
            {"tau_1", qdd_[0]},
            {"Foot_Pos_2", foot_pos_[1]},
            {"tau_ref_2", torque[2]},
            {"Foot_Des_2", foot_pos_des_[1]},
            {"tau_2", qdd_[1]},
            {"Q_1", q_[0]},
            {"Q_2", q_[1]},}
        }
    };

    auto data = nlohmann::json::to_msgpack(test);
    zmq::message_t message(data.size());
    memcpy(message.data(), &data[0], data.size());
    publisher->send(message, zmq::send_flags::dontwait);

    auto time_now = std::chrono::high_resolution_clock::now();
    auto total_elapsed = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_time).count();
}

void LegTest::Setup()
{
    // TODO: We need some sort of exceptions here if setup fails
    CANDevice::Config_t config;
    config.bitrate = 1e6; //1mbps
    config.d_bitrate = 5e6; //2mbps
    config.sample_point = 0.80f; //87.5%
    config.d_sample_point = 0.625f; //60%
    config.clock_freq = 80e6; // 80mhz // Read from driver?
    config.mode_fd = 1; // FD Mode

    if(!can.Open(DEVICE, config, true))
    {
        std::cout << "Unable to open CAN Device" << std::endl;
        return;
    }

    // Setup Filters
    can.ClearFilters(); // Clear Existing/Reset.  Filters are saved on the device hardware.  Must make sure to clear
    can.AddFilter(1, 2); // Only Listen to messages on id 1.

    servo1 = new NomadBLDC(1, 0x10, &can);
    servo1->SetName("INPUT");
    if(!servo1->Connect())
    {
        std::cout << "[ERROR]: Unable to connect to Nomad Servo!" << std::endl;
        exit(1);
    }

    std::cout << "Nomad Servo: " << "[" << servo1->GetName() << "] : " << servo1->GetServoId() << " Connected!" << std::endl;

    servo2 = new NomadBLDC(2, 0x11, &can);
    servo2->SetName("INPUT2");
    if(!servo2->Connect())
    {
        std::cout << "[ERROR]: Unable to connect to Nomad Servo!" << std::endl;
        exit(1);
    }

    std::cout << "Nomad Servo: " << "[" << servo2->GetName() << "] : " << servo2->GetServoId() << " Connected!" << std::endl;

    // Setup ZMQ
    publisher = new zmq::socket_t(ctx, ZMQ_PUB);
    std::string transport("tcp://*:9872");
    publisher->bind(transport);

    std::cout << "Please Zero Ac`tuator.  Enter to Continue." << std::endl;
    getchar();

    // Load DART
    SetupDART();

    servo1->ZeroOutput();
    usleep(30000);
    servo2->ZeroOutput();
    usleep(30000);

    // Start Motor Control Mode
    usleep(8000000);
    servo1->SetControlMode(PD_MODE);
    servo2->SetControlMode(PD_MODE);

    servo1->ClosedLoopTorqueCommand(0.0, 0.0f, 0.0f, 0.0f, 0.0f);
    servo2->ClosedLoopTorqueCommand(0.0, 0.0f, 0.0f, 0.0f, 0.0f);

    usleep(30000);

    // Get Initial Values
    q_[0] = servo1->GetPosition();
    qd_[0] = servo1->GetVelocity();
    qdd_[0]= servo1->GetTorque();

    q_[1] = servo2->GetPosition();
    qd_[1] = servo2->GetVelocity();
    qdd_[1]= servo2->GetTorque();

    robot_->setPosition(0, 0);
    robot_->setVelocity(0, 0);
    robot_->setForce(0, 0);

    robot_->setPosition(1, q_[0]);
    robot_->setVelocity(1, qd_[0]);
    robot_->setForce(1, qdd_[0]);

    robot_->setPosition(2, q_[1]);
    robot_->setVelocity(2, qd_[1]);
    robot_->setForce(2, qdd_[1]);

    robot_->computeForwardKinematics();
    robot_->computeForwardDynamics();

    J_ = robot_->getLinearJacobian(foot_body_, hip_body_);
    J_ = J_.bottomRows(2);

    foot_pos_ = foot_body_->getTransform(hip_body_).translation().tail(2);
    foot_vel_ = (J_ * robot_->getVelocities()).tail(2);
    foot_pos_des_ = foot_pos_;

    std::cout << "Press Key to Start Control." << std::endl;
    usleep(5000000);
    current_state_ = FSMState_e::Homing;
    std::cout << "Foot Pos Here: " << foot_pos_[0] << ", " << foot_pos_[1] << std::endl;
}

void LegTest::Exit()
{
    std::cout << "Exiting!" << std::endl;

    // Set back to idle.  In theory when no commands are sent it should auto back to idle or edamp?
    servo1->SetControlMode(IDLE_MODE);
    servo2->SetControlMode(IDLE_MODE);
}

int main(int argc, char *argv[])
{
    Realtime::RealTimeTaskManager::Instance();
    if (!Realtime::RealTimeTaskManager::EnableRTMemory(500 * 1024 * 1024)) { // 500MB
        std::cout << "Error configuring Realtime Memory Requiremets!  Realtime Execution NOT guaranteed." << std::endl;
    }
    else {
        std::cout << "Real Time Memory Enabled!" << std::endl;
    }

    LegTest pj_Node("Test", 1/1000.0f); //500hz
    pj_Node.SetStackSize(1024 * 1024);
    pj_Node.SetTaskPriority(Realtime::Priority::HIGHEST);
    pj_Node.SetCoreAffinity(2);
    pj_Node.Start();

    char input;
    std::cin >> input;
    if(input == 'q') {
        std::cout << "Got Q:" << std::endl;
    }

    pj_Node.Exit();
    pj_Node.Stop();
}


#include <iostream>
#include <cmath>
#include <vector>
#include <array>
#include <math.h>
#include "heatEquation.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void printQuadrature();
double f( double x);
double g (double x);
double h (double x);
const double M_PI = 2*acos(0);

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

SpaceMesh smesh;
//smesh.GenerateDefaultSpaceMesh();
//smesh.GloballyBisectSpaceMesh();
smesh.GenerateSpaceMesh({0, 0.15, 0.25, 0.5, 1});

//smesh.GloballyBisectSpaceMesh();
//smesh.GloballyBisectSpaceMesh();

TimeMesh tmesh;
tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize(), 2), 1.0);

HeatEquation heat;
heat.SetSpaceTimeMesh( smesh, tmesh, "soultion1.txt");
heat.Solve();

smesh.PrintSpaceNodes();
heat.PrintSolution();

}

Simple Heat Equation solver class/Driver for simple heat equation classes.cpp


#include <iostream>

#include "LinearStateSpaceModel.h"

#include <Eigen/Core>

int main()
{
	// Define the initial state of the system
	const int n = 2;
	const int q = 1;
	const int p = 1;
	float states[n] = { 1, 2 };
	float input[p] = { 0.6f };
	float state_mtx[n * n] = { 0, 2, -1, -3 };
	float input_mtx[n * p] = { 5, 0 };
	float output_mtx[q * n] = { 1, 0 };
	float feedforward_gain[q * p] = { 0 };

	// Convert input to eigen datatypes
	Eigen::Matrix<float, n, 1> x(states);
	Eigen::Matrix<float, p, 1> u(input);
	Eigen::Matrix<float, n, n, Eigen::RowMajor> A(state_mtx);
	Eigen::Matrix<float, n, p> B(input_mtx);
	Eigen::Matrix<float, q, n> C(output_mtx);
	Eigen::Matrix<float, q, p> D(feedforward_gain);

	// Create state space model
	auto ss = LinearStateSpaceModel<float, n, q, p>(A, B, C, D);
	ss.set_state(x);
	ss.set_input(u);

	// Iterate for 10 loops
	for (int i = 0; i < 10; i++)
	{
		// Calculate
		ss.propogate();

		// Output
		std::cout << "State Estimate @ t = " << (i + 1) << ": \n" << ss.x << std::endl;
		std::cout << "Output Estimate @ t = " << (i + 1) << ": " << ss.y << std::endl;
	}
}


#include <math.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <sstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>

#include "util.h"
#include "gestureDetection.h"

#define DTW_INFINITY 1e30

double maximum(double a, double b, double c) {
	if(a>b && a>c) {
		return a;
	}
	else if(b>a && b>c) {
		return b;
	}
	else return c;
}

double measure_deviation(std::vector<double> arr1, std::vector<double> arr2) {

	/*
		Function to find deviation/difference between two measured vectors having size = 3.

		@params:
		arr1	Input array 1
		arr2	Input array 2
	*/

	return maximum(std::fabs(arr1[0] - arr2[0]), std::fabs(arr1[1] - arr2[1]), std::fabs(arr1[2] - arr2[2]));
}

double minimum(double a, double b, double c) {
	if(a<b && a<c) {
		return a;
	}
	else if(b<a && b<c) {
		return b;
	}
	else return c;
}

double DTWScore(std::vector<std::vector<double> > arr1, std::vector<std::vector<double> > arr2) {

	/*
		Function to estimate how close two measurements are. The closeness is denoted by the score, which is computed
		using the DTW(Dynamic Time Warping) Algorithm. Lower the score, more close they are.

		@params:
		arr1	Input array 1
		arr2	Input array 2
	*/

	int m = arr1.size() - 1;
	int n = arr2.size() - 1;

	double DTW[m+1][n+1], dev;

	for(int i=1; i<=m; i++) {
		DTW[i][0] = DTW_INFINITY;
	}
	for(int j=1; j<=n; j++) {
		DTW[0][j] = DTW_INFINITY;
	}
	DTW[0][0] = 0;

	for(int i=1; i<=m; i++) {
		for(int j=1; j<=n; j++) {
			dev = measure_deviation(arr1[i], arr2[j]);
			DTW[i][j] = dev + minimum(DTW[i-1][j], DTW[i][j-1], DTW[i-1][j-1]);
		}
	}

	//DTW[m][n] is the score.
	return DTW[m][n];
}

void FixedBin::assign(int _size) {
	filled = 0;
	size = _size;
	bin.resize(_size);
}

void FixedBin::push(std::vector<double> vec) {
	if(filled == size) {
		for(int i=0;i<filled-1;i++) {
			bin[i] = bin[i+1];
		}
		bin[filled - 1] = vec;
	}
	else {
		bin.at(filled) = vec;
		++filled;
	}
}

int FixedBin::get_size() {
	return size;
}

int FixedBin::get_filled() {
	return filled;
}

void FixedBin::get(int pos, std::vector<double>& vec) {
	vec.resize(3);
	vec = bin.at(pos);
}

std::vector<std::vector<double> > FixedBin::clone() {
	std::vector<std::vector<double> > vec(filled);

	//vec.empty();
	//vec.resize(filled);
	for(int i=0;i<filled;i++) {
		vec.at(i) = bin.at(i);
	}
	return vec;
}

void FaceGesture::assign(int normal_size) {
	normal = new FixedBin();
	normal->assign(normal_size);
}


#pragma once
#pragma ide diagnostic ignored "modernize-use-nodiscard"
#pragma ide diagnostic ignored "NotImplementedFunctions"
#pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection"
#pragma ide diagnostic ignored "OCUnusedStructInspection"
#pragma ide diagnostic ignored "OCUnusedTypeAliasInspection"

#include <Eigen/Geometry>

namespace fvlam
{
// ==============================================================================
// Translate2 class
// ==============================================================================

  class Translate2
  {
  public:
    using MuVector = Eigen::Vector2d;
    using TangentVector = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, 1>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;

  private:
    MuVector t_;

  public:
    Translate2() :
      t_{MuVector::Zero()}
    {}

    explicit Translate2(MuVector t) :
      t_(std::move(t))
    {}

    Translate2(double x, double y) :
      t_(x, y)
    {}

    const auto &x() const
    { return t_.x(); } //
    const auto &y() const
    { return t_.y(); } //

    const auto &t() const
    { return t_; }

    MuVector mu() const
    { return t_; }

    template<class T>
    static Translate2 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Translate2 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    Translate2 operator+(const Translate2 &other) const
    {
      return Translate2(t_ + other.t_);
    }

    Translate2 operator*(double factor) const
    {
      return Translate2(t_ * factor);
    }
  };

// ==============================================================================
// Translate3 class
// ==============================================================================

  class Translate3
  {
  public:
    using MuVector = Eigen::Vector3d;
    using TangentVector = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, 1>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;

  private:
    MuVector t_;

  public:
    Translate3() :
      t_{MuVector::Zero()}
    {}

    explicit Translate3(MuVector t) :
      t_(std::move(t))
    {}

    Translate3(double x, double y, double z) :
      t_(x, y, z)
    {}

    const auto &x() const
    { return t_.x(); } //
    const auto &y() const
    { return t_.y(); } //
    const auto &z() const
    { return t_.z(); } //

    const auto &t() const
    { return t_; }

    MuVector mu() const
    { return t_; }

    template<class T>
    static Translate3 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Translate3 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    /// Exponential map at identity - create a translation from canonical coordinates \f$ [T_x,T_y,T_z] \f$
    static Translate3 Expmap(const TangentVector &x)
    { return Translate3(x); }

    /// Log map at identity - return the canonical coordinates \f$ [T_x,T_y,T_z] \f$ of this translation
    static TangentVector Logmap(const Translate3 &translate3)
    { return translate3.t_; }

    Translate3 cross(const Translate3 &v) const
    {
      return Translate3{t_.cross(v.t_)};
    }

    Translate3 operator+(const Translate3 &other) const
    {
      return Translate3(t_ + other.t_);
    }

    Translate3 operator*(double factor) const
    {
      return Translate3(t_ * factor);
    }
  };

// ==============================================================================
// Translate3WithCovariance class
// ==============================================================================

  class Translate3WithCovariance
  {
  public:
    using MuVector = Eigen::Matrix<double, Translate3::MuVector::MaxSizeAtCompileTime +
                                           Translate3::CovarianceMatrix::MaxSizeAtCompileTime, 1>;

  private:
    bool is_valid_;
    bool is_cov_valid_;
    Translate3 t_;
    Translate3::CovarianceMatrix cov_;

  public:
    Translate3WithCovariance() :
      is_valid_{false}, is_cov_valid_{false}, t_{}, cov_{Translate3::CovarianceMatrix::Zero()}
    {}

    explicit Translate3WithCovariance(Translate3 t) :
      is_valid_{true}, is_cov_valid_{false}, t_{std::move(t)}, cov_{Translate3::CovarianceMatrix::Zero()}
    {}

    Translate3WithCovariance(Translate3 t, Translate3::CovarianceMatrix cov) :
      is_valid_{true}, is_cov_valid_{true}, t_{std::move(t)}, cov_{std::move(cov)}
    {}

    auto is_valid() const
    { return is_valid_; }

    auto is_cov_valid() const
    { return is_cov_valid_; }

    const auto &t() const
    { return t_; }

    const auto &cov() const
    { return cov_; }

    template<class T>
    static Translate3WithCovariance from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    std::string to_string() const;

    bool equals(const Translate3WithCovariance &other, double tol = 1.0e-9, bool check_relative_also = true) const
    {
      return t_.equals(other.t_, tol, check_relative_also) &&
             Translate3::cov_equals(cov_, other.cov_, tol, check_relative_also);
    }
  };

// ==============================================================================
// Rotate3 class
// ==============================================================================

  class Rotate3
  {
  public:
    using MuVector = Eigen::Matrix<double, 3, 1>;
    using TangentVector = Eigen::Matrix<double, 3, 1>;
    using RotationMatrix = Eigen::Matrix<double, 3, 3>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;
    using Derived = Eigen::Quaterniond;

  private:
    Derived q_{Derived::Identity()};
    RotationMatrix debug_r_{RotationMatrix::Zero()};
    MuVector debug_xyz_{MuVector::Zero()};

    static MuVector xyz(const Derived &q);

  public:
    Rotate3() = default;

    explicit Rotate3(const Derived &q) :
      q_(q), debug_r_{q_.toRotationMatrix()}, debug_xyz_{xyz(q_)}
    {}

    explicit Rotate3(const RotationMatrix &rotation_matrix) :
      q_(rotation_matrix), debug_r_{q_.toRotationMatrix()}, debug_xyz_{xyz(q_)}
    {}

    static Rotate3 Rx(double x)
    { return Rotate3{Derived{Eigen::AngleAxisd{x, Eigen::Vector3d::UnitX()}}}; }

    static Rotate3 Ry(double y)
    { return Rotate3{Derived{Eigen::AngleAxisd{y, Eigen::Vector3d::UnitY()}}}; }

    static Rotate3 Rz(double z)
    { return Rotate3{Derived{Eigen::AngleAxisd{z, Eigen::Vector3d::UnitZ()}}}; }

    static Rotate3 RzRyRx(double x, double y, double z)
    {
      return Rotate3{Derived{Eigen::AngleAxisd{z, Eigen::Vector3d::UnitZ()}} *
                     Derived{Eigen::AngleAxisd{y, Eigen::Vector3d::UnitY()}} *
                     Derived{Eigen::AngleAxisd{x, Eigen::Vector3d::UnitX()}}};
    }

    static Rotate3 Ypr(double y, double p, double r)
    { return RzRyRx(r, p, y); }

    const auto &q() const
    { return q_; }

    RotationMatrix rotation_matrix() const
    { return q_.toRotationMatrix(); }

    MuVector xyz() const
    { return xyz(q_); }

    MuVector mu() const
    { return xyz(q_); }

    template<class T>
    static Rotate3 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Rotate3 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    Rotate3 inverse() const
    {
      return Rotate3(q_.inverse());
    }

    /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z] \f$
    static Rotate3 Expmap(const TangentVector &x);

    /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation
    static TangentVector Logmap(const Rotate3 &rotate3);

    struct ChartAtOrigin
    {
      static Rotate3 retract(const TangentVector &v)
      { return Expmap(v); } //
      static TangentVector local(const Rotate3 &r)
      { return Logmap(r); } //
    };

    Rotate3 compose(const Rotate3 &other) const
    { return *this * other; } //
    Rotate3 between(const Rotate3 &other) const
    { return (*this).inverse() * other; } //
    Rotate3 slerp(const Rotate3 &other, double t) const
    { return compose(Expmap(t * Logmap(between(other)))); } //

    Rotate3 retract(const TangentVector &v) const
    { return compose(ChartAtOrigin::retract(v)); } //
    TangentVector local_coordinates(const Rotate3 &other) const
    { return ChartAtOrigin::local(between(other)); } //

    Rotate3 operator*(const Rotate3 &other) const
    {
      return Rotate3(q_ * other.q_);
    }

    Translate3 operator*(const Translate3 &other) const
    {
      return Translate3(q_ * other.t());
    }
  };

// ==============================================================================
// Transform3 class
// ==============================================================================

  class Transform3
  {
  public:
    using MuVector = Eigen::Matrix<double,
      Rotate3::MuVector::MaxSizeAtCompileTime +
      Translate3::MuVector::MaxSizeAtCompileTime, 1>;
    using TangentVector = Eigen::Matrix<double,
      Rotate3::MuVector::MaxSizeAtCompileTime +
      Translate3::MuVector::MaxSizeAtCompileTime, 1>;
    using CovarianceMatrix = Eigen::Matrix<double, MuVector::MaxSizeAtCompileTime, MuVector::MaxSizeAtCompileTime>;

  private:
    bool is_valid_;
    Rotate3 r_;
    Translate3 t_;

  public:
    Transform3() :
      is_valid_{false}, r_{}, t_{}
    {}

    Transform3(Rotate3 r, Translate3 t) :
      is_valid_{true}, r_(std::move(r)), t_(std::move(t))
    {}

    Transform3(double rx, double ry, double rz, double tx, double ty, double tz) :
      is_valid_{true}, r_{Rotate3::RzRyRx(rx, ry, rz)}, t_(Translate3{tx, ty, tz})
    {}

    explicit Transform3(const MuVector &mu) :
      is_valid_{true},
      r_(Rotate3::RzRyRx(mu(0), mu(1), mu(2))),
      t_(Translate3(mu(3), mu(4), mu(5)))
    {}

    const auto &r() const
    { return r_; }

    const auto &t() const
    { return t_; }

    auto is_valid() const
    { return is_valid_; }

    MuVector mu() const
    { return (MuVector() << r_.mu(), t_.mu()).finished(); }

    template<class T>
    static Transform3 from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    template<class T>
    static CovarianceMatrix cov_from(T &other); //
    template<class T>
    static T cov_to(const CovarianceMatrix &cov); //
    template<class T>
    static void cov_to(const CovarianceMatrix &cov, T &other); //

    std::string to_string() const; //
    static std::string cov_to_string(const CovarianceMatrix &cov); //

    bool equals(const Transform3 &other, double tol = 1.0e-9, bool check_relative_also = true) const; //
    static bool cov_equals(const CovarianceMatrix &own, const CovarianceMatrix &other,
                           double tol = 1.0e-9, bool check_relative_also = true);

    Transform3 inverse() const
    {
      auto qi = r_.q().inverse();
      return Transform3(Rotate3(qi), Translate3(qi * -t_.t()));
    }

    /// Exponential map at identity - create a transform from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
    static Transform3 Expmap(const TangentVector &x);

    /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this transform
    static TangentVector Logmap(const Transform3 &transform3);

    struct ChartAtOrigin
    {
      static Transform3 retract(const TangentVector &v); //
      static TangentVector local(const Transform3 &pose); //
    };

    Transform3 compose(const Transform3 &other) const
    { return *this * other; } //
    Transform3 between(const Transform3 &other) const
    { return (*this).inverse() * other; } //

    Transform3 retract(const TangentVector &v) const
    { return compose(ChartAtOrigin::retract(v)); } //
    TangentVector local_coordinates(const Transform3 &other) const
    { return ChartAtOrigin::local(between(other)); } //

    Translate3 operator*(const Translate3 &other) const
    {
      return Translate3(r_ * other + t_);
    }

    Transform3 operator*(const Transform3 &other) const
    {
      return Transform3{r_ * other.r_, t_ + r_ * other.t_};
    }
  };

// ==============================================================================
// Transform3WithCovariance class
// ==============================================================================

  class Transform3WithCovariance
  {
  public:
    using MuVector = Eigen::Matrix<double, Transform3::MuVector::MaxSizeAtCompileTime +
                                           Transform3::CovarianceMatrix::MaxSizeAtCompileTime, 1>;

  private:
    bool is_cov_valid_;
    Transform3 tf_;
    Transform3::CovarianceMatrix cov_;

  public:
    Transform3WithCovariance() :
      is_cov_valid_{false}, tf_{}, cov_{Transform3::CovarianceMatrix::Zero()}
    {}

    explicit Transform3WithCovariance(Transform3 tf) :
      is_cov_valid_{false}, tf_(std::move(tf)), cov_(Transform3::CovarianceMatrix::Zero())
    {}

    Transform3WithCovariance(Transform3 tf, Transform3::CovarianceMatrix cov) :
      is_cov_valid_{true}, tf_(std::move(tf)), cov_(std::move(cov))
    {}

    auto is_valid() const
    { return tf_.is_valid(); }

    auto is_cov_valid() const
    { return is_cov_valid_; }

    const auto &tf() const
    { return tf_; }

    const auto &cov() const
    { return cov_; }

    template<class T>
    static Transform3WithCovariance from(T &other);

    template<class T>
    T to() const;

    template<class T>
    void to(T &other) const;

    std::string to_string() const;

    bool equals(const Transform3WithCovariance &other, double tol = 1.0e-9, bool check_relative_also = true) const;
  };
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef GEOMETRY_CIRCLE_2_INTEGRATED_FLUX_HPP
#define GEOMETRY_CIRCLE_2_INTEGRATED_FLUX_HPP

#include "rjmcmc/geometry/Circle_2.hpp"
#include <boost/gil/image.hpp>
#include <boost/gil/extension/matis/float_images.hpp>

template<typename View>
void Add1CirclePoints(const View& view, double cx, double cy, double dx, double dy, double d, double & res, double & w)
{
//    typedef View::pixel_t pixel_t;
    typedef boost::gil::dev2n32F_pixel_t pixel_t;
	int i = (int) (cx + dx);
	int j = (int) (cy + dy);
	if(i<0 || j<0 || i>=view.width() || j>=view.height()) return;
	const pixel_t& grad = view(i,j);
	res += boost::gil::at_c<0>(grad) * dx + boost::gil::at_c<1>(grad) * dy;
	w   += d;
}

template<typename View>
void Add4CirclePoints(const View& view, double cx, double cy, double d, double & res, double & w)
{
	Add1CirclePoints(view, cx, cy, 0, d, d, res, w);
	Add1CirclePoints(view, cx, cy, 0,-d, d, res, w);
	Add1CirclePoints(view, cx, cy, d, 0, d, res, w);
	Add1CirclePoints(view, cx, cy,-d, 0, d, res, w);
}

template<typename View>
void Add8CirclePoints(const View& view, double cx, double cy, double dx, double dy, double & res, double & w)
{
	double d = sqrt(dx*dx+dy*dy);
	Add1CirclePoints(view, cx, cy, dx, dy, d, res, w);
	Add1CirclePoints(view, cx, cy,-dx, dy, d, res, w);
	Add1CirclePoints(view, cx, cy,-dx,-dy, d, res, w);
	Add1CirclePoints(view, cx, cy, dx,-dy, d, res, w);
	Add1CirclePoints(view, cx, cy, dy, dx, d, res, w);
	Add1CirclePoints(view, cx, cy,-dy, dx, d, res, w);
	Add1CirclePoints(view, cx, cy,-dy,-dx, d, res, w);
	Add1CirclePoints(view, cx, cy, dy,-dx, d, res, w);
}

template<typename OrientedImage, typename K>
double integrated_flux(const OrientedImage& v, const geometry::Circle_2<K> &c)
{
        typedef typename OrientedImage::view_t view_t;
    view_t view(v.view());
	int x0 = v.x0();
	int y0 = v.y0();

	double cx = c.center().x() - x0;
	double cy = c.center().y() - y0;
	double r  = geometry::radius(c);
	double res = 0., w = 0.;
	double dx = 0;
	double dy = r;
	double p = 3 - 2*r;
	Add4CirclePoints(view, cx, cy, dy, res, w);
	while (dx < dy) {
		if (p < 0) {
			p += 4*dx+6;
		} else {
			--dy;
			Add8CirclePoints(view, cx, cy, dx, dy, res, w);
			p += 4*(dx-dy)+10;
		}
		++dx;
		Add8CirclePoints(view, cx, cy, dx, dy, res, w);
	}
	if(w==0) return 0.;
	return (res * geometry::perimeter(c)) / w;
}

#endif // GEOMETRY_CIRCLE_2_INTEGRATED_FLUX_HPP


#include "rank_commitment.h"
#include <NTL/GF2E.h>
#include <NTL/GF2XFactoring.h>
#include <utils/utils.h>

void rank::rank_commitment::init(rank_commitment::context_t *context) {
    context->irred_poly = NTL::BuildIrred_GF2X(EM);
    NTL::GF2E::init(context->irred_poly);

    context->is_init = 1;
}

void rank::rank_commitment::generate_public_key(
        public_key_t *public_key) {

    utils::generate_random_matrix_gf2e(
            public_key->G,
            K,
            EN);
}

void rank::rank_commitment::generate_commitment(
        NTL::vec_GF2E &c,
        const NTL::vec_GF2 &s,
        const NTL::vec_GF2 &m,
        const NTL::mat_GF2E &g,
        const NTL::vec_GF2E &e) {

    auto s_m = utils::gf2e_from_two_gf2(s, m);
    c = (s_m * g) + e;
}

void rank::rank_commitment::generate_commitment_without_e(
        NTL::vec_GF2E &c,
        const NTL::vec_GF2 &s,
        const NTL::vec_GF2 &m,
        const NTL::mat_GF2E &g) {

    auto s_m = utils::gf2e_from_two_gf2(s, m);

    NTL::vec_GF2E e;
    utils::generate_vector_of_specific_rank(
            e,
            EN,
            RHO);

    c = (s_m * g) + e;
}

int rank::rank_commitment::verify_proof(
        const NTL::vec_GF2E &c,
        const NTL::vec_GF2 &s,
        const NTL::vec_GF2 &m,
        const NTL::mat_GF2E &g) {

    auto s_m = utils::gf2e_from_two_gf2(s, m);
    auto e = (s_m * g) + c;

    if (utils::rank_of_vector(e) != RHO) {
        return 1;
    }

    return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_HYPERGEOMETRIC_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_HYPERGEOMETRIC_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/distributions/hypergeometric.hpp>

#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/prob/uniform_rng.hpp>

namespace stan {
namespace math {

template <class RNG>
inline int hypergeometric_rng(int N, int a, int b, RNG& rng) {
  using boost::math::hypergeometric_distribution;
  using boost::variate_generator;

  static const char* function = "hypergeometric_rng";

  check_bounded(function, "Draws parameter", N, 0, a + b);
  check_positive(function, "Draws parameter", N);
  check_positive(function, "Successes in population parameter", a);
  check_positive(function, "Failures in population parameter", b);

  hypergeometric_distribution<> dist(b, N, a + b);

  double u = uniform_rng(0.0, 1.0, rng);
  int min = 0;
  int max = a - 1;
  while (min < max) {
    int mid = (min + max) / 2;
    if (cdf(dist, mid + 1) > u)
      max = mid;
    else
      min = mid + 1;
  }
  return min + 1;
}

}  // namespace math
}  // namespace stan
#endif


//
// Created by haohanwang on 3/25/16.
//

#include "LinearRegression.h"

#include <Eigen/Sparse>

//#include "ModelOptions.hpp"

using namespace Eigen;

LinearRegression::LinearRegression() {
    L1_reg = 0;
    L2_reg = 0;
};

LinearRegression::LinearRegression(const ModelOptions_t& options) {
    L1_reg = 0;
    L2_reg = 0;
}

void LinearRegression::setL1_reg(float l1) { L1_reg = l1; };

void LinearRegression::setL2_reg(float l2) { L2_reg = l2; };

float LinearRegression::cost() {
    return 0.5 * (y - X * beta).squaredNorm()/X.rows() + L1_reg * beta.cwiseAbs().sum() + L2_reg * beta.squaredNorm();
};

SparseMatrix<float> LinearRegression::derivative() {
    return ((-1.0 * X.transpose() * (y - X * beta)).array() + L1_reg * (beta.array() / beta.cwiseAbs().array()).sum() +
            L2_reg * beta.sum()).matrix();
};

SparseMatrix<float> LinearRegression::proximal_derivative() {
    return -1.0 * X.transpose() * (y - X * beta);
};

SparseMatrix<float> LinearRegression::proximal_operator(SparseMatrix<float> in, float lr) {
    if (L1_reg == 0 && L2_reg == 0){
        return in;
    }
    if (L1_reg != 0 && L2_reg == 0){
        VectorXf sign = ((in.array()>0).matrix()).cast<float>();//sign
        sign += -1.0*((in.array()<0).matrix()).cast<float>();
        in = ((in.array().abs()-lr*L1_reg).max(0)).matrix();//proximal
        return (in.array()*sign.array()).matrix();//proximal multipled back with sign
    }
    else if (L2_reg != 0){
        return in/(1+2*lr*L2_reg);
    }
    else{
        VectorXf sign = ((in.array()>0).matrix()).cast<float>();
        sign += -1.0*((in.array()<0).matrix()).cast<float>();
        in = ((in.array().abs()-lr*L1_reg).max(0)).matrix();
        in = in.array()*sign.array()/(1+2*lr*L2_reg);
        return in.matrix();
    }
}


#include "TranslationFirstPoseController.h"
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

namespace DQ_robotics
{

TranslationFirstPoseController::TranslationFirstPoseController(DQ_kinematics robot, MatrixXd translation_feedback_gain, MatrixXd rotation_feedback_gain, double translation_damping, double rotation_damping) : DQ_controller()
{

    //Initialization of argument parameters
    robot_dofs_          = (robot.links() - robot.n_dummy());
    robot_               = robot;
    kp_                  = translation_feedback_gain;
	kr_                  = rotation_feedback_gain;
	translation_damping_ = translation_damping;
	rotation_damping_    = rotation_damping;

    //Initilization of remaining parameters
    thetas_         = MatrixXd(robot_dofs_,1);
    delta_thetas_   = MatrixXd::Zero(robot_dofs_,1);

    analytical_jacobian_  = MatrixXd(8,robot_dofs_);
	rotation_jacobian_    = MatrixXd(4,robot_dofs_);
	translation_jacobian_ = MatrixXd(4,robot_dofs_);

	nullspace_projector_  = MatrixXd(robot_dofs_, robot_dofs_);

    translation_jacobian_pseudoinverse_   = MatrixXd(robot_dofs_,4);
	rotation_jacobian_pseudoinverse_   = MatrixXd(robot_dofs_,4);

    identity4_           = Matrix<double,4,4>::Identity();
	identityDOFS_        = MatrixXd::Identity(robot_dofs_,robot_dofs_);

    error_translation_  = MatrixXd(4,1);
	error_rotation_     = MatrixXd(4,1);

    end_effector_pose_           = DQ(0,0,0,0,0,0,0,0);
	reference_translation_       = DQ(0,0,0,0,0,0,0,0);
	reference_rotation_          = DQ(0,0,0,0,0,0,0,0);

}

VectorXd TranslationFirstPoseController::getNewJointPositions( const DQ reference, const VectorXd thetas)
{

    delta_thetas_ = getNewJointVelocities(reference, thetas);

    // Send updated thetas to simulation
    return (thetas_ + delta_thetas_);

}

VectorXd TranslationFirstPoseController::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{

    ///--Remapping arguments
    thetas_ = thetas;

	//Get translation and rotation individually
	reference_translation_ = reference.translation();
	reference_rotation_    = P(reference);

    ///--Controller Step

    //Calculate jacobian and FKM
    analytical_jacobian_  = robot_.analyticalJacobian(thetas_);
    end_effector_pose_    = robot_.fkm(thetas_);

    //Error
    error_translation_ = vec4( reference_translation_ -  end_effector_pose_.translation() );

	error_rotation_    = vec4( reference_rotation_    - P(end_effector_pose_) );

	//Calculate Jacobians
	translation_jacobian_ = translationJacobian(analytical_jacobian_, vec8(end_effector_pose_));
	rotation_jacobian_    = rotationJacobian(analytical_jacobian_);

	//Pseudoinverses calculation
	translation_jacobian_pseudoinverse_ = (translation_jacobian_.transpose())
										 *(translation_jacobian_*translation_jacobian_.transpose() + translation_damping_*translation_damping_*identity4_).inverse();

	rotation_jacobian_pseudoinverse_ =    (rotation_jacobian_.transpose())
										 *(rotation_jacobian_*rotation_jacobian_.transpose() + rotation_damping_*rotation_damping_*identity4_).inverse();

	//Nullspace projector
	nullspace_projector_ = (identityDOFS_ - (pseudoInverse(translation_jacobian_))*translation_jacobian_ );

    delta_thetas_ = translation_jacobian_pseudoinverse_ * kp_ * error_translation_ +
					nullspace_projector_ * rotation_jacobian_pseudoinverse_ * kr_ * error_rotation_;

    return delta_thetas_;

}

}


#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ndt_generic/eigen_utils.h>

#include <iostream>
#include <iomanip>

using namespace std;

int main()
{
    // Simple check of the normalization
    {
        Eigen::VectorXd x(6);
        x << 1, 2, 3, 0.1, 0.2, 2.7;

        std::cout << "x: " << x << std::endl;

        Eigen::Affine3d T = ndt_generic::vectorToAffine3d(x);

        std::cout << "T : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T.rotation() : " << T.rotation() << std::endl;

        x = ndt_generic::affine3dToVector(T);

        std::cout << "x: " << x << std::endl;

        std::cout << "------------------------- The two T matrices should be the same below ------------------" << std::endl;

        x(3) += M_PI;

        std::cout << "x : " << x << std::endl;
        T = ndt_generic::vectorToAffine3d(x);
        std::cout << "T : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T.rotation() : " << T.rotation() << std::endl;

        ndt_generic::normalizeEulerAngles6dVec(x);

        std::cout << "x : " << x << std::endl;
        T = ndt_generic::vectorToAffine3d(x);
        std::cout << "T : " << ndt_generic::affine3dToStringRPY(T) << std::endl;
        std::cout << "T.rotation() : " << T.rotation() << std::endl;
    }

    // Test the fusion
    {
        std::cout << "---------------------------------------------" << std::endl;
        Eigen::Matrix3d covA;
        covA.setIdentity();

        Eigen::Matrix3d covB;
        covB.setIdentity();

        Eigen::Vector3d a(1,2,3);
        Eigen::Vector3d b(0,1,0);

        Eigen::Vector3d weighted = ndt_generic::getWeightedPoint(a,covA,b,covB);

        std::cout << "weighted : " << ndt_generic::getWeightedPoint(a,covA,b,covB);
        std::cout << "should be : 0.5, 1.5, 1.5" << std::endl;
        covB *= 10.;
        std::cout << "weighted : " << ndt_generic::getWeightedPoint(a,covA,b,covB);
        std::cout << "should be : 0.9091, 1.9091, 2.7273" << std::endl;
    }

    {
        std::cout << "---------------------------------------------" << std::endl;
        Eigen::Affine3d a = Eigen::Translation<double,3>(1,2,3)*
        Eigen::AngleAxis<double>(0.1,Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis<double>(0.2,Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis<double>(0.3,Eigen::Vector3d::UnitZ()) ;

        Eigen::Affine3d b = Eigen::Translation<double,3>(0,1,0)*
        Eigen::AngleAxis<double>(0.0,Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis<double>(0.1,Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis<double>(0.0,Eigen::Vector3d::UnitZ()) ;

        Eigen::MatrixXd covA(6,6);
        covA.setIdentity();

        Eigen::MatrixXd covB(6,6);
        covB.setIdentity();

        std::cout << "weighted : " << ndt_generic::affine3dToStringRPY(ndt_generic::getWeightedPose(a, covA, b, covB)) << std::endl;
    }

}


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <array>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <optional>
#include <unordered_set>

using Mat = Eigen::Matrix<int, 5, 5>;

auto parseRow(std::string const &line) {
  Eigen::Vector<int, 5> out;
  std::stringstream ss(line);
  for (auto i = 0; i < 5; ++i) {
    ss >> out[i];
  }
  return out;
}

auto parseFile(std::string const &fn) {
  auto file = std::ifstream(fn);

  std::string line;
  std::getline(file, line);
  for (auto &c : line) {
    if (c == ',') {
      c = ' ';
    }
  }
  std::stringstream ss(line);

  std::uint16_t num = 0;
  std::vector<std::uint16_t> out;
  while (ss >> num) {
    out.push_back(num);
  }

  // Now for the matrices
  std::vector<Mat> outM;
  int row = 0;
  while (std::getline(file, line)) {
    auto pos = line.find_first_not_of(' ');
    if (pos == std::string::npos || line[pos] == '\n') {
      row = 0;
      outM.emplace_back();
      continue;
    }

    outM.back().row(row++) = parseRow(line);
  }

  return std::make_pair(out, outM);
}

auto matWins(Mat const &m) {
  for (auto r = 0; r < m.rows(); ++r) {
    if (m.row(r).sum() == -5) {
      return true;
    }
  }

  for (auto c = 0; c < m.cols(); ++c) {
    if (m.col(c).sum() == -5) {
      return true;
    }
  }

  return false;
}

int main(int _, char **argv) {
  auto [rands, mats] = parseFile(argv[1]);

  std::optional<Mat> winner;
  std::optional<Mat> loser;
  std::uint16_t rand_winner = 0;
  std::uint16_t rand_loser = 0;
  std::unordered_set<Mat*> winners;
  for (auto r : rands) {
    for (auto &m : mats) {
      if(winners.count(&m) == 1){
        continue;
      }
      m = (m.unaryExpr([&](int x) { return (x == r) ? -1 : x; }));
      if (matWins(m)) {
        if(!winner){
          winner = m;
          rand_winner = r;
        }
        winners.insert(&m);
        if(winners.size() == mats.size()){
          loser = m;
          rand_loser = r;
        }
      }
    }
    if(loser){
      break;
    }
  }

  auto sum = (*winner).unaryExpr([](int x) { return x == -1 ? 0 : x; }).sum();
  auto sum_loser = (*loser).unaryExpr([](int x) { return x == -1 ? 0 : x; }).sum();
  fmt::print("Winning: {}\n", sum * rand_winner);
  fmt::print("Losing: {}\n", sum_loser * rand_loser);
}


///////////////////////////////////////////////////////////////////////////////
// accumulator::statistics::percentage_effective_sample_size.hpp             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2009
#include <boost/parameter/binding.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/statistics/detail/importance_sampling/statistics/variance_of_mean_normalized.hpp>
#include <boost/accumulators/framework/depends_on.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace accumulator{
namespace impl
{

    // Var(w/c) = Var(w) / c^2, where c = mean(w)
    template<typename T>
    class percentage_effective_sample_size
            : public boost::accumulators::accumulator_base
        {
        typedef boost::accumulators::dont_care dont_care_;

        typedef tag::variance_of_mean_normalized tag_vmn_;
        typedef boost::accumulators::tag::accumulator tag_acc_;

        public:
        typedef T result_type;
        percentage_effective_sample_size(){}
        percentage_effective_sample_size(dont_care_){}
        void operator()(dont_care_)const{}

        template<typename Args>
        result_type result(const Args& args) const
        {

            typedef
                typename boost::parameter::binding<Args,tag_acc_>::type cref_;
            cref_ acc = args[boost::accumulators::accumulator];

            T vmn = accumulators::extract_result<tag_vmn_>(acc);
            return one / (one+vmn);
        }

        static const T one;
    };

    template<typename T>
    const T percentage_effective_sample_size<T>::one = static_cast<T>(1);

}//impl

///////////////////////////////////////////////////////////////////////////////
// tag::percentage_effective_sample_size
namespace tag
{
    struct percentage_effective_sample_size
      : boost::accumulators::depends_on<tag::variance_of_mean_normalized>
    {
      typedef statistics::detail::accumulator::impl
        ::percentage_effective_sample_size<boost::mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::percentage_effective_sample_size
namespace extract
{

  template<typename AccSet>
  typename
    boost::mpl::apply<
        AccSet,
        tag::percentage_effective_sample_size
    >::type::result_type
  percentage_effective_sample_size(AccSet const& acc){
    typedef tag::percentage_effective_sample_size the_tag;
    return boost::accumulators::extract_result<the_tag>(acc);
  }

}

using extract::percentage_effective_sample_size;

}// accumulator
}// detail
}// statistics
}// boost

#endif


//          Copyright Carl Philipp Reh 2009 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/catch/begin.hpp>
#include <fcppt/catch/end.hpp>
#include <fcppt/math/is_zero_boost_units.hpp>
#include <fcppt/math/vector/arithmetic.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/optional/comparison.hpp>
#include <fcppt/optional/make.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <catch2/catch.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_CATCH_BEGIN

TEST_CASE("math units", "[math]")
{
  using unit_type = int;

  using length = boost::units::quantity<boost::units::si::length, unit_type>;

  using time = boost::units::quantity<boost::units::si::time, unit_type>;

  using velocity = boost::units::quantity<boost::units::si::velocity, unit_type>;

  using length2 = fcppt::math::vector::static_<length, 2>;

  using time2 = fcppt::math::vector::static_<time, 2>;

  using velocity2 = fcppt::math::vector::static_<velocity, 2>;

  length2 const l1(length(-100 * boost::units::si::meter), length(200 * boost::units::si::meter));

  time2 const t1(time(4 * boost::units::si::second), time(2 * boost::units::si::second));

  CHECK(
      l1 / t1 == fcppt::optional::make(velocity2{
                     -25 * boost::units::si::meter / boost::units::si::second,
                     100 * boost::units::si::meter / boost::units::si::second}));
}

FCPPT_CATCH_END


/*
The MATLAB gateway function to BBFMM2D developed by Sivaram.
The code provides a O(N) solution to a kernel matrix-vector product.
Written by Judith Yue Li 10/02/2013
*/

#include <iostream>
#include "math.h"
#include "mex.h"
#include "matrix.h"
#include "environment.hpp"
#include "BBFMM2D.hpp"
#include <Eigen/Core>
#include "kernelfun.hpp"

using namespace Eigen;
using namespace std;
double pi 	=	4.0*atan(1.0);
extern void _main();

#define IS_REAL_2D_FULL_DOUBLE(P) (!mxIsComplex(P) && mxGetNumberOfDimensions(P) == 2 && !mxIsSparse(P) && mxIsDouble(P))
#define IS_REAL_SCALAR(P) (IS_REAL_2D_FULL_DOUBLE(P) && mxGetNumberOfElements(P) == 1)

// Pass location from matlab to C
void read_location(const mxArray* x, const mxArray* y, vector<Point>& location){
    unsigned long N;
    double *xp, *yp;
    N = mxGetM(x);
    xp = mxGetPr(x);
    yp = mxGetPr(y);
    for (unsigned long i = 0; i < N; i++){
        Point new_Point;
        new_Point.x = xp[i];
        new_Point.y = yp[i];
        location.push_back(new_Point);
    }
}

void mexFunction(int nlhs,mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // Macros for the output and input arguments
    #define QH_OUT          plhs[0]
    #define QHexact_OUT     plhs[1]
    #define x_IN            prhs[0]
    #define y_IN            prhs[1]
    #define H_IN            prhs[2]
    #define nCheb_IN        prhs[3]
    #define print_IN        prhs[4]

    unsigned long N;
    unsigned m;
    // Instruction
    char errmsg[1023 + 1];
    sprintf(errmsg,"Calling sequence is\n\tQH = %s(xloc,yloc,H,nCheb,PrintFlag); or\n\t[QH QHexact] = %s(xloc,yloc,H,nCheb,PrintFlag);\n", mexFunctionName(), mexFunctionName());

    // Argument Checking:
    // Check number of argument
    if(nrhs != 5) {
        mexPrintf(errmsg);
        mexErrMsgTxt("Wrong number of input arguments");
    }else if(nlhs > 2){
        mexErrMsgTxt("Too many output arguments");
    }

    if( !IS_REAL_2D_FULL_DOUBLE(x_IN)) {
        mexErrMsgTxt("Third input argument is not a real 2D full double array.");
    }
    if( !IS_REAL_2D_FULL_DOUBLE(y_IN)) {
        mexErrMsgTxt("Third input argument is not a real 2D full double array.");
    }
    if( !IS_REAL_2D_FULL_DOUBLE(H_IN)) {
        mexErrMsgTxt("Third input argument is not a real 2D full double array.");
    }
    if( !IS_REAL_SCALAR(nCheb_IN)){
        mexErrMsgTxt("nChebnotes must be a real double scalar");
    }
    if( mxGetM(x_IN)!= mxGetM(y_IN) || mxGetM(x_IN) != mxGetM(H_IN)){
        mexErrMsgTxt("The dimension of the input matrices is wrong");
    }

    //processing on input arguments
    N = mxGetM(H_IN); // get the first dimension of H
    m = mxGetN(H_IN); // get the second dimension of H
    unsigned short nChebNodes = *mxGetPr(nCheb_IN);
    bool print = *mxGetPr(print_IN);
    vector<Point> location;
    read_location(x_IN,y_IN,location);
    double *charges;
    charges = mxGetPr(H_IN);
    // Load data to local array using Eigen <Map>
    MatrixXd H = Map<MatrixXd>(charges, N, m); // Map<MatrixXd> H(charges,N,m);

    // Compute Fast matrix vector product
    // 1. Build Tree
    clock_t startBuild  = clock();
    H2_2D_Tree Atree(nChebNodes, charges, location, N, m, print); //Build the fmm tree
    clock_t endBuild = clock();

    double FMMTotalTimeBuild = double(endBuild-startBuild)/double(CLOCKS_PER_SEC);
    if(print)
        mexPrintf("\nTime taken for FMM(build tree) is: %.4g\n",FMMTotalTimeBuild);

    // 2.Calculateing potential
    clock_t startA = clock();
    // Create the output matrix
    QH_OUT = mxCreateDoubleMatrix(N, m, mxREAL);
    // Get a pointer to the real data in the output matrix
    double *QHp;
    QHp = mxGetPr(QH_OUT);
    myKernel A;
    A.calculate_Potential(Atree, QHp);
    clock_t endA = clock();
    double FMMTotalTimeA = double(endA-startA)/double(CLOCKS_PER_SEC);
    if(print){
        mexPrintf("\nTime taken for FMM(calculating potential) is: %.4g\n",FMMTotalTimeA);
        mexPrintf("\nTotal time taken for FMM is: %.4g\n",FMMTotalTimeA+FMMTotalTimeBuild);
    }

    /*///////////////////////////////
    // Compute exact covariance Q //
    ///////////////////////////////*/

    if(nlhs == 2){
    if(print){
        mexPrintf("\nStarting exact computation...\n");
    }
    clock_t start = clock();
    MatrixXd Q;
    A.kernel_2D(N, location, N, location, Q);// Q is initialized inside function A.kernel_2D
    clock_t end = clock();
    double exactAssemblyTime = double(end-start)/double(CLOCKS_PER_SEC);

    // Compute exact Matrix vector product
    start = clock();
    QHexact_OUT = mxCreateDoubleMatrix(N, m, mxREAL);
    double *QHexactp;
    QHexactp = mxGetPr(QHexact_OUT);
    Map<MatrixXd> QHT(QHexactp,N,m);
    QHT = Q*H;
    end = clock();
    double exactComputingTime = double(end-start)/double(CLOCKS_PER_SEC);
    if(print){
        mexPrintf("\nThe total exact computation time is: %.4g\n",exactAssemblyTime + exactComputingTime);
    }

    // Compute the difference
    MatrixXd QHfast = Map<MatrixXd>(QHp, N, m);
    MatrixXd error = QHfast - QHT;
    double absoluteError = error.norm();
    double relativeError = absoluteError/QHT.norm();
    if(print){
        mexPrintf("The relative difference is: %13.6E \n", relativeError);
    }

    }

    return;
}


#include "interface.h"

#include <cmath>
#include <cstdio>
#include <cstring>
#include <cstdlib>

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

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>
#include <Eigen/IterativeLinearSolvers>

constexpr bool DEBUG = false;

using namespace Eigen;

constexpr int N = WIDTH * HEIGHT;

void print_total_velocities(int line, Grid *grid) {
    double total_sum_x = 0.0;
    double total_sum_y = 0.0;
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        total_sum_x += std::abs(grid->velocity_x[y][x]);
        total_sum_y += std::abs(grid->velocity_y[y][x]);
    }
    if (DEBUG) std::cout << "total at line " << line << ": " << total_sum_x << ", " << total_sum_y << std::endl;
}

#define PV do { if (DEBUG) print_total_velocities(__LINE__, grid); } while (false)

static SimParams params;
static SparseMatrix<double> laplacian;
static ConjugateGradient<SparseMatrix<double>, Lower|Upper, IncompleteCholesky<double>> solver;
// static SimplicialLDLT<SparseMatrix<double>> solver;

static void step(Grid *grid, const Grid *prev);

void sim_init_grid(Grid *grid) {
    std::memset(grid, 0, sizeof *grid);
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        grid->velocity_x[y][x] = 0.0;
        grid->velocity_y[y][x] = 0.0;
    }
    for (int y = 0; y < 20; y++) for (int x = 0; x < 20; x++) {
        grid->density[y + HEIGHT - 40][WIDTH / 2 - 10 + x] = 1.0;
        grid->temperature[y + HEIGHT - 40][WIDTH / 2 - 10 + x] = 20;
    }
}

inline int INDEX(int x, int y) { return x + y * WIDTH; }

bool is_valid(int x, int y) {
    if (x < 0 || x >= WIDTH) return false;
    if (y < 0 || y >= HEIGHT) return false;
    if (params.obstacle_enabled
     && x >= params.obstacle_xmin && x < params.obstacle_xmax
     && y >= params.obstacle_ymin && y < params.obstacle_ymax) {
        return false;
    }
    return true;
}

void sim_init() {
    solver.setMaxIterations(40);
    solver.setTolerance(1e-10);
    std::vector<Eigen::Triplet<double>> rows;
    rows.reserve(5 * N);
    // fill in Laplacian matrix
    int NEIGHBOR_OFFSETS[][2] = {
        {-1, 0},
        { 1, 0},
        { 0,-1},
        { 0, 1},
    };
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        if (!is_valid(x, y)) {
            rows.emplace_back(INDEX(x, y), INDEX(x, y), 1.0);
            continue;
        }
        int neighbor_count = 0;
        for (const int (&d)[2] : NEIGHBOR_OFFSETS) {
            int dx = x + d[0], dy = y + d[1];
            if (is_valid(dx, dy)) {
                rows.emplace_back(INDEX(x, y), INDEX(dx, dy), -1.0);
                neighbor_count += 1;
            }
        }
        // rows.emplace_back(INDEX(x, y), INDEX(x, y), neighbor_count);
        rows.emplace_back(INDEX(x, y), INDEX(x, y), neighbor_count);
    }
    laplacian.resize(N, N);
    laplacian.setFromTriplets(rows.begin(), rows.end());
    rows.clear();
    solver.compute(laplacian);
}

extern "C" {
    double glfwGetTime();
}

std::atomic<TimeNode *> timer_root(nullptr);

void sim_main() {
    TimeNode *prev_root = timer_root.exchange(nullptr);
    while (prev_root) {
        TimeNode *next = prev_root->next;
        delete prev_root;
        prev_root = next;
    }
    Grid *prev = grids.get_current(WRITER);
    while (running.load(std::memory_order_relaxed)) {
        // update FPS counter
        TimeNode *root = timer_root.load();
        TimeNode *node = new TimeNode();
        node->time = glfwGetTime();
        node->next = root;
        timer_root.store(node);
        Grid *next = grids.swap(WRITER);
        auto new_params = param_buf.swap(READER);
        if (new_params->updated) {
            params = *new_params;
            new_params->updated = false;
        }
        step(next, prev);
        next->updated = true;
        /*
        for (int y = 0; y < HEIGHT; y++) {
            for (int x = 0; x < WIDTH; x++) {
                std::cout << (next->temperature[y][x] > 10.0f ? 'A' : ' ');
            }
            std::cout << "\n";
        }
        std::cout << std::endl;
        */
        prev = next;
    }
}

template<int W, int H>
double querySafe(const double (&values)[H][W], int x, int y) {
    if (x < 0 || y < 0 || x >= W || y >= H) return 0.0;
    return values[y][x];
}

template<int W, int H>
double interpolate(const double (&values)[H][W], Vector2d position) {
    int ix = (int) std::floor(position(0)),
        iy = (int) std::floor(position(1));
    double fx = position(0) - ix, fy = position(1) - iy;
    double vx0 = querySafe(values, ix, iy  ) * (1.0 - fx) + querySafe(values, ix+1, iy  ) * fx;
    double vx1 = querySafe(values, ix, iy+1) * (1.0 - fx) + querySafe(values, ix+1, iy+1) * fx;
    return vx0 * (1.0 - fy) + vx1 * fy;
}

// estimate velocity at position by interpolating nearest neighbors
Vector2d interpolateVelocity(const Grid &grid, Vector2d position) {
    double vx = interpolate(grid.velocity_x, position + Vector2d(0.5, 0.0));
    double vy = interpolate(grid.velocity_y, position + Vector2d(0.0, 0.5));
    return Vector2d(vx, vy);
}

void process_forces(Grid *grid) {
    memset(grid->force_x, 0, sizeof grid->force_x);
    memset(grid->force_y, 0, sizeof grid->force_y);
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        grid->force_y[y][x] += -params.alpha * grid->density[y][x] + params.beta * grid->temperature[y][x];
    }
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++)
    {
      Vector2d vel_u = interpolateVelocity(*grid, Vector2d(x,y+1));
      Vector2d vel_d = interpolateVelocity(*grid, Vector2d(x,y-1));
      Vector2d vel_l = interpolateVelocity(*grid, Vector2d(x+1,y));
      Vector2d vel_r = interpolateVelocity(*grid, Vector2d(x-1,y));

      //Since 2-D we only need z direction of omega
      grid->vorticity[y][x] = 0.5 * (vel_r(1) - vel_l(1) - vel_u(0) + vel_d(0));
    }
    for (int y = 1; y < HEIGHT - 1; y++) for (int x = 1; x < WIDTH - 1; x++)
    {
      Vector2d N;
      //Used for normalization
      using std::abs;
      N(0) = 0.5 * (abs(grid->vorticity[y][x+1]) - abs(grid->vorticity[y][x-1]));
      N(1) = 0.5 * (abs(grid->vorticity[y+1][x]) - abs(grid->vorticity[y-1][x]));
      N /= N.norm() + 1e-5;
      grid->force_x[y][x] += N(1) * grid->vorticity[y][x] * params.epsilon;
      grid->force_y[y][x] -= N(0) * grid->vorticity[y][x] * params.epsilon;
    }
}

void apply_force(Grid *grid) {
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        grid->velocity_x[y][x] += params.timestep * interpolate(grid->force_x, Vector2d(x - 0.5, y));
        grid->velocity_y[y][x] += params.timestep * interpolate(grid->force_y, Vector2d(x, y - 0.5));
    }
}

void calculate_pressure(Grid *grid) {
    VectorXd b(N);
    double sum_b = 0.0;
    double dot_b = 0.0;
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        int n = INDEX(x, y);
        b(n) = -grid->velocity_x[y][x] + grid->velocity_x[y][x + 1]
             + -grid->velocity_y[y][x] + grid->velocity_y[y + 1][x];
        b(n) *= -1.0;
        sum_b += std::abs(b(n));
        dot_b += b(n);
    }
    if (DEBUG) std::cout << "sum of b's: " << sum_b << std::endl;
    if (DEBUG) std::cout << "dot b with 1: " << dot_b << std::endl;
    Map<VectorXd> pressureMap((double *) grid->pressure, N);
    VectorXd temp = solver.solve(b);
    if (DEBUG) std::cout << "solver error: " << ((laplacian * temp) - b).norm() << std::endl;
    // if (b.norm() < 1e-10) {
    //     temp.setZero();
    // }
    double sum_p = 0.0;
    for (int i = 0; i < N; i++) {
        sum_p += std::abs(temp(i));
    }
    if (DEBUG) std::cout << "sum of P: " << sum_p << std::endl;
    pressureMap = temp;
}

bool have_exported = false;

void step(Grid *grid, const Grid *prev) {
    *grid = *prev;
    PV;
    process_forces(grid);
    PV;
    for (int y = 0; y < HEIGHT + 1; y++) for (int x = 0; x < WIDTH + 1; x++) {
        // update velocity to obtain u*
        grid->velocity_x[y][x] = interpolate(
            prev->velocity_x,
            Vector2d(x, y) - params.timestep *
                interpolateVelocity(*prev, Vector2d(x - 0.5, y))
        );
        grid->velocity_y[y][x] = interpolate(
            prev->velocity_y,
            Vector2d(x, y) - params.timestep *
                interpolateVelocity(*prev, Vector2d(x, y - 0.5))
        );
    }
    apply_force(grid);
    PV;
    calculate_pressure(grid);
    {
        double sum_div = 0.0;
        for (int y = 1; y < HEIGHT - 1; y++) for (int x = 1; x < WIDTH - 1; x++) {
            double div = -grid->velocity_x[y][x] + grid->velocity_x[y][x+1]
                         -grid->velocity_y[y][x] + grid->velocity_y[y+1][x];
            sum_div += std::abs(div);
            // if (std::fabs(div) > 1e-2) std::cout << "+++" << div << std::endl;
        }
        if (DEBUG) std::cout << "sum of div before: " << sum_div << std::endl;
    }
    for (int y = 1; y < HEIGHT; y++) for (int x = 1; x < WIDTH; x++) {
        // update velocity based on pressure
        grid->velocity_x[y][x] -= (grid->pressure[y][x] - grid->pressure[y][x - 1]);
        grid->velocity_y[y][x] -= (grid->pressure[y][x] - grid->pressure[y - 1][x]);
    }
    PV;
    {
        double sum_div = 0.0;
        for (int y = 1; y < HEIGHT - 1; y++) for (int x = 1; x < WIDTH - 1; x++) {
            double div = -grid->velocity_x[y][x] + grid->velocity_x[y][x+1]
                         -grid->velocity_y[y][x] + grid->velocity_y[y+1][x];
            sum_div += std::abs(div);
            // if (std::fabs(div) > 1e-2) std::cout << "+++" << div << std::endl;
        }
        if (DEBUG) std::cout << "sum of div after: " << sum_div << std::endl;
    }
    for (int i = 0; i <= WIDTH; i++) {
        grid->velocity_y[0][i] = 0;
        grid->velocity_y[HEIGHT][i] = 0;
    }
    for (int i = 0; i <= HEIGHT; i++) {
        grid->velocity_y[i][0] = 0;
        grid->velocity_y[i][WIDTH] = 0;
    }
    if (params.obstacle_enabled) {
        for (int y = params.obstacle_ymin; y <= params.obstacle_ymax; y++) {
            for (int x = params.obstacle_xmin; x <= params.obstacle_xmax; x++) {
                if (!(x == params.obstacle_xmin || x == params.obstacle_xmax - 1)) {
                    grid->velocity_y[y][x] = 0;
                }
                if (!(y == params.obstacle_ymin || y == params.obstacle_ymax - 1)) {
                    grid->velocity_x[y][x] = 0;
                }
            }
        }
    }
    // advect temperature and density
    for (int y = 0; y < HEIGHT; y++) for (int x = 0; x < WIDTH; x++) {
        Vector2d pt = Vector2d(x, y) - params.timestep *
            interpolateVelocity(*grid, Vector2d(x, y));
        grid->density[y][x] = interpolate(prev->density, pt);
        if (grid->density[y][x] < 0.0) {
            std::cout << "NEGATIVE DENSITY!" << std::endl;
        }
        grid->temperature[y][x] = interpolate(prev->temperature, pt);
        // heat transfer
        double old = grid->temperature[y][x];
        grid->temperature[y][x] -= params.kappa * grid->temperature[y][x] * params.timestep;
        if (old * grid->temperature[y][x] < 0.0) {
            grid->temperature[y][x] = 0.0;
        }
    }
    PV;
    if (params.emitter_density > 1e-6) {
        for (int z = 0; z < 10; z++) {
            int y = 2;
            int x = (WIDTH - 10) / 2 + z;
            double d = grid->density[y][x];
            double dens = params.emitter_density;
            grid->density[y][x] += dens;
            grid->temperature[y][x] = (grid->temperature[y][x] * d + dens * params.emitter_temp) / grid->density[y][x];
            // grid->velocity_y[y][x] = 1.0;
        }
    }
    if (params.obstacle_enabled) {
        for (int y = params.obstacle_ymin; y < params.obstacle_ymax; y++) {
            for (int x = params.obstacle_xmin; x < params.obstacle_xmax; x++) {
                grid->pressure[y][x] = 0;
                grid->temperature[y][x] = 0;
                grid->density[y][x] = 0;
            }
        }
    }
    PV;
    if (params.want_to_export && !have_exported) {
        have_exported = true;
        std::ofstream f("out.csv");
        for (int y = 0; y <= HEIGHT; y++) for (int x = 0; x <= WIDTH; x++) {
            f << x << "," << y << "," << grid->velocity_x[y][x] << "," << grid->velocity_y[y][x] << std::endl;
        }
    }
}


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_DATA_MATRIX_HPP
#define UTILITIES_DATA_MATRIX_HPP

#include "Vector.hpp"
#include "../UtilitiesAPI.hpp"
#include "../core/Logger.hpp"

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

namespace openstudio{

  /// Matrix
  typedef boost::numeric::ublas::matrix<double> Matrix;

//////////////////////////////////////////////////////////////////////////
// Begin SWIG'able, copy and paste into Matrix.i
//////////////////////////////////////////////////////////////////////////

  /// new operators

  UTILITIES_API bool operator==(const Matrix& lhs, const Matrix& rhs);
  UTILITIES_API bool operator!=(const Matrix& lhs, const Matrix& rhs);

  /// common methods

  /// linear interpolation of the function v = f(x, y) at point xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API double interp(const Vector& x, const Vector& y, const Matrix& v, double xi, double yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, double yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Matrix& v, double xi, const Vector& yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// linear interpolation of the function v = f(x, y) at points xi, yi
  /// assumes that x and y are strictly increasing
  UTILITIES_API Matrix interp(const Vector& x, const Vector& y, const Matrix& v, const Vector& xi, const Vector& yi, InterpMethod interpMethod = LinearInterp, ExtrapMethod extrapMethod = NoneExtrap);

  /// matrix product
  UTILITIES_API Matrix prod(const Matrix& lop, const Matrix& rop);

  /// vector product
  UTILITIES_API Vector prod(const Matrix& m, const Vector& v);

  /// outer product
  UTILITIES_API Matrix outerProd(const Vector& lhs, const Vector& rhs);

  /// take the natural logarithm of Matrix elements, componentwise
  UTILITIES_API Matrix log(const Matrix& v);

  /// take the logarithm of Matrix elements with respect to base, componentwise
  UTILITIES_API Matrix log(const Matrix& v, double base);

  /// generates a M x N Matrix whose elements come from the uniform distribution on [a,b].
  UTILITIES_API Matrix randMatrix(double a, double b, unsigned M, unsigned N);

  /// sum of all elements
  UTILITIES_API double sum(const Matrix& matrix);

  /// maximum of all elements
  UTILITIES_API double maximum(const Matrix& matrix);

  /// minimum of all elements
  UTILITIES_API double minimum(const Matrix& matrix);

  /// mean of all elements
  UTILITIES_API double mean(const Matrix& matrix);

  /// get the connected components from an NxN adjacency matrix (1.0 for i-j connected, 0.0 for i-j not connected)
  UTILITIES_API std::vector<std::vector<unsigned> > findConnectedComponents(const Matrix& matrix);

  // from the boost vault:
  // The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipes in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.
  /// Matrix inversion routine, using lu_factorize and lu_substitute in uBLAS to invert a matrix */
 template<class T>
 bool invert(const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse) {

   // create a working copy of the input
   boost::numeric::ublas::matrix<T> A(input);

   // create a permutation matrix for the LU-factorization
   boost::numeric::ublas::permutation_matrix<std::size_t> pm(A.size1());

   // perform LU-factorization
   typename boost::numeric::ublas::matrix<T>::size_type res = boost::numeric::ublas::lu_factorize(A, pm);
   if( res != 0 ){
     LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_factorize returned res = " << res <<
                    ", A = " << A << ", pm = " << pm << " for input = " << input);
     return false;
   }

   // create identity matrix of "inverse"
   inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

   // backsubstitute to get the inverse
   try {
     boost::numeric::ublas::lu_substitute(A, pm, inverse);
   }catch (std::exception& e){
     LOG_FREE(Info, "boost.ublas", "boost::numeric::ublas::lu_substitute threw exception '" << e.what() <<
                    "' for A = " << A << ", pm = " << pm);
     return false;
   }

   return true;
 }

} // openstudio

#endif //UTILITIES_DATA_MATRIX_HPP