#include <ros/ros.h>
#include "ros_utils.hpp"
#include "kinematics.hpp"
#include "estimators.hpp"
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <casadi/casadi.hpp>

#include <mhe_estimator/ArticulatedAngles.h>
#include <mhe_estimator/CanData.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Twist.h"
#include "ackermann_msgs/AckermannDrive.h"
#include "std_srvs/Trigger.h"
#include "std_srvs/Empty.h"

//--------------------------| Moving Horizen Window Length |---------------------------//
const long unsigned int N_mhe = 60;
//-------------------------------------------------------------------------------------//

int main(int argc, char **argv)
{
    using namespace mhe_estimator;
    using namespace ast;
    using namespace ast::ros;
    ::ros::init(argc, argv, "mhe_estimator_node");
    NodeHandle nh;

    /*----------------| Get Params |----------------*/
    CarParams carParams;
    MheParams mheParams;
    ParamsIn(carParams, mheParams, nh);
    /*---------------------------------------------*/

    /*------------| Global Var and Obj |-----------*/
    ::ros::Time lastPerceptionTime;
    const long unsigned int N_mhePlus1 = N_mhe + 1;
    boost::array<Vec4, N_mhePlus1> q4wLoc;        //x y theta beta window
    boost::array<Vec2, N_mhe> control2w;           //control2 window

    boost::array<Vec4, N_mhePlus1> q4wCov;        //state cov window
    boost::array<Vec2, N_mhe> control2wCov;        //control cov window

    boost::array<Vec5, N_mhePlus1> q5wLocTrailer; //x y theta beta window triler =1
    boost::array<Vec2, N_mhe> control2wTrailer;    //control2 window triler =1

    boost::array<Vec5, N_mhePlus1> q5wCovTrailer; //state cov window trailer1
    boost::array<Vec2, N_mhe> control2wCovTrailer; //control cov window trailer1

    Vec4 qLoc;                 //triler == 0
    Vec2 controls;              //triler == 0
    Vec4 qEstFirstSample;       //triler == 0

    Vec4 qCov;               //cov triler == 0
    Vec4 qCovMhe;
    Vec2 controlsCov;           //cov triler == 0

    Vec5 qLocTrailer;         //triler == 1
    Vec2 controlsTrailer;        //triler == 1
    Vec5 qEstFirstSampleTrailer; //triler == 1

    Vec5 qCovTrailer;
    Vec5 qCovMheTrailer;         //Cov triler == 1
    Vec2 controlsCovTrailer;     //cov triler == 1

    Vec3 q;
    Vec4 qTrailerLoc;
    Vec3 qCarEst;
    Vec4 qCarEstMhe;
    Vec2 ctrlEstMhe;
    Vec5 qMheTrailer;
    Vec2 ctrMheTrailer;
    Vec4 qOneTrailerEst;
    int sampleNo;
    /*---------------------------------------------*/

    /*----------------| Get Casadi Solver  |----------------*/
    casadi::Function solverCar;
    casadi::Function solverTrailer;
    ROS_INFO_STREAM("Creating CasADi Solvers");
    mheSetupCar(solverCar, carParams, mheParams,N_mhe);
    mheSetupTrailer(solverTrailer, carParams, mheParams,N_mhe);
    ROS_INFO_STREAM("Car like solver: "<< solverCar <<"");
    ROS_INFO_STREAM("single wagon like solver: "<< solverTrailer <<"");

    casadi::DM argx0 = casadi::SX::zeros((4*(N_mhe + 1))+(2*N_mhe));  // 4 =  n_state    2 = n_control   //triler = 0
    casadi::DM argx0Trailer = casadi::SX::zeros((5*(N_mhe + 1))+(2*N_mhe));  // 4 =  n_state    2 = n_control  //triler =1
    /*------------------------------------------------------*/

    /*----------------| ROS Subscribers and Publishers  |-----------------------------------------*/

    auto canDataIn = nh.Input<CanData>("/processed_can_data");
    boost::shared_ptr<CanData> canData(new CanData());

    auto perceptionPoseCamIn = nh.Input<geometry_msgs::PoseStamped>("/pose_estimator/charger_pose/location_cam");
    auto perceptionPoseGpsIn = nh.Input<geometry_msgs::PoseStamped>("/pose_estimator/charger_pose/location_gps");
    boost::shared_ptr<geometry_msgs::PoseStamped> perceptionPose(new geometry_msgs::PoseStamped());

    auto perceptionTwistOut = nh.Output<geometry_msgs::Twist>("mhe_node/perception/twist");
    boost::shared_ptr<geometry_msgs::Twist> perceptionTwistData(new geometry_msgs::Twist());

    auto perceptionTwistMheOut = nh.Output<geometry_msgs::Twist>("mhe_node/mhe_estimated/twist");
    geometry_msgs::Twist perceptionTwistMheData;

    auto perceptionTwistWeightedOut = nh.Output<geometry_msgs::Twist>("mhe_node/weighted_estimated/twist");
    geometry_msgs::Twist perceptionTwistWeightedData;

    auto articulatedAnglesOut = nh.Output<ArticulatedAngles>("mhe_node/can_data/articulated_angles");
    ArticulatedAngles articulatedAnglesData;

    auto articulatedAnglesMheOut = nh.Output<ArticulatedAngles>("mhe_node/mhe_estimated/articulated_angles");
    ArticulatedAngles articulatedAnglesMheData;

    auto articulatedAnglesWeightedOut = nh.Output<ArticulatedAngles>("mhe_node/weighted_estimated/articulated_angles");
    ArticulatedAngles articulatedAnglesWeightedData;

    auto poseWithCovarianceIn = nh.Input<geometry_msgs::PoseWithCovarianceStamped>("mhe_node/perception_data/pose_with_covariance");
    //geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceData;

    auto poseWithCovarianceMheOut = nh.Output<geometry_msgs::PoseWithCovarianceStamped>("mhe_node/mhe_estimated/pose_with_covariance");
    geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceMheData;

    auto poseWithCovarianceWeightedOut = nh.Output<geometry_msgs::PoseWithCovarianceStamped>("mhe_node/weighted_estimated/pose_with_covariance");
    geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceWeightedData;

    auto ackermannDriveOut = nh.Output<ackermann_msgs::AckermannDrive>("mhe_node/can_data/ackermann_drive");
    ackermann_msgs::AckermannDrive ackermannDriveData;

    auto ackermannDriveMheOut = nh.Output<ackermann_msgs::AckermannDrive>("mhe_node/mhe_estimated/ackermann_drive");
    ackermann_msgs::AckermannDrive ackermannDriveMheData;

    //auto ackermannDriveWeightedOut = nh.Output<ackermann_msgs::AckermannDrive>("mhe_node/weighted_estimated/ackermann_drive");
    //ackermann_msgs::AckermannDrive ackermannDriveWeightedData;
    /*------------------------------------------------------------------------------------------*/

    ROS_INFO_STREAM("Estimator loop rate: "<< mheParams.loopRate <<"");
    Real t = 0;
    bool firstPoseMeasured = false;
    ::ros::Rate loop_rate(mheParams.loopRate);
    while(::ros::ok())
    {

        auto now = ::ros::Time::now();

        *canData = canDataIn();

        ackermannDriveData.speed = canData->tachoVelocity;
        ackermannDriveData.steering_angle = canData->steeringAngle;
        auto& poseWithCovarianceData = poseWithCovarianceIn();

        if (!mheParams.covarianceFromTopicStamp)
        {
            if(mheParams.perceptionGPS)
            {
                *perceptionPose = perceptionPoseGpsIn();
            }else
            {
                *perceptionPose = perceptionPoseCamIn();
            }
        } else
        {
            perceptionPose->pose = poseWithCovarianceData.pose.pose;
            perceptionPose->header.stamp = poseWithCovarianceData.header.stamp;
        }

        auto perceptionTh = tf::getYaw(perceptionPose->pose.orientation);
        ::ros::Duration timeDiff = perceptionPose->header.stamp - lastPerceptionTime;
        lastPerceptionTime = perceptionPose->header.stamp;
        bool isPerceptionPoseFresh = timeDiff.toSec() >= 0.01;

        perceptionTwistData->linear.x = perceptionPose->pose.position.x;
        perceptionTwistData->linear.y = perceptionPose->pose.position.y;
        perceptionTwistData->angular.z = perceptionTh;
        articulatedAnglesData.trailer1 = canData->beta1;
        if(perceptionPose->pose.position.x != 0 && perceptionPose->pose.position.y != 0)
        {
            if (carParams.TrailerNumber == 0)
            {

                //Th base on estimator
                qLoc[1] = perceptionPose->pose.position.x - carParams.L*cos(perceptionTh);
                qLoc[2] = perceptionPose->pose.position.y - carParams.L*sin(perceptionTh);
                qLoc[3] = canData->steeringAngle;

                if(mheParams.mheActive)
                {
                    controlsCov[0] = 0;
                    controlsCov[1] = 1/mheParams.noiseVarianceLinearVel;
                    if (!mheParams.covarianceFromTopicStamp)
                    {
                        qCov[0] = 1/mheParams.noiseVarianceTh;
                        qCov[1] = 1/mheParams.noiseVariancePos;
                        qCov[2] = 1/mheParams.noiseVariancePos;
                    }else
                    {
                        qCov[0] = poseWithCovarianceData.pose.covariance.elems[35];
                        qCov[1] = poseWithCovarianceData.pose.covariance.elems[0];
                        qCov[2] = poseWithCovarianceData.pose.covariance.elems[7];
                    }

                    qCov[3] = 1/mheParams.noiseVariancesteering;

                    qLoc[0] = continuousAngle(perceptionTh, qCarEstMhe[0]);
                    controls[0] = 0;  //no measured value for dbeta
                    controls[1] = canData->tachoVelocity; //uCar.longitudinalVelocity + noiseArray[4];

                    std::rotate(q4wLoc.begin(), q4wLoc.begin()+1, q4wLoc.end());
                    std::rotate(control2w.begin(), control2w.begin()+1, control2w.end());
                    std::rotate(q4wCov.begin(), q4wCov.begin()+1, q4wCov.end());
                    std::rotate(control2wCov.begin(), control2wCov.begin()+1, control2wCov.end());

                    ::ros::Duration sampleDiff = ::ros::Time::now() - perceptionPose->header.stamp;
                    int sampleNumber = floor(sampleDiff.toSec()/(1.0/mheParams.loopRate));
                    if (sampleNumber > N_mhe)
                    {
                        ROS_WARN_STREAM("WARNING: localization delay = "<< sampleNumber<<" sample" );
                    }
                    if(sampleNumber >=0  && sampleNumber < N_mhe)
                    {
                        if(!firstPoseMeasured)
                        {
                          ROS_INFO_STREAM("First config "<<qLoc[0] <<" "<< qLoc[1]<<" "<< qLoc[2]<<" "<<qLoc[3]<<" ");
                          for(size_t i = 0; i < q4wCov.size(); ++i)
                          {
                            q4wCov[i] = qCov;
                            q4wLoc[i] = qLoc;
                          }
                          qEstFirstSample = qLoc;

                          for(size_t i = 0; i < control2w.size(); ++i)
                          {
                              controls[0] = 0.0;
                              controls[1] = 0.0;
                              control2w[i] = controls;
                              control2wCov[i] = controlsCov;
                          }
                          sampleNo = sampleNumber;
                          ROS_INFO_STREAM("feeding at: N_mhe - "<< sampleNo << " index");
                          ROS_INFO("First pose received. Starting.");
                          firstPoseMeasured = true;
                        }

                        q4wCov[N_mhe] = {0.0, 0.0, 0.0, 0.0};
                        //-----   canData to window -------//
                        q4wCov[N_mhe][3] = qCov[3];
                        q4wLoc[N_mhe][3] = qLoc[3];

                        control2w[N_mhe-1] = controls;
                        control2wCov[N_mhe-1] = controlsCov;
                        //---------------------------------//

                        // ---------perception to window -------//
                        q4wCov[N_mhe - sampleNo][0] = qCov[0];
                        q4wCov[N_mhe - sampleNo][1] = qCov[1];
                        q4wCov[N_mhe - sampleNo][2] = qCov[2];

                        q4wLoc[N_mhe - sampleNo][0] = qLoc[0];
                        q4wLoc[N_mhe - sampleNo][1] = qLoc[1];
                        q4wLoc[N_mhe - sampleNo][2] = qLoc[2];
                        //---------------------------------//

                    }

                    q4wLoc[0] = qEstFirstSample;
                    auto qCovF = qCov;
                    qCovF[0] = qCov[0]*5;
                    qCovF[1] = qCov[1]*5;
                    qCovF[2] = qCov[2]*5;
                    qCovF[3] = qCov[3]*5;
                    q4wCov[0] = qCovF;

                    estimateMhe(argx0, q4wLoc, control2w, q4wCov, control2wCov, carParams, mheParams, solverCar);
                    std::vector<double> resx = std::vector<double>(argx0);
                    qCarEstMhe[0] = resx[(4*(N_mhe+1))-4]; //theta    return estimated 4 = n_states
                    qCarEstMhe[1] = resx[(4*(N_mhe+1))-3]; //x
                    qCarEstMhe[2] = resx[(4*(N_mhe+1))-2]; //y
                    qCarEstMhe[3] = resx[(4*(N_mhe+1))-1]; //beta0

                    qEstFirstSample[0] = resx[0]; //theta return estimated 4 = n_states
                    qEstFirstSample[1] = resx[1]; //x
                    qEstFirstSample[2] = resx[2]; //y
                    qEstFirstSample[3] = resx[3]; //beta0

                    ctrlEstMhe[0] = resx[(4*(N_mhe+1))+(4*N_mhe)-4]; //dbeta
                    ctrlEstMhe[1] = resx[(4*(N_mhe+1))+(4*N_mhe)-3]; //u2

                    qCovMhe[0] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-4];//theta
                    qCovMhe[1] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-3];//x
                    qCovMhe[2] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-2];//y
                    qCovMhe[3] = resx[(4*(N_mhe+1))+(4*N_mhe)+(4*(N_mhe+1))-1];//beta0

                    poseWithCovarianceMheData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qCarEstMhe[0]);
                    poseWithCovarianceMheData.pose.pose.position.x = qCarEstMhe[1] + carParams.L*cos(qCarEstMhe[0]);
                    poseWithCovarianceMheData.pose.pose.position.y = qCarEstMhe[2] + carParams.L*sin(qCarEstMhe[0]);

                    poseWithCovarianceMheData.pose.covariance.elems[0] = qCovMhe[1];
                    poseWithCovarianceMheData.pose.covariance.elems[7] = qCovMhe[2];
                    poseWithCovarianceMheData.pose.covariance.elems[35] = qCovMhe[0];
                    poseWithCovarianceMheOut(poseWithCovarianceMheData);

                    ackermannDriveMheData.steering_angle = qCarEstMhe[3];
                    ackermannDriveMheData.speed = ctrlEstMhe[1];
                    ackermannDriveMheData.steering_angle_velocity = ctrlEstMhe[0];
                    ackermannDriveMheOut(ackermannDriveMheData);

                    perceptionTwistMheData.angular.z = qCarEstMhe[0];
                    perceptionTwistMheData.linear.x = qCarEstMhe[1] + carParams.L*cos(qCarEstMhe[0]);
                    perceptionTwistMheData.linear.y = qCarEstMhe[2] + carParams.L*sin(qCarEstMhe[0]);
                    perceptionTwistMheOut(perceptionTwistMheData);

                    perceptionTwistOut(perceptionTwistData);
                    articulatedAnglesOut(articulatedAnglesData);
                    ackermannDriveOut(ackermannDriveData);

                }
                if(mheParams.WeightedActive)
                {
                    qLoc[0] = continuousAngle(perceptionTh, qCarEst[0]);
                    q = {qLoc[0],qLoc[1],qLoc[2]};
                    auto simFunc = [&](const Vec3& q, Vec3& dq, const double t)
                    {
                        //if(!carParams.moveGuidancePoint)
                            dq = RDCarKinematicsGPRear(carParams, q, ackermannDriveData);
                        //else
                        //  dq = RDCarKinematicsGPFront(carParams, q, ackermannDriveData);
                    };
                    Vec3 qCarPred = qCarEst;
                    boost::numeric::odeint::integrate(simFunc, qCarPred, 0.0, 1.0/(Real)mheParams.loopRate, 1.0/(Real)mheParams.loopRate);

                    if(!isPerceptionPoseFresh)
                    {
                        qCarEst = qCarPred;
                    }
                    else
                    {
                        //ROS_INFO("fresh gps pose");
                        estimateEst(qCarEst, q, qCarPred ,mheParams);
                    }
                    poseWithCovarianceWeightedData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qCarEst[0]);
                    poseWithCovarianceWeightedData.pose.pose.position.x = qCarEst[1] + carParams.L*cos(qCarEst[0]);
                    poseWithCovarianceWeightedData.pose.pose.position.y = qCarEst[2] + carParams.L*sin(qCarEst[0]);
                    poseWithCovarianceWeightedData.header.stamp = ::ros::Time::now();
                    poseWithCovarianceWeightedOut(poseWithCovarianceWeightedData);

                    perceptionTwistWeightedData.angular.z = qCarEst[0];
                    perceptionTwistWeightedData.linear.x = qCarEst[1] + carParams.L*cos(qCarEst[0]);
                    perceptionTwistWeightedData.linear.y = qCarEst[2] + carParams.L*sin(qCarEst[0]);
                    perceptionTwistWeightedOut(perceptionTwistWeightedData);
                }

            }else if (carParams.TrailerNumber == 1)
            {

                qLocTrailer[0] = canData->beta1;
                //Th base on estimator is different
                qLocTrailer[2] = perceptionPose->pose.position.x - carParams.L*cos(perceptionTh);
                qLocTrailer[3] = perceptionPose->pose.position.y - carParams.L*sin(perceptionTh);
                qLocTrailer[4] = canData->steeringAngle;

                if(mheParams.mheActive)
                {
                    controlsCovTrailer[0] = 0;
                    controlsCovTrailer[1] = 1/mheParams.noiseVarianceLinearVel;

                    qCovTrailer[0] = 1/mheParams.noiseVarianceTrailer1;
                    if (!mheParams.covarianceFromTopicStamp)
                    {
                        qCovTrailer[1] = 1/mheParams.noiseVarianceTh;
                        qCovTrailer[2] = 1/mheParams.noiseVariancePos;
                        qCovTrailer[3] = 1/mheParams.noiseVariancePos;
                    }else
                    {
                        qCovTrailer[1] = poseWithCovarianceData.pose.covariance.elems[35];
                        qCovTrailer[2] = poseWithCovarianceData.pose.covariance.elems[0];
                        qCovTrailer[3] = poseWithCovarianceData.pose.covariance.elems[7];
                    }

                    qCovTrailer[4] = 1/mheParams.noiseVariancesteering;

                    qLocTrailer[1] = continuousAngle(perceptionTh, qMheTrailer[1]);
                    controlsTrailer[0] = 0; //no measured value for dbeta
                    controlsTrailer[1] = canData->tachoVelocity; //uCar.longitudinalVelocity + noiseArray[4];

                    std::rotate(q5wLocTrailer.begin(), q5wLocTrailer.begin()+1, q5wLocTrailer.end());
                    std::rotate(control2wTrailer.begin(), control2wTrailer.begin()+1, control2wTrailer.end());
                    std::rotate(q5wCovTrailer.begin(), q5wCovTrailer.begin()+1, q5wCovTrailer.end());
                    std::rotate(control2wCovTrailer.begin(), control2wCovTrailer.begin()+1, control2wCovTrailer.end());

                    ::ros::Duration sampleDiff = ::ros::Time::now() - perceptionPose->header.stamp;
                    int sampleNumber = floor(sampleDiff.toSec()/(1.0/mheParams.loopRate));
                    if (sampleNumber > N_mhe)
                    {
                        ROS_WARN_STREAM("WARNING: localization delay = "<< sampleNumber<<" sample" );
                    }
                    if(sampleNumber >=0  && sampleNumber < N_mhe)
                    {
                        if(!firstPoseMeasured)
                        {
                          ROS_INFO_STREAM("First config "<<qLocTrailer[0] <<" "<< qLocTrailer[1]<<" "<< qLocTrailer[2]<<" "<<qLocTrailer[3]<<" "<<qLocTrailer[4]);

                          for(size_t i = 0; i < q5wCovTrailer.size(); ++i)
                          {
                            q5wCovTrailer[i] = qCovTrailer;
                            q5wLocTrailer[i] = qLocTrailer;
                          }

                          qEstFirstSampleTrailer = qLocTrailer;

                          for(size_t i = 0; i < control2wTrailer.size(); ++i)
                          {
                            controlsTrailer[0] = 0.0;
                            controlsTrailer[1] = 0.0;
                            control2wTrailer[i] = controlsTrailer;
                            control2wCovTrailer[i] = controlsCovTrailer;
                          }
                          sampleNo = sampleNumber;
                          ROS_INFO_STREAM("feeding at: N_mhe - "<< sampleNo << " index");
                          ROS_INFO("First pose received. Starting.");
                          firstPoseMeasured = true;
                        }

                        q5wCovTrailer[N_mhe] = {0.0, 0.0, 0.0, 0.0, 0.0};

                        //-----   canData to window -------//
                        q5wCovTrailer[N_mhe][0] = 1/mheParams.noiseVarianceTrailer1;
                        q5wCovTrailer[N_mhe][4] = 1/mheParams.noiseVariancesteering;

                        q5wLocTrailer[N_mhe][0] = qLocTrailer[0];
                        q5wLocTrailer[N_mhe][4] = qLocTrailer[4];

                        control2wTrailer[N_mhe-1] = controlsTrailer;
                        control2wCovTrailer[N_mhe-1] = controlsCovTrailer;
                        //---------------------------------//

                        // ---------perception to window -------//
                        q5wCovTrailer[N_mhe -sampleNo][1] = qCovTrailer[1];
                        q5wCovTrailer[N_mhe -sampleNo][2] = qCovTrailer[2];
                        q5wCovTrailer[N_mhe -sampleNo][3] = qCovTrailer[3];

                        q5wLocTrailer[N_mhe -sampleNo][1] = qLocTrailer[1];
                        q5wLocTrailer[N_mhe -sampleNo][2] = qLocTrailer[2];
                        q5wLocTrailer[N_mhe -sampleNo][3] = qLocTrailer[3];
                        //--------------------------------------------//

                    }

                    q5wLocTrailer[0] = qEstFirstSampleTrailer;
                    auto qCovTrailerF = qCovTrailer;
                    qCovTrailerF[0] = 5/mheParams.noiseVarianceTrailer1;

                    qCovTrailerF[1] = qCovTrailer[1]*5;
                    qCovTrailerF[2] = qCovTrailer[2]*5;
                    qCovTrailerF[3] = qCovTrailer[3]*5;

                    qCovTrailerF[4] = 5/mheParams.noiseVariancesteering;
                    q5wCovTrailer[0] = qCovTrailerF;

                    control2wTrailer[N_mhe-1] = controlsTrailer;
                    control2wCovTrailer[N_mhe-1] = controlsCovTrailer;

                    estimateMheTrailer(argx0Trailer, q5wLocTrailer, control2wTrailer, q5wCovTrailer, control2wCovTrailer, carParams, mheParams,solverTrailer);

                    std::vector<double> resxTrailer = std::vector<double>(argx0Trailer);
                    qMheTrailer[0] = resxTrailer[(5*(N_mhe+1))-5]; //beta1                return estimated 4 = n_states
                    qMheTrailer[1] = resxTrailer[(5*(N_mhe+1))-4]; //theta
                    qMheTrailer[2] = resxTrailer[(5*(N_mhe+1))-3]; //x
                    qMheTrailer[3] = resxTrailer[(5*(N_mhe+1))-2]; //y
                    qMheTrailer[4] = resxTrailer[(5*(N_mhe+1))-1]; //beta0

                    qEstFirstSampleTrailer[0] = resxTrailer[0]; //beta1                return estimated 4 = n_states
                    qEstFirstSampleTrailer[1] = resxTrailer[1]; //theta
                    qEstFirstSampleTrailer[2] = resxTrailer[2]; //x
                    qEstFirstSampleTrailer[3] = resxTrailer[3]; //y
                    qEstFirstSampleTrailer[4] = resxTrailer[4]; //beta0

                    ctrMheTrailer[0] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)-5]; //dbeta
                    ctrMheTrailer[1] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)-4]; //u2

                    qCovMheTrailer[0] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-5]; //beta1
                    qCovMheTrailer[1] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-4]; //theta
                    qCovMheTrailer[2] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-3]; //x
                    qCovMheTrailer[3] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-2]; //y
                    qCovMheTrailer[4] = resxTrailer[(5*(N_mhe+1))+(5*N_mhe)+(5*(N_mhe+1))-1]; //beta0

                    poseWithCovarianceMheData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qMheTrailer[1]);
                    poseWithCovarianceMheData.pose.pose.position.x = qMheTrailer[2] + carParams.L*cos(qMheTrailer[1]);
                    poseWithCovarianceMheData.pose.pose.position.y = qMheTrailer[3] + carParams.L*sin(qMheTrailer[1]);

                    poseWithCovarianceMheData.pose.covariance.elems[0] = qCovMheTrailer[2];
                    poseWithCovarianceMheData.pose.covariance.elems[7] = qCovMheTrailer[3];
                    poseWithCovarianceMheData.pose.covariance.elems[35] = qCovMheTrailer[1];
                    poseWithCovarianceMheOut(poseWithCovarianceMheData);

                    ackermannDriveMheData.steering_angle = qMheTrailer[4];
                    ackermannDriveMheData.speed = ctrMheTrailer[1];
                    ackermannDriveMheData.steering_angle_velocity = ctrMheTrailer[0];
                    ackermannDriveMheOut(ackermannDriveMheData);

                    perceptionTwistMheData.angular.z = qMheTrailer[1];
                    perceptionTwistMheData.linear.x = qMheTrailer[2] + carParams.L*cos(qMheTrailer[1]);
                    perceptionTwistMheData.linear.y = qMheTrailer[3] + carParams.L*sin(qMheTrailer[1]);
                    perceptionTwistMheOut(perceptionTwistMheData);

                    articulatedAnglesMheData.trailer1 = qMheTrailer[0];
                    articulatedAnglesMheOut(articulatedAnglesMheData);

                    perceptionTwistOut(perceptionTwistData);
                    articulatedAnglesOut(articulatedAnglesData);
                    ackermannDriveOut(ackermannDriveData);

                }
                if(mheParams.WeightedActive)
                {

                    qLocTrailer[1] = continuousAngle(perceptionTh, qOneTrailerEst[1]);
                    qTrailerLoc = {qLocTrailer[0],qLocTrailer[1],qLocTrailer[2],qLocTrailer[3] };
                    auto simFunc = [&](const Vec4& qTrailer, Vec4& dq, const double t)
                    {
                        if(!carParams.moveGuidancePoint)
                            dq = OneTrailerKinematicsGPRear(carParams, qTrailer, ackermannDriveData);
                        else
                            dq = OneTrailerKinematicsGPFront(carParams, qTrailer, ackermannDriveData);
                    };

                    Vec4 qOneTrailerPred = qOneTrailerEst;
                    boost::numeric::odeint::integrate(simFunc, qOneTrailerPred, 0.0, 1.0/(Real)mheParams.loopRate, 1.0/(Real)mheParams.loopRate);

                    if(!isPerceptionPoseFresh)
                    {
                        qOneTrailerEst = qOneTrailerPred;
                    }
                    else
                    {
                        ROS_INFO("fresh gps pose");
                        estimateEstTrailer(qOneTrailerEst, qTrailerLoc, qOneTrailerPred, mheParams);
                        //betaEst = estParams.weightBeta*betaPred + (1-estParams.weightBeta)*(beta);
                    }
                    poseWithCovarianceWeightedData.pose.pose.orientation = tf::createQuaternionMsgFromYaw(qOneTrailerEst[1]);
                    poseWithCovarianceWeightedData.pose.pose.position.x = qOneTrailerEst[2]  + carParams.L*cos(qOneTrailerEst[1]);
                    poseWithCovarianceWeightedData.pose.pose.position.y = qOneTrailerEst[3]  + carParams.L*sin(qOneTrailerEst[1]);
                    poseWithCovarianceWeightedData.header.stamp = ::ros::Time::now();
                    poseWithCovarianceWeightedOut(poseWithCovarianceWeightedData);

                    articulatedAnglesWeightedData.trailer1 = qOneTrailerEst[0];
                    articulatedAnglesWeightedOut(articulatedAnglesWeightedData);

                    perceptionTwistWeightedData.angular.z = qOneTrailerEst[1];
                    perceptionTwistWeightedData.linear.x = qOneTrailerEst[2] + carParams.L*cos(qOneTrailerEst[1]);
                    perceptionTwistWeightedData.linear.y = qOneTrailerEst[3] + carParams.L*sin(qOneTrailerEst[1]);
                    perceptionTwistWeightedOut(perceptionTwistWeightedData);

                }

            }else
            {
                ROS_ERROR(" Trailer Number out of range, accepted values: 0, 1");
            }

        }

        auto loopTime =  ::ros::Time::now() - now;
        ROS_INFO_STREAM("loop time" << loopTime.toSec());

        t += 1/((Real) mheParams.loopRate);
        ::ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}


/*
 *  tutte.cpp
 *
 *
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright (c) 2011-2014, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#include "chinese_remainder.hpp"
#include "graph_type.hpp"
#include "parse_graph.hpp"
#include "transfer.hpp"
#include "tree_decomposition/heuristics.hpp"
#include "tree_decomposition/tree_decomposition.hpp"
#include "tutte.hpp"
#include "utility/gmp.hpp"
#include "utility/polynomial_two.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/algorithm/equal.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>

#include <exception>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

const char manifesto[] =
  "tutte - computes the Tutte Polynomial\n"
  "Copyright (c) 2011-2014, Andrea Bedini <andrea.bedini@gmail.com>.\n\n"
  "Distributed under the terms of the Modified BSD License.\n"
  "The full license is in the file COPYING, distributed as part of\n"
  "this software.\n";

/*
 * code to parse user-supplied elimination order
 */

template<class OutputIterator>
void parse_elimination_order(std::string const& s, OutputIterator out)
{
  boost::tokenizer<> tok(s);
  for (auto& c : tok)
    *out++ = boost::lexical_cast<unsigned>(c);
}

/*
 *  validate the ordering by checking if it's a permutation of [0..num_vertices)
 */
template<class Range, class Graph>
bool validate_elimination_order(Range range, Graph const& g)
{
  using namespace boost;
  return range::equal(sort(range), irange((size_t) 0, num_vertices(g)));
}

/*
 *  The algorithm to run, dependent on the weight type
 */

template<typename T>
using algo = tutte<polynomial_two<T>>;

int main (int argc, char *argv[])
{
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "Produce help message")
    ("input-file", po::value<std::string>(), "Read the graph from a file.")
    // tree decomposition options
    ("degree", "Use greedy degree algorithm [default].")
    ("fill-in", "Use greedy fill-in algorithm.")
    ("local-degree", "Use 'local' greedy degree algorithm.")
    ("local-fill-in", "Use 'local' greedy fill-in algorithm.")
    ("elimination-order", po::value<std::string>(), "Specify a vertex elimination order.")
    ("print-tree", "Print tree decomposition.")
    ("tree-only", "Print tree decomposition and exit.")
    // tutte options
    ("flow,f", "Compute the flow polynomial")
    ("chromatic,c", "Compute the chromatic polynomial")
    ("Q,Q", po::value<int32_t>(), "Fix Q value, to be used with v")
    ("v,v", po::value<int32_t>(), "Fix v value, to be used with Q")
    ("chinese-remainder", "Use the chinese remainder trick.")
    ;

  po::variables_map vm;

  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
  } catch (po::error& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  if (vm.count("help")) {
    std::cerr << manifesto << "\n" << desc << "\n";
    return 0;
  }

  int check = 0;
  check += vm.count("degree");
  check += vm.count("fill-in");
  check += vm.count("local-degree");
  check += vm.count("local-fill-in");
  check += vm.count("elimination-order");

  if (check > 1) {
    std::cerr <<
      "error: please specify at most one between degree, fill-in,"
      "local-degree, local-fill-in and elimination-order\n";
    return 1;
  }

  graph_type g;
  try {
    std::string s;
    if (vm.count("input-file")) {
      std::string filename = vm["input-file"].as<std::string>();
      std::ifstream input(filename.c_str(), std::ios_base::in);
      if (not input.is_open()) {
        std::cerr << "error: file " << filename << " not found\n";
        return 1;
      }
      input >> s;
    } else {
      std::cin >> s;
    }
    g = parse_graph(s);
  } catch (std::exception& e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  std::cerr << "Graph with " << num_vertices(g) << " vertices and "
            << num_edges(g) << " edges.\n";

  // check for connectedness
  auto component = boost::make_vector_property_map<int>(
    get(boost::vertex_index, g));
  auto num = connected_components(g, component);
  if (num > 1) {
    std::cerr << "The input graph is not connected. A connected input is required\n";
    return 1;
  }

  std::vector<unsigned int> order(num_vertices(g));

  if (vm.count("fill-in")) {
    heuristics::greedy_fillin_order(g, order.begin());
  } else if (vm.count("local-degree")) {
    heuristics::greedy_local_degree_order(g, order.begin());
  } else if (vm.count("local-fill-in")) {
    heuristics::greedy_local_fillin_order(g, order.begin());
  } else if (vm.count("elimination-order")) {
    // parse the std::string
    std::string s = vm["elimination-order"].as<std::string>();
    parse_elimination_order(s, order.begin());
    bool valid = validate_elimination_order(order, g);
    if (not valid) {
      std::cerr << "error: elimination order not valid\n";
      return 1;
    }
    std::cerr << "Vertex ordering: " << s << "\n";
  } else {
    heuristics::greedy_degree_order(g, order.begin());
  }

  auto td = tree_decomposition::build_tree_decomposition(order, g);

  if (vm.count("print-tree") or vm.count("tree-only")) {
    std::cerr << "Elimination order: ";
    for (auto x : order)
      std::cerr << x << " ";
    std::cerr << "\n";

    std::cerr << "Tree decomposition: " << td << "\n"
              << "Tree decomposition width: "
              << max_bag_size(td) - 1 << "\n";
  }

  if (vm.count("tree-only"))
    return 0;

  if (vm.count("Q") && vm.count("v")) {
    std::cerr << "Running with fixed values of Q and v\n";
    auto Q = vm["Q"].as<int32_t>();
    auto v = vm["v"].as<int32_t>();
    chinese_remainder::chinese_remainder<tutte>(td, Q, v);
  } else {
    auto Q = polynomial_two<int>::Q();
    auto v = polynomial_two<int>::v();

    if (vm.count("flow")) {
      v = -Q;
    } else if (vm.count("chromatic")) {
      v = -1;
    }

    if (vm.count("chinese-remainder")) {
      chinese_remainder::chinese_remainder<algo>(td, Q, v);
    } else {
      using gmp::mpz_int;
      auto result = transfer::transfer(algo<mpz_int>(Q, v), td);
      std::cout << result << "\n";
    }
  }
}


// Boost.Units

#include <iostream>

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/io.hpp>

int main()
{
    boost::units::quantity<boost::units::si::mass> w = 20.6 * boost::units::si::kilograms + 10.2 * boost::units::si::kilograms;
    std::cout << w << std::endl;
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/BrainStimulator/BiotSavartSolverAlgorithm.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Math/MiscMath.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Thread/Barrier.h>
#include <Core/Thread/Parallel.h>
#include <string>
#include <cassert>
#include <memory>
#include <iomanip>
#include <locale>
#include <Core/Logging/Log.h>
#include <boost/lexical_cast.hpp>

using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::BrainStimulator;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Thread;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun::Core::Logging;
using namespace SCIRun;

ALGORITHM_PARAMETER_DEF(BrainStimulator, Mesh);
ALGORITHM_PARAMETER_DEF(BrainStimulator, Coil);
ALGORITHM_PARAMETER_DEF(BrainStimulator, VectorBField);
ALGORITHM_PARAMETER_DEF(BrainStimulator, VectorAField);
ALGORITHM_PARAMETER_DEF(BrainStimulator, OutType);

class KernelBase
{

public:
	KernelBase(const AlgorithmBase* algo, int t) :
	  algo_(algo),
	  numprocessors_(Parallel::NumCores()),
	  barrier_("BSV KernelBase Barrier", numprocessors_),
	  typeOut_(t)
	{
	}

	virtual ~KernelBase()
	{
	}

	//! Local entry function, must be implemented by each specific kernel
	virtual bool integrate(FieldHandle& mesh, FieldHandle& coil, DenseMatrixHandle& outdata) = 0;

protected:

	//! ref to the executing algorithm context
	const AlgorithmBase* algo_;
	unsigned int numprocessors_;

	//! model miscs.
	VMesh* vmesh_ {nullptr};
	VField* vfield_ {nullptr};
	size_type modelSize_ {0};

	//! coil miscs.
	VMesh* vcoil_ {nullptr};
	VField* vcoilField_ {nullptr};
	size_type coilSize_ {0};

	//! parallel essential primitives
	Barrier barrier_;
	std::vector<bool> success_;

	//! output Field
	int typeOut_;
	DenseMatrixHandle matOut_;

	bool preIntegration( FieldHandle& mesh, FieldHandle& coil )
	{
		vmesh_ = mesh->vmesh();
		vcoil_ = coil->vmesh();
		vfield_ = mesh->vfield();
		vcoilField_ = coil->vfield();

		numprocessors_ = Parallel::NumCores();

		#ifdef _DEBUG
			//! DEBUG when we want to test with one CPU only
			numprocessors_ = 1;
		#endif

		success_.resize(numprocessors_, true);

		//! get number of nodes for the model
		modelSize_ = vmesh_->num_nodes();
		assert(modelSize_ > 0);

		matOut_.reset(new DenseMatrix(static_cast<int>(modelSize_), 3));

    algo_->remark("Output matrix size: " + std::to_string(modelSize_) + "x3");
    algo_->remark("Number of processors:  " + boost::lexical_cast<std::string>(numprocessors_));
    algo_->remark("[Important] CPU usage will be very high while running this module. \r\nTo limit the number of cores used, adjust the maximum core setting in Preferences->Advanced");

		return true;
	}

	bool postIntegration(DenseMatrixHandle& outdata)
	{
		//! check for error
		for (size_t j=0; j < success_.size(); j++)
		{
			if (!success_[j]) return (false);
		}
		outdata = matOut_;
		return true;
	}
};

namespace
{
  template<class T>
  std::string formatWithCommas(const T& value)
  {
    std::stringstream ss;
    ss.imbue(std::locale(""));
    ss << std::fixed << value;
    return ss.str();
  }
}

class PieceWiseKernel : public KernelBase
{
public:
	PieceWiseKernel(const AlgorithmBase* algo, int t ) : KernelBase(algo,t)
	{
		//we keep last calculated step
		//however if segments lenght varies,
		//it makes more sense to keep a look-up table of previous steps for given lenght
		autostep_ = 0.1;
		extstep_ = -1.0;
	}

	//! Complexity O(M*N) ,where M is the number of nodes of the model and N is the numbder of nodes of the coil
	bool integrate(FieldHandle& mesh, FieldHandle& coil, DenseMatrixHandle& outdata) override
	{
		if (!preIntegration(mesh,coil))
		{
			return (false);
		}

    algo_->remark("Launching PieceWiseKernel. Size of computation remark coming soon..");

		vmesh_->synchronize(Mesh::NODES_E | Mesh::EDGES_E);

		VMesh::Node::array_type enodes;
		Point enode1, enode2;

		//! get numbder of nodes for the coil
		coilSize_ = vcoil_->num_nodes();

		//! basic assumption
		assert(modelSize_ > 0 && coilSize_ > 1);

		coilNodes_.clear();
		coilNodes_.reserve(coilSize_);

		for (VMesh::Edge::index_type i = 0; i < vcoil_->num_edges(); i++)
		{
			vcoil_->get_nodes(enodes,i);
			vcoil_->get_point(enode1,enodes[0]);
			vcoil_->get_point(enode2,enodes[1]);
			coilNodes_.push_back(Vector(enode1));
			coilNodes_.push_back(Vector(enode2));
		}

		//! Start the multi threaded
		Parallel::RunTasks([this](int i) { ParallelKernel(i); }, numprocessors_);

		return postIntegration(outdata);
	}

	void setIntegrationStep(double step)
	{
		assert(step >= 0.0);
		extstep_ = step;
	}

	double getIntegrationStep() const
	{
		return extstep_;
	}

private:
	//! integration step, will auto adapt
	double autostep_;
	//! integration step, externally provided
	double extstep_;
	//! keep nodes on the coil cached
	std::vector<Vector> coilNodes_;

	//! execute in parallel
	void ParallelKernel(int proc_num)
	{
		assert(proc_num >= 0);

		int cnt = 0;

		const index_type begins = (modelSize_ * proc_num) / numprocessors_;
		const index_type ends  = (modelSize_ * (proc_num+1)) / numprocessors_;

		assert( begins <= ends );

		//! buffer of points used for integration
		std::vector<Vector> integrPoints;
		integrPoints.reserve(256);

		//! keep previous step length
		//! used for optimization purpose
		double prevSegLen = 123456789.12345678;

		//! number of integration points
		int nips = 0;

    bool remarkedOnProblemSize = false;

		try
		{
			for (index_type iM = begins; iM < ends; iM++)
			{
        Point modelNodeP;
				vmesh_->get_node(modelNodeP, iM);
        const Vector modelNodeV(modelNodeP);

				// result
				Vector F;

				for (size_t iC0 = 0, iC1 =1, iCV = 0; iC0 < coilNodes_.size(); iC0+=2, iC1+=2, iCV++)
				{
          double currentFromField;
					vcoilField_->get_value(currentFromField,iCV);

					const double current = currentFromField == 0.0 ? 1.0 : currentFromField;
          auto absCurrent = std::fabs(current);

					Vector coilNodeThis;
					Vector coilNodeNext;

					if (current >= 0.0)
					{
						coilNodeThis = coilNodes_[iC0];
						coilNodeNext = coilNodes_[iC1];
					}
					else
					{
						coilNodeThis = coilNodes_[iC1];
						coilNodeNext = coilNodes_[iC0];
					}

					//! Length of the curve element
					Vector diffNodes = coilNodeNext - coilNodeThis;
					double newSegLen = diffNodes.length();

					//first check if externally suplied integration step is available and use it
					if (extstep_ > 0)
					{
						nips = newSegLen / extstep_;
					}
					else
					{
						//! optimization
						//! only recompute integration step only if segment length changes
						if (Abs(prevSegLen - newSegLen ) > 0.00000001)
						{
							prevSegLen = newSegLen;

							//auto adaptive integration step calculation
							nips = adjustNumberOfIntegrationPoints(newSegLen);
						}
					}

					if (nips < 3)
					{
						algo_->warning("integration step too big");
					}

					integrPoints.clear();

          if (!remarkedOnProblemSize && proc_num == 0)
          {
            auto problemSize = (ends - begins) * coilNodes_.size() * nips;
            algo_->remark("Per core load: " + formatWithCommas(problemSize) + " field computations.");
            algo_->remark("To speed up this module, reduce the number of nodes in either the input mesh or the coil, or pick a simpler algorithm.");
            remarkedOnProblemSize = true;
          }

					//! curve segment discretization
					for (int iip = 0; iip < nips; iip++)
					{

						double interpolant = static_cast<double>(iip) / static_cast<double>(nips);
						Vector v = Interpolate( coilNodeThis, coilNodeNext, interpolant );
						integrPoints.push_back( v );
					}

					//! integration step over line segment
					for (int iip = 0; iip < nips -1; iip++)
					{
            const auto piip = integrPoints[iip];
            const auto piip1 = integrPoints[iip+1];
						//! Vector connecting the infinitesimal curve-element
						Vector Rxyz = (piip + piip1) / 2  - modelNodeV;

						//! Infinitesimal curve-element components
						Vector dLxyz = piip1 - piip;

						double Rn = Rxyz.length();

						if (typeOut_ == 1)
						{
              //! check for distance between coil and model close to zero
              //! it might cause numerical stability issues with respect to the cross-product
              if (Rn < 0.00001)
              {
                algo_->warning("coil<->model distance approaching zero!");
              }
							//! Biot-Savart Magnetic Field
							F += 1.0e-7 * Cross( Rxyz, dLxyz ) * (absCurrent / (Rn*Rn*Rn) );
						}
						else if (typeOut_ == 2)
						{
							//! Biot-Savart Magnetic Vector Potential Field
							F += 1.0e-7 * dLxyz * (absCurrent / (Rn) );
						}
					}
				}

				matOut_->put(iM,0, F[0]);
				matOut_->put(iM,1, F[1]);
				matOut_->put(iM,2, F[2]);

				//! progress reporter
				if (proc_num == 0)
				{
					cnt++;
					if (cnt == 200)
					{
						cnt = 0;
						algo_->update_progress_max(iM, ends-begins);
					}
				}
			}
			success_[proc_num] = true;
		}
		catch (...)
		{
			algo_->error("PieceWiseKernel crashed while integrating");
			success_[proc_num] = false;
		}

		//! check point
		barrier_.wait();

		// Bail out if one of the processes failed
		for (size_t q = 0; q < numprocessors_; q++)
			if (!success_[q]) return;
	}

	//! Auto adjust accuracy of integration
	int adjustNumberOfIntegrationPoints(double len)
	{
		int minNP = 100;//more than 1 for sure
		int maxNP = 200;//no more than 1000
		int NP = 0;
		bool over = false;
		bool under = false;

		do
		{
			NP = ceil( len / autostep_ );

			under = NP < minNP;
			over = NP > maxNP;

			if (under) autostep_ *= 0.5;
			if (over) autostep_ *= 1.5;

		} while ( under || over );

		return NP;
	}
};

//! TODO
class VolumetricKernel : public KernelBase
{
public:
	using KernelBase::KernelBase;

	bool integrate(FieldHandle& mesh, FieldHandle& coil, DenseMatrixHandle& outdata) override
	{
		if (!preIntegration(mesh,coil))
		{
			return (false);
		}

    algo_->remark("Launching VolumetricKernel");

		//! get numbder of nodes for the coil
		coilSize_ = vcoil_->num_elems();

		//! basic assumption
		assert(modelSize_ > 0 && coilSize_ > 1);

		vmesh_->synchronize(Mesh::NODES_E | Mesh::EDGES_E);

		//! Start the multi threaded
		Parallel::RunTasks([this](int i) { ParallelKernel(i); }, numprocessors_);

		return postIntegration(outdata);
	}
private:
	void ParallelKernel(int proc_num)
	{
		assert(proc_num >= 0);

		int cnt = 0;
		Point modelNode;
		Point coilCenter;
		Vector current;

		const VMesh::Node::index_type begins = (modelSize_ * proc_num) / numprocessors_;
		const VMesh::Node::index_type ends  = (modelSize_ * (proc_num+1)) / numprocessors_;

		assert( begins <= ends );

		try
		{
			for (VMesh::Node::index_type iM = begins; iM < ends; iM++)
			{
				vmesh_->get_node(modelNode,iM);

				//! accumulatedresult
				Vector F, R;
				double evol = 0.0;
				double Rl;

				for (VMesh::Elem::index_type iC = 0; iC < coilSize_; iC++)
				{
					vcoilField_->get_value(current,iC);

					vcoilField_->get_center(coilCenter, iC);//auto resolve based on basis_order

					evol = vcoil_->get_volume(iC);

					R = coilCenter - modelNode;

					Rl = R.length();

					if (typeOut_ == 1)
					{
						//! Biot-Savart Magnetic Field
						F += Cross ( current , R ) * ( evol / (4.0 * M_PI * Rl) );
					}
					else if (typeOut_ == 2)
					{
						//! Biot-Savart Magnetic Vector Potential Field
						F += current * ( evol / (4.0 * M_PI * Rl) );
					}
				}

				matOut_->put(iM, 0, F[0]);
				matOut_->put(iM, 1, F[1]);
				matOut_->put(iM, 2, F[2]);

				//! progress reporter
				if (proc_num == 0)
				{
					cnt++;
					if (cnt == 200)
					{
						cnt = 0;
						algo_->update_progress_max(iM, ends - begins);
					}
				}
			}

			success_[proc_num] = true;
		}
		catch (...)
		{
			algo_->error("VolumetricKernel crashed while integrating");
			success_[proc_num] = false;
		}

		//! check point
		barrier_.wait();

		// Bail out if one of the processes failed
		for (size_t q = 0; q < numprocessors_; q++)
			if (!success_[q])
				return;
	}
};

//! Magnetic Dipoles solver
class DipolesKernel : public KernelBase
{
public:
	using KernelBase::KernelBase;

	bool integrate(FieldHandle& mesh, FieldHandle& coil, DenseMatrixHandle& outdata) override
	{
		if (!preIntegration(mesh,coil))
		{
			return false;
		}

    algo_->remark("Launching DipolesKernel");

		//! get number of nodes for the coil
		coilSize_ = vcoil_->num_elems();

		//! basic assumption
		assert(modelSize_ > 0 && coilSize_ > 1);

		//needed?
		vmesh_->synchronize(Mesh::NODES_E | Mesh::EDGES_E);

		//! Start the multi threaded
		Parallel::RunTasks([this](int i) { ParallelKernel(i); }, numprocessors_);

		return postIntegration(outdata);
	}

private:
	void ParallelKernel(int proc_num)
	{
		assert(proc_num >= 0);

		int cnt = 0;
		Point modelNode;
		Point dipoleLocation;
		Vector dipoleMoment;

		const VMesh::Node::index_type begins = (modelSize_ * proc_num) / numprocessors_;
		const VMesh::Node::index_type ends  = (modelSize_ * (proc_num+1)) / numprocessors_;

		assert( begins <= ends );

		try
		{
			for (VMesh::Node::index_type iM = begins; iM < ends; iM++)
			{
				vmesh_->get_node(modelNode,iM);

				//! accumulated result
				Vector F, R;
				double Rl;

				for (VMesh::Elem::index_type iC = 0; iC < coilSize_; iC++)
				{
					vcoilField_->get_value(dipoleMoment, iC);
					vcoilField_->get_center(dipoleLocation, iC);//auto resolve based on basis_order

					R = dipoleLocation - modelNode;
					Rl = R.length();

					if (typeOut_ == 1)
					{
						//! Biot-Savart Magnetic Field
						F += 1.0e-7 * ( 3 * R * Dot ( dipoleMoment, R ) / (Rl*Rl*Rl*Rl*Rl) - dipoleMoment / (Rl*Rl*Rl) ) ;
					}
					if (typeOut_ == 2)
					{
						//! Biot-Savart Magnetic Vector Potential Field
						F += 1.0e-7 * Cross ( dipoleMoment , R ) / (Rl*Rl*Rl) ;
					}
				}

				matOut_->put(iM, 0, F[0]);
				matOut_->put(iM, 1, F[1]);
				matOut_->put(iM, 2, F[2]);

				//! progress reporter
				if (proc_num == 0)
				{
					cnt++;
					if (cnt == 200)
					{
						cnt = 0;
						algo_->update_progress_max(iM, ends - begins);;
					}
				}
			}

			success_[proc_num] = true;
		}
		catch (...)
		{
			algo_->error(std::string("DipoleKernel crashed while integrating"));
			success_[proc_num] = false;
		}

		//! check point
		barrier_.wait();

		// Bail out if one of the processes failed
		for (size_t q = 0; q < numprocessors_; q++)
			if (!success_[q])
				return;
	}
};

bool BiotSavartSolverAlgorithm::run(FieldHandle mesh, FieldHandle coil, DenseMatrixHandle& outdata, int outtype) const
{
  if (!mesh)
  {
    error("No input domain field");
    return (false);
  }

  if (!coil)
  {
    error("No input coil source field");
    return (false);
  }

  if (coil->vfield()->basis_order() == -1)
  {
		error("Need data on coil mesh.");
		return (false);
  }

  if (coil->vmesh()->is_curvemesh())
  {
    if (coil->vfield()->is_constantdata() && coil->vfield()->is_scalar())
    {
      PieceWiseKernel pwk(this, outtype);
      if (!pwk.integrate(mesh,coil,outdata))
      {
				error("Aborted during integration");
				return (false);
      }
    }
    else
    {
      error("Curve mesh expected with constant scalar data.");
      return (false);
    }
  }
  else if (coil->vmesh()->is_pointcloudmesh())
  {
		if ((coil->vfield()->is_lineardata() || coil->vfield()->is_constantdata() ) && coil->vfield()->is_vector())
		{
			DipolesKernel dp(this, outtype);
			if (!dp.integrate(mesh,coil,outdata))
			{
				error("Aborted during integration");
				return (false);
			}
		}
		else
		{
			error("Pointcloud expected with linear vector data.");
			return (false);
		}
	}
  else if (coil->vmesh()->is_volume())
  {
		if (coil->vfield()->is_constantdata() && coil->vfield()->is_vector())
		{
			VolumetricKernel vp(this, outtype);
			if (!vp.integrate(mesh,coil,outdata))
			{
				error("Aborted during integration");
				return (false);
			}
		}
		else
		{
			error("Volumetric mesh expected with constant vector data.");
			return (false);
		}
	}
	else
	{
		error("Unsupported mesh type! Only curve or volumetric.");
		return (false);
	}

  return (true);
}

AlgorithmOutput BiotSavartSolverAlgorithm::run(const AlgorithmInput& input) const
{
	AlgorithmOutput output;

	auto mesh = input.get<Field>(Parameters::Mesh);
	auto coil = input.get<Field>(Parameters::Coil);

	auto oports = get(Parameters::OutType).toInt();
	std::map<int, DenseMatrixHandle> cases;
	if (oports == 3)
		cases = {{1, nullptr}, {2, nullptr}};
	else
		cases[oports] = nullptr;

  for (auto& c : cases)
  {
    if (!run(mesh, coil, c.second, c.first))
    {
      error("Error: Algorithm of BiotSavartSolver failed.");
    }
  }
	output[Parameters::VectorBField] = cases[1];
	output[Parameters::VectorAField] = cases[2];

	return output;
}


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

#include <vector>
#include <boost/numeric/mtl/mtl.hpp>
#include "SubElementAssembler.h"
#include "ScalableQuadrature.h"
#include "SubPolytope.h"

namespace compositeFEM
{

  SubElementAssembler::SubElementAssembler(Operator* op,
      const FiniteElemSpace* rowFeSpace_,
      const FiniteElemSpace* colFeSpace_)
    : StandardAssembler(op, NULL, NULL, NULL, NULL, rowFeSpace_, colFeSpace_)
  {
    /**
     * Create a scalable quadrature for subassembler and replace the original
     * quadrature of the subassembler with the scalable quadrature.
     */

    if (zeroOrderAssembler)
    {
      checkQuadratures();
      zeroOrderScalableQuadrature =
        new ScalableQuadrature(zeroOrderAssembler->getQuadrature());
      zeroOrderAssembler->setQuadrature(zeroOrderScalableQuadrature);
    }
    else
    {
      zeroOrderScalableQuadrature = NULL;
    }

    if (firstOrderAssemblerGrdPsi)
    {
      checkQuadratures();
      firstOrderGrdPsiScalableQuadrature =
        new ScalableQuadrature(firstOrderAssemblerGrdPsi->getQuadrature());
      firstOrderAssemblerGrdPsi->setQuadrature(firstOrderGrdPsiScalableQuadrature);
    }
    else
    {
      firstOrderGrdPsiScalableQuadrature = NULL;
    }

    if (firstOrderAssemblerGrdPhi)
    {
      checkQuadratures();
      firstOrderGrdPhiScalableQuadrature =
        new ScalableQuadrature(firstOrderAssemblerGrdPhi->getQuadrature());
      firstOrderAssemblerGrdPhi->setQuadrature(firstOrderGrdPhiScalableQuadrature);
    }
    else
    {
      firstOrderGrdPhiScalableQuadrature = NULL;
    }

    if (secondOrderAssembler)
    {
      checkQuadratures();
      secondOrderScalableQuadrature =
        new ScalableQuadrature(secondOrderAssembler->getQuadrature());
      secondOrderAssembler->setQuadrature(secondOrderScalableQuadrature);
    }
    else
    {
      secondOrderScalableQuadrature = NULL;
    }
  }

  void SubElementAssembler::scaleQuadratures(const SubElInfo& subElInfo)
  {
    if (zeroOrderAssembler)
    {
      zeroOrderScalableQuadrature->scaleQuadrature(subElInfo);
    }
    if (firstOrderAssemblerGrdPsi)
    {
      firstOrderGrdPsiScalableQuadrature->scaleQuadrature(subElInfo);
    }
    if (firstOrderAssemblerGrdPhi)
    {
      firstOrderGrdPhiScalableQuadrature->scaleQuadrature(subElInfo);
    }
    if (secondOrderAssembler)
    {
      secondOrderScalableQuadrature->scaleQuadrature(subElInfo);
    }
  }

  void SubElementAssembler::getSubElementVector(SubElInfo* subElInfo,
      const ElInfo* elInfo,
      DenseVector<double>& userVec)
  {
    /**
     * Manipulate the quadratures of the SubAssemblers for subelement.
     */
    scaleQuadratures(*subElInfo);

    calculateElementVector(elInfo, userVec);

    /**
     * The integration has been performed with a quadrature living on element. The
     * determinant of element has been used instead of the determinant of subelement. Thus
     * the result must be corrected with respect to subelement.
     */
    double corrFactor = subElInfo->getDet() / fabs(elInfo->getDet());
    for (int i = 0; i < nRow; i++)
      userVec[i] *= corrFactor;
  }

  void SubElementAssembler::getSubElementMatrix(SubElInfo* subElInfo,
      const ElInfo* elInfo,
      ElementMatrix& userMat)
  {
    /**
     * Manipulate the quadratures of the SubAssemblers for subelement.
     */
    scaleQuadratures(*subElInfo);

    /**
     * Integrate using the manipulated quadrature.
     */
    calculateElementMatrix(elInfo, userMat);

    /**
     * The integration has been performed with a quadrature living on element. The
     * determinant of element has been used instead of the determinant of subelement.
     * Thus the result must be corrected with respect to subelement.
     */
    double corrFactor = subElInfo->getDet() / fabs(elInfo->getDet());
    for (int i = 0; i < nRow; i++)
    {
      for (int j = 0; j < nCol; j++)
      {
        userMat[i][j] *= corrFactor;
      }
    }
  }

  void SubElementAssembler::getSubPolytopeVector(SubPolytope* subPolytope,
      SubElementAssembler* subElementAssembler,
      const ElInfo* elInfo,
      DenseVector<double>& subPolVec)
  {
    /// Note: There is no reset of subPolVec.
    std::vector<SubElInfo*>::iterator it;
    DenseVector<double> subElVec(nRow);

    /// Assemble for each subelement of subpolytope.
    for (it = subPolytope->getSubElementsBegin();
         it != subPolytope->getSubElementsEnd();
         it++)
    {
      set_to_zero(subElVec);
      subElementAssembler->getSubElementVector(*it, elInfo, subElVec);

      /// Add results for subelement to total result for subpolytope.
      subPolVec += subElVec;
    }
  }

  void SubElementAssembler::getSubPolytopeMatrix(SubPolytope* subPolytope,
      SubElementAssembler* subElementAssembler,
      const ElInfo* elInfo,
      ElementMatrix& subPolMat)
  {
    /**
     * Note: There is no reset of subPolMat.
     */
    std::vector<SubElInfo*>::iterator it;
    ElementMatrix subElMat(nRow, nCol);

    /**
     * Assemble for each subelement of subpolytope.
     */
    for (it = subPolytope->getSubElementsBegin();
         it != subPolytope->getSubElementsEnd();
         it++)
    {
      set_to_zero(subElMat);
      subElementAssembler->getSubElementMatrix(*it, elInfo, subElMat);

      /**
       * Add results for subelement to total result for subpolytope.
       */
      for (int i = 0; i < nRow; i++)
        for (int j = 0; j < nCol; j++)
          subPolMat[i][j] += subElMat[i][j];
    }
  }

}


/*
    Lightmetrica - A modern, research-oriented renderer

    Copyright (c) 2015 Hisanari Otsu

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
*/

#include <lightmetrica/lightmetrica.h>
#include "radiosityutils.h"
#include <sstream>
#include <boost/format.hpp>
#if LM_COMPILER_MSVC
#pragma warning(disable:4714)
#pragma warning(disable:4701)
#pragma warning(disable:4456)
#include <Eigen/Sparse>
#else
#include <eigen3/Eigen/Sparse>
#endif

#define LM_RADIOSITY_DEBUG 0

namespace Eigen
{
    template <>
    struct NumTraits<lightmetrica_v2::Vec3>
    {
        using T          = lightmetrica_v2::Vec3;
        using VT         = lightmetrica_v2::Float;
        using Real       = T;
        using NonInteger = T;
        using Nested     = T;
        using Literal    = T;
        enum
        {
            IsComplex = 0,
            IsInteger = 0,
            IsSigned = 1,
            RequireInitialization = 1,
            ReadCost = 3,
            AddCost = 3,
            MulCost = 3
        };
        static inline T epsilon()         { return T(NumTraits<VT>::epsilon()); }
        static inline T dummy_precision() { return T(NumTraits<VT>::dummy_precision()); }
        static inline T highest()         { return T(std::numeric_limits<VT>::max()); }
        static inline T lowest()          { return T(std::numeric_limits<VT>::min()); }
    };

    namespace internal
    {
        template<>
        struct significant_decimals_impl<lightmetrica_v2::Vec3>
        {
            static inline int run()
            {
                return significant_decimals_impl<lightmetrica_v2::Float>::run();
            }
        };
    }
}

LM_NAMESPACE_BEGIN

// For Eigen
// https://eigen.tuxfamily.org/dox/TopicCustomizingEigen.html
auto abs(const Vec3& v) -> Vec3 { return Vec3(std::abs(v.x), std::abs(v.y), std::abs(v.z)); }
auto sqrt(const Vec3& v) -> Vec3 { return Vec3(std::sqrt(v.x), std::sqrt(v.y), std::sqrt(v.z)); }
auto log(const Vec3& v) -> Vec3 { return Vec3(std::log(v.x), std::log(v.y), std::log(v.z)); }
auto ceil(const Vec3& v) -> Vec3 { return Vec3(std::ceil(v.x), std::ceil(v.y), std::ceil(v.z)); }
auto operator<<(std::ostream& os, const Vec3& v) -> std::ostream&
{
    os << "(" << v.x << "," << v.y << "," << v.z << ")";
    return os;
}

/*!
    \brief Radiosity renderer.

    Implements the radiosity algorithm by directly solving linear system.
    This implementation currently only supports the diffues BSDF (`bsdf::diffuse`)
    and the area light (`light::area`).

    References:
      - [Cohen & Wallace 1995] Radiosity and realistic image synthesis
      - [Willmott & Heckbert 1997] An empirical comparison of radiosity algorithms
      - [Schroder & Hanrahan 1993] On the form factor between two polygons
*/
class Renderer_Radiosity final : public Renderer
{
public:

    LM_IMPL_CLASS(Renderer_Radiosity, Renderer);

public:

    LM_IMPL_F(Initialize) = [this](const PropertyNode* prop) -> bool
    {
        subdivLimitArea_ = prop->ChildAs<Float>("subdivlimitarea", 0.1_f);
        wireframe_ = prop->ChildAs<int>("wireframe", 0);
        return true;
    };

    LM_IMPL_F(Render) = [this](const Scene* scene_, Random* initRng, const std::string& outputPath) -> void
    {
        const auto* scene = static_cast<const Scene3*>(scene_);
        auto* film = static_cast<const Sensor*>(scene->GetSensor()->emitter)->GetFilm();

        // --------------------------------------------------------------------------------

        // Create patches
        Patches patches;
        patches.Create(scene, subdivLimitArea_);

        // --------------------------------------------------------------------------------

        #pragma region Setup matrices

        LM_LOG_INFO("Setup matrix");

        const int N = patches.Size();
        //using Matrix = Eigen::SparseMatrix<Vec3>;
        using Matrix = Eigen::Matrix<Vec3, Eigen::Dynamic, Eigen::Dynamic>;
        using Vector = Eigen::Matrix<Vec3, Eigen::Dynamic, 1>;

        // Setup emission term
        Vector E(N);
        for (int i = 0; i < N; i++)
        {
            const auto& patch = patches.At(i);
            const auto* light = patch.primitive->light;
            if (!light)
            {
                continue;
            }
            E(i) = light->Emittance().ToRGB();
        }

        // Setup matrix of interactions
        Matrix K(N, N);
        K.setIdentity();
        for (int i = 0; i < N; i++)
        {
            for (int j = 0; j < N; j++)
            {
                const auto Fij = RadiosityUtils::EstimateFormFactor(scene, patches.At(i), patches.At(j));
                if (Fij > 0_f)
                {
                    K.coeffRef(i, j) -= patches.At(i).primitive->bsdf->Reflectance().ToRGB() * Fij;
                }
            }

            const double progress = 100.0 * i / N;
            LM_LOG_INPLACE(boost::str(boost::format("Progress: %.1f%%") % progress));
        }

        LM_LOG_INFO("Progress: 100.0%");

        #pragma endregion

        // --------------------------------------------------------------------------------

        #pragma region Solve radiosity equation

        LM_LOG_INFO("Solving linear system");

        Eigen::BiCGSTAB<Matrix> solver;
        solver.compute(K);
        Vector B = solver.solve(E);

        #if LM_RADIOSITY_DEBUG
        std::stringstream ss;
        ss << B << std::endl;
        LM_LOG_INFO(ss.str());
        #endif

        #pragma endregion

        // --------------------------------------------------------------------------------

        #pragma region Rendering (ray casting)

        LM_LOG_INFO("Visualizing result");

        const int width  = film->Width();
        const int height = film->Height();
        for (int y = 0; y < height; y++)
        {
            for (int x = 0; x < width; x++)
            {
                // Raster position
                Vec2 rasterPos((Float(x) + 0.5_f) / Float(width), (Float(y) + 0.5_f) / Float(height));

                // Position and direction of a ray
                SurfaceGeometry geomE;
                Vec3 wo;
                scene->GetSensor()->emitter->SamplePositionAndDirection(rasterPos, Vec2(), geomE, wo);

                // Setup a ray
                Ray ray = { geomE.p, wo };

                // Intersection query
                Intersection isect;
                if (!scene->Intersect(ray, isect))
                {
                    // No intersection -> black
                    film->SetPixel(x, y, SPD());
                    continue;
                }

                // Compute patch index & visualize the radiosity
                patches.IteratePatches(isect, subdivLimitArea_, [&](size_t patchindex, const Vec2& uv) -> void
                {
                    if (wireframe_)
                    {
                        // Visualize wire frame
                        // Compute minimum distance from each edges
                        const auto mind = Math::Min(uv.x, Math::Min(uv.y, 1_f - uv.x - uv.y));
                        if (mind < 0.05f)
                        {
                            film->SetPixel(x, y, SPD(Math::Abs(Math::Dot(isect.geom.sn, -ray.d))));
                        }
                    }
                    else
                    {
                        film->SetPixel(x, y, SPD(B(patchindex)));
                    }
                });
            }

            if (y % 10 == 0)
            {
                const double progress = 100.0 * y / film->Height();
                LM_LOG_INPLACE(boost::str(boost::format("Progress: %.1f%%") % progress));
            }
        }

        LM_LOG_INFO("Progress: 100.0%");

        #pragma endregion

        // --------------------------------------------------------------------------------

        #pragma region Save image
        {
            LM_LOG_INFO("Saving image");
            LM_LOG_INDENTER();
            film->Save(outputPath);
        }
        #pragma endregion
    };

private:

    Float subdivLimitArea_;
    int wireframe_;

};

LM_COMPONENT_REGISTER_IMPL(Renderer_Radiosity, "renderer::radiosity");

LM_NAMESPACE_END


// Copyright 2020 Poofee (https://github.com/Poofee)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------
/*****************************************************************************
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *****************************************************************************
 *                                                                           *
 *  Authors: Poofee                                                          *
 *  Email:   poofee@qq.com                                                   *
 *  Address:                                                                 *
 *  Original Date: 2020-07-15                                                *
 *                                                                           *
 *****************************************************************************/
#include "relay1250.h"

#include <stdio.h>
#include <stdlib.h>
#include <cmath>
#if !defined(ARMA_32BIT_WORD)
#define ARMA_32BIT_WORD
#endif
#include <armadillo>
#include <vector>
#include <ctime>
#include <omp.h>

#include "SuperLU_MT.h"
#include "qcustomplot.h"
#include "slu_mt_ddefs.h"

using namespace std;
using namespace arma;

#define PI 3.14159265358979323846
#define r 2

const double miu0 = PI*4e-7;

Relay1250::Relay1250()
{

}

Relay1250::~Relay1250()
{

}

/*!
 \brief 参数初始化

*/
void Relay1250::init()
{
    /** 文件名，需不带扩展名 **/
    sprintf(fileName,"%s","JRS1250/JRS1250bgm");
    /** 初始化时间步长 **/
    current_step = 0;
    min_time = 0;
    max_time = 4e-3;
    for(double t = 1e-5; t < 3e-3; t+=1e-5){
        timesteps.emplace_back(1e-5);
    }
    for(double t = 3e-3; t <= 3e-3; t+=5e-5){
        timesteps.emplace_back(5e-5);
    }

    /** 初始化变量 **/
    current_step = 0;
    Ddisplacements.resize(timesteps.size()+1);
    Ddisplacements.at(current_step) = 0;
    displacements.resize(timesteps.size()+1);
    displacements.at(current_step) = 0;
    velocities.resize(timesteps.size()+1);
    velocities.at(current_step) = 0;
    accelerations.resize(timesteps.size()+1);
    accelerations.at(current_step) = 0;
    PhiCoil.resize(timesteps.size()+1);
    PhiCoil.at(current_step) = 0;
    ICoil.resize(timesteps.size()+1);
    ICoil.at(current_step) = 0;
    magforces.resize(timesteps.size()+1);
    magforces.at(current_step) = 0;
    UCoil.resize(timesteps.size()+1);
    UCoil.at(current_step) = 420;

    MAX_NONLINEARSTEPS = 20;
    /** 设置材料参数 **/
    CMaterial* air_material = new CMaterial;
    materialList.emplace_back(air_material);

    CMaterial* pm_material = new CMaterial;
    materialList.emplace_back(pm_material);
    pm_material->H_c = 1e6;
    pm_material->miu = 1.12;

    CMaterial* dt4e_material = new CMaterial;
    materialList.emplace_back(dt4e_material);
    dt4e_material->sigma = 1/(2e-7);
    dt4e_material->BHpoints = 20;
    double* Hdata = (double*)malloc(20*sizeof(double));
    double* Bdata = (double*)malloc(20*sizeof(double));
    /** 1250的BH曲线 **/

    CMaterial* coil_material = new CMaterial;
    materialList.emplace_back(coil_material);
    coil_material->tau = 60/(3e-3 * 17e-3);

    /** 设置映射关系 **/
    materialMap[1] = dt4e_material;/** 衔铁 **/
    materialMap[2] = coil_material;/** 线圈 **/
    materialMap[3] = dt4e_material;/** 铁芯 **/
    materialMap[4] = dt4e_material;/** 铁芯 **/
    materialMap[5] = dt4e_material;/** 铁芯 **/
    materialMap[6] = pm_material;/** 永磁 **/
    materialMap[7] = dt4e_material;/** 铁芯（导磁环） **/
    materialMap[8] = dt4e_material;/** 铁芯 **/
    materialMap[9] = air_material;/** 外部空气 **/
    materialMap[10] = air_material;/** 可压缩空气 **/
    /** 设置形变区域 **/
    tag_xiantie = 1;
    tag_air = 10;

    Precision = 1e-6;
}

/*!
 \brief 计算区域index的电磁力，如果没有指定的话，就按衔铁算。

 \param index
*/
void Relay1250::calcMagForce(int index)
{

}

/*!
 \brief 计算三角形单元的基本项

 \param index
*/
void Relay1250::makeTriangle(int index)
{
    int k,m,n;
    double p0,p1,p2,q0,q1,q2,area;
    k = pmeshele[index].n[0];
    m = pmeshele[index].n[1];
    n = pmeshele[index].n[2];

    p0 = pmeshnode[m].y - pmeshnode[n].y;
    pmeshele[index].P[0] = p0;
    p1 = pmeshnode[n].y - pmeshnode[k].y;
    pmeshele[index].P[1] = p1;
    p2 = pmeshnode[k].y - pmeshnode[m].y;
    pmeshele[index].P[2] = p2;

    q0 = pmeshnode[n].x - pmeshnode[m].x;
    pmeshele[index].Q[0] = q0;
    q1 = pmeshnode[k].x - pmeshnode[n].x;
    pmeshele[index].Q[1] = q1;
    q2 = pmeshnode[m].x - pmeshnode[k].x;
    pmeshele[index].Q[2] = q2;

    area = 0.5*abs(p1 * q2 - q1 * p2);
    pmeshele[index].AREA = area;
    pmeshele[index].rc = (pmeshnode[k].x +
                          pmeshnode[m].x +
                          pmeshnode[n].x) / 3;
    pmeshele[index].zc = (pmeshnode[k].y +
                          pmeshnode[m].y +
                          pmeshnode[n].y) / 3;

    int flag = 0;
    for (int f = 0; f < 3; f++) {
        if (pmeshnode[pmeshele[index].n[f]].x < 1e-7) {
            flag++;
        }
    }
    /** 计算三角形重心半径 **/
    if (flag == 2) {
        pmeshele[index].ydot = pmeshele[index].rc;
    } else {
        pmeshele[index].ydot  = 1 / (pmeshnode[k].x + pmeshnode[m].x);
        pmeshele[index].ydot += 1 / (pmeshnode[k].x + pmeshnode[n].x);
        pmeshele[index].ydot += 1 / (pmeshnode[m].x + pmeshnode[n].x);
        pmeshele[index].ydot = 1.5 / pmeshele[index].ydot;
    }

//    double Y11,Y12,Y13,Y22,Y23,Y33;
    pmeshele[index].Y11 = q0 * q0 + p0 * p0;
    pmeshele[index].Y12 = q0 * q1 + p0 * p1;
    pmeshele[index].Y13 = q0 * q2 + p0 * p2;
    pmeshele[index].Y22 = q1 * q1 + p1 * p1;
    pmeshele[index].Y23 = q1 * q2 + p1 * p2;
    pmeshele[index].Y33 = q2 * q2 + p2 * p2;

    pmeshele[index].Y11 /= 4. * area;
    pmeshele[index].Y12 /= 4. * area;
    pmeshele[index].Y13 /= 4. * area;
    pmeshele[index].Y22 /= 4. * area;
    pmeshele[index].Y23 /= 4. * area;
    pmeshele[index].Y33 /= 4. * area;
}

/*!
 \brief 使用传统的牛顿法进行求解

*/
void Relay1250::AxisNRsolve()
{
    /** 计时 **/
    clock_t time[10];
    int tt = 0;
    time[tt++] = SuperLU_timer_();
    /** 电路参数 **/
    double C1 = 200e-6;
    double C2 = 200e-6;
    double R1 = 1e3;
    double Rc = 28;
    double Y1= 1/R1;
    double Yc= 1/Rc;
    /** 变量 **/
    double beta = 1;
    /** 时间步循环 **/
    current_step = 0;
    double totalTime = 0;
    for(int i = 0; i < timesteps.size();i++){
        totalTime += timesteps.at(i);
        printf("Time step %d, current time is %lf s.\n",i,totalTime);
        /** remesh，要使用增量位移，向下为负，向上为正 **/
        remesh(0,Ddisplacements.at(i));
        /** 读取第i步的分网 **/
        loadMesh();
        /** 查找边界，默认一次边界 **/
        findBoundaryEdges(-1);
        findBoundaryPoints(-1);

        double* unknown_b = (double*)calloc(num_pts - boundaryPoints.size()+1, sizeof(double));
        /** 将边界点排序到末尾 **/
        int node_bdr = boundaryPoints.size();
        int node_all = allPoints.size();
        int num_dof = node_all - node_bdr + 1;/** 自由度：未知节点+电压 **/
        /** 初始化，提出读取的gmsh多余的节点以及边界点 **/
        for(int i2=0;i2<num_pts;i2++){
            pmeshnode[i2].bdr = 3;
        }
        for(int i2=0;i2<node_all;i2++){
            pmeshnode[allPoints.at(i2)].bdr = 0;
        }
        for(int i2=0;i2<node_bdr;i2++){
            pmeshnode[boundaryPoints.at(i2)].bdr = 3;
        }
        /** 计算固定区域的单元节点数目，起始编号为0 **/
        for(int i_tri = 0; i_tri < num_triangle; i_tri++){
            int i1 = num_ele-num_triangle+i_tri;
            if(pmeshele[i1].geometry_tag == tag_air){
                air_position = i_tri;
                break;
            }
        }

        umat locs(2, 9 * num_triangle);
        locs.zeros();
        mat vals(1, 9 * num_triangle);
        vec bbJz = zeros<vec>(num_pts);
        uvec node_reorder = zeros<uvec>(num_pts);
        uvec node_pos = zeros<uvec>(num_pts);
        vec bn = zeros<vec>(num_pts);
        vec A = zeros<vec>(num_pts);
        vec A_old = A;/** 上一步的大小肯定和这一步不一样 **/
        double Y11,Y12,Y13,Y22,Y23,Y33;

        for (int ibdr = 0; ibdr < num_pts; ibdr++) {
            if (pmeshnode[ibdr].bdr == 3) {
                node_bdr++;
                node_reorder(num_pts - node_bdr) = ibdr;
                node_pos(ibdr) = num_pts - node_bdr;
                pmeshnode[ibdr].A = 0;
                A(ibdr) = 0;
            } else {
                node_reorder(ibdr - node_bdr) = ibdr;
                node_pos(ibdr) = ibdr - node_bdr;
            }
        }
        /** 单元初始化，如果是非线性区域，可以考虑优化先初始值设置 **/
        for(int i_tri = 0; i_tri < num_triangle; i_tri++){
            int i1 = num_ele-num_triangle+i_tri;
            pmeshele[i1].B = 0;
            /** 对于涡流区域，没有形变，直接将miu带过来 **/
            if(i_tri < air_position){
                pmeshele[i1].miu = pmeshelelast[i1].miut;
                pmeshele[i1].miut = pmeshelelast[i1].miut;
            }else{
                pmeshele[i1].miu = miu0;
                pmeshele[i1].miut = miu0;
            }
        }
        double miuold = miu0;
        /** 非线性迭代求解 **/
        for(int non_iter = 0;non_iter < MAX_NONLINEARSTEPS;non_iter++){

            /** 有限元装配 **/
            double ce[3][3] = { {0} };/** 雅可比矩阵 **/
            double Je[3][3] = { {0} };/** 用于牛顿迭代 **/
            double Te[3][3] = { {0} };/** 涡流矩阵 **/
            double De[3] = { 0 };/** 电流密度矩阵 **/
            double dt = timesteps.at(non_iter);

            int pos = 0;
            for(int i_tri = 0; i_tri < num_triangle; i_tri++){
                int k,m,n,i1;
                i1 = num_ele-num_triangle+i_tri;
                k = pmeshele[i1].n[0];m = pmeshele[i1].n[1];n = pmeshele[i1].n[2];

                /** 计算除了磁导率部分的系数 **/
                makeTriangle(i1);

                /** 计算雅可比矩阵 **/
                double ydot = pmeshele[i1].ydot;
                double miut = pmeshele[i1].miut;
                miuold = pmeshele[i1].miu;
                /** 相当于C矩阵 **/
                Y11 = pmeshele[i1].Y11;Y22 = pmeshele[i1].Y22;Y33 = pmeshele[i1].Y33;
                Y12 = pmeshele[i1].Y12;Y13 = pmeshele[i1].Y13;Y23 = pmeshele[i1].Y23;

                /** 应当是当前迭代的mu **/
                ce[0][0] = Y11 / ydot;ce[1][1] = Y22 / ydot;ce[2][2] = Y33 / ydot;
                ce[0][1] = Y12 / ydot;ce[0][2] = Y13 / ydot;ce[1][2] = Y23 / ydot;
                ce[1][0] = ce[0][1];ce[2][0] = ce[0][2];ce[2][1] = ce[1][2];

                double v[3];
                v[0] = Y11*A(k) + Y12*A(m) + Y13*A(n);
                v[1] = Y12*A(k) + Y22*A(m) + Y23*A(n);
                v[2] = Y13*A(k) + Y23*A(m) + Y33*A(n);

                CMaterial* mat = materialMap[pmeshele[i1].geometry_tag];
                if (non_iter != 0) {
                    double tmp;
                    if (mat->BHpoints == 0) {
                        tmp = 0;
                    } else {
                        tmp = mat->getdvdB(pmeshele[i1].B);
                        if (pmeshele[i].B > 1e-9){
                            tmp /= pmeshele[i].B * pmeshele[i].AREA;//B==0?
                            tmp /= ydot * ydot * ydot;
                        }

                    }
                    Je[0][0] = v[0] * v[0] * tmp;Je[1][1] = v[1] * v[1] * tmp;Je[2][2] = v[2] * v[2] * tmp;
                    Je[0][1] = v[0] * v[1] * tmp;Je[0][2] = v[0] * v[2] * tmp;Je[1][2] = v[1] * v[2] * tmp;
                    Je[1][0] = Je[0][1];Je[2][0] = Je[0][2];Je[2][1] = Je[1][2];
                }

                /** 计算右侧向量 **/
                double dtmp = mat->tau * pmeshele[i1].AREA/3;
                De[0] = dtmp;De[1] = dtmp;De[2] = dtmp;
                double dtmp1 = 2*PI*Yc*dtmp*dtmp;
                /** 计算涡流矩阵 **/
                double etmp = mat->sigma/ydot*pmeshele[i1].AREA/12;
                Te[0][0] = etmp * 2+dtmp1;Te[1][1] = etmp* 2+dtmp1;Te[2][2] = etmp* 2+dtmp1;
                Te[0][1] = etmp+dtmp1;Te[0][2] = etmp+dtmp1;Te[1][2] = etmp+dtmp1;
                Te[1][0] = Te[0][1];Te[2][0] = Te[0][2];Te[2][1] = Te[1][2];

                for(int kk=0;kk<3;kk++){
                    for(int mm=0;mm<3;mm++){
                        Te[kk][mm] /= dt;
                        Je[kk][mm] += ce[kk][mm]/miut;
                        Je[kk][mm] += Te[kk][mm];
                    }
                }

                double jr = pmeshele[i].AREA*mat->Jr / 3;
                for (int j = 0; j < 3; j++) {
                    bbJz(pmeshele[i].n[j]) += jr;
                    /** 计算永磁部分 **/
                    bbJz(pmeshele[i].n[j]) -= mat->H_c / 2.*pmeshele[i].Q[j];
                }

                for (int row = 0; row < 3; row++) {
                    for (int col = 0; col < 3; col++) {
                        /** 判断节点是否在未知节点内 **/
                        /** 得到排序之后的编号 **/
                        int n_row = node_pos(pmeshele[i].n[row]);
                        int n_col = node_pos(pmeshele[i].n[col]);
                        if (n_row < num_pts - node_bdr && n_col < num_pts - node_bdr) {
                            locs(0, pos) = n_row;
                            locs(1, pos) = n_col;
                            vals(0, pos) = Je[row][col];
                            pos++;
                        }
                        /** 与雅可比矩阵相关的项 **/
//                        bn(pmeshele[i].n[row]) += Je[row][col] * A(pmeshele[i].n[col]);
                    }
                }
            }/** 单元循环结束 **/
            /** 右下角的点 **/
            locs(0, pos) = num_dof-1;
            locs(1, pos) = num_dof-1;
            vals(0, pos) = ((Y1+Yc)/dt-(C1+C2))/(2*PI);
            pos++;
            if (non_iter == 0) {
                locs.reshape(2, pos);
                vals.reshape(1, pos);
            }
            bn += bbJz;
            /** 调用线性求解器求解 **/
            /** 使用构造函数来生成稀疏矩阵 **/
            sp_mat X(true, locs, vals, num_pts - node_bdr, num_pts - node_bdr, true, true);

            for (int i = 0; i < num_pts - node_bdr; i++) {
                unknown_b[i] = bn(node_reorder(i));
            }
            //---------------------superLU_MT---------------------------------------
            CSuperLU_MT superlumt(num_pts - node_bdr, X, unknown_b);
            if (superlumt.solve() == 1) {
                printf("Error: superlumt.slove. Info:%d\n",superlumt.info);
                break;
            } else {
                double *sol = nullptr;
                A_old = A;
                sol = superlumt.getResult();

                for (int i = 0; i < num_pts - node_bdr; i++) {
                    pmeshnode[node_reorder(i)].A = sol[i];// / pmeshnode[i].x;//the A is r*A_real
                    A(node_reorder(i)) = sol[i];
                }
            }
            /** 更新磁场结果 **/
//            FILE *fp1 = fopen("B_T3_NR.txt", "w");
            for (int i = 0; i < num_triangle; i++) {
                double bx = 0;
                double by = 0;
                int i1 = num_ele-num_triangle+i;
                for (int j = 0; j < 3; j++) {
                    bx += pmeshele[i1].Q[j] * A(pmeshele[i1].n[j]);
                    by += pmeshele[i1].P[j] * A(pmeshele[i1].n[j]);
                }
                CMaterial* mat = materialMap[pmeshele[i1].geometry_tag];
                pmeshele[i].B = sqrt(bx*bx + by*by) / 2. / pmeshele[i].AREA / pmeshele[i1].ydot;
                pmeshele[i].Bx = bx / 2. / pmeshele[i1].AREA / pmeshele[i1].ydot;
                pmeshele[i].By = by / 2. / pmeshele[i1].AREA / pmeshele[i1].ydot;
                pmeshele[i].miut = mat->getMiu(pmeshele[i].B);
//                fprintf(fp1, "%lf \t %lf \t %lf \t %lf \t %lf\n", pmeshele[i].rc, pmeshele[i].zc, pmeshele[i].Bx, pmeshele[i].By, pmeshele[i].B);
//                y[i] = pmeshele[i].miut;
            }
//            fclose(fp1);
            double error = norm((A_old - A), 2) / norm(A, 2);
//            iter++;
            if (error < Precision) {
                break;
            }
            bn.zeros();
            pos = 0;

        }/** 牛顿迭代结束 **/

        /** 计算电磁力 **/
        calcMagForce(-1);

        /** 更新结果 **/
        current_step += 1;
        Ddisplacements.at(current_step) = 0;
        displacements.at(current_step) = 0;
        velocities.at(current_step) = 0;
        accelerations.at(current_step) = 0;
        PhiCoil.at(current_step) = 0;
        ICoil.at(current_step) = 0;
        magforces.at(current_step) = 0;
        UCoil.at(current_step) = 0;

        /** 回收空间 **/
        if(unknown_b) delete unknown_b;
    }

}

/*!
 \brief 使用传输线法进行求解

*/
void Relay1250::AxisTLMsolve()
{

}

/*!
 \brief 运行测试案例

*/
void Relay1250::run()
{
    init();
    openGeo();

    AxisNRsolve();

    /** 输出信息 **/
    outputResults();
}

/*!
 \brief 将一些结果进行输出。

*/
void Relay1250::outputResults()
{

}


/******************************************************************************
**
** Copyright (C) 2009-2011 Kyle Lutz <kyle.r.lutz@gmail.com>
** All rights reserved.
**
** This file is a part of the chemkit project. For more information
** see <http://www.chemkit.org>.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions
** are met:
**
**   * Redistributions of source code must retain the above copyright
**     notice, this list of conditions and the following disclaimer.
**   * Redistributions in binary form must reproduce the above copyright
**     notice, this list of conditions and the following disclaimer in the
**     documentation and/or other materials provided with the distribution.
**   * Neither the name of the chemkit project nor the names of its
**     contributors may be used to endorse or promote products derived
**     from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
** "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
** LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
** A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
** OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
** LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**
******************************************************************************/

#include "graphicstransform.h"

#include <Eigen/LU>

#include <chemkit/constants.h>

namespace chemkit {

// === GraphicsTransform =================================================== //
/// \class GraphicsTransform graphicstransform.h chemkit/graphicstransform.h
/// \ingroup chemkit-graphics
/// \brief The GraphicsTransform class represents a transformation
///        matrix.

// --- Construction and Destruction ---------------------------------------- //
/// Creates a new, empty graphics transform.
///
/// The tranformation returned is:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       0 & 0 & 0 & 0 \\
///       0 & 0 & 0 & 0 \\
///       0 & 0 & 0 & 0 \\
///       0 & 0 & 0 & 0 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform::GraphicsTransform()
{
    m_matrix = new Eigen::Matrix<float, 4, 4>();

    // set transformation matrix to all zeros
    m_matrix->setZero();
}

/// Creates a new transform as a copy of \p transform.
GraphicsTransform::GraphicsTransform(const GraphicsTransform &transform)
{
    m_matrix = new Eigen::Matrix<float, 4, 4>(*transform.m_matrix);
}

/// Creates a new transform that contains \p matrix.
GraphicsTransform::GraphicsTransform(const Eigen::Matrix<float, 4, 4> &matrix)
{
    m_matrix = new Eigen::Matrix<float, 4, 4>(matrix);
}

/// Destroys the graphics transform.
GraphicsTransform::~GraphicsTransform()
{
    delete m_matrix;
}

// --- Properties ---------------------------------------------------------- //
/// Returns the data for the transform.
///
/// Use the following code to load a GraphicsTransform into
/// OpenGL:
/// \code
/// glLoadMatrixf(transform.data());
/// \endcode
const float* GraphicsTransform::data() const
{
    return m_matrix->data();
}

// --- Math ---------------------------------------------------------------- //
/// Inverts the transform.
void GraphicsTransform::invert()
{
    *m_matrix = m_matrix->inverse();
}

/// Returns the inverted version of the transform.
GraphicsTransform GraphicsTransform::inverted() const
{
    GraphicsTransform transform = *this;
    transform.invert();
    return transform;
}

/// Multiplies \p ray by the transform.
GraphicsRay GraphicsTransform::multiply(const GraphicsRay &ray) const
{
    Point3f origin = multiplyPoint(ray.origin());
    Point3f direction = multiplyVector(ray.direction());

    return GraphicsRay(origin, direction);
}

/// Multiplies \p point by the transform.
Point3f GraphicsTransform::multiplyPoint(const Point3f &point) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = point.x();
    vector4[1] = point.y();
    vector4[2] = point.z();
    vector4[3] = 1;

    vector4 = *m_matrix * vector4;

    return Point3f(vector4[0], vector4[1], vector4[2]);
}

/// Multiplies \p vector by the transform.
Vector3f GraphicsTransform::multiplyVector(const Vector3f &vector) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = vector.x();
    vector4[1] = vector.y();
    vector4[2] = vector.z();
    vector4[3] = 0;

    vector4 = *m_matrix * vector4;

    return Vector3f(vector4[0], vector4[1], vector4[2]);
}

/// Multiplies \p transform by the transform.
GraphicsTransform GraphicsTransform::multiply(const GraphicsTransform &transform) const
{
    return GraphicsTransform(*m_matrix * *transform.m_matrix);
}

Eigen::Matrix<float, 4, 1> GraphicsTransform::multiply(const Eigen::Matrix<float, 4, 1> &vector) const
{
    return *m_matrix * vector;
}

/// Multiplies \p point by the inverse of the transform.
Point3f GraphicsTransform::inverseMultiplyPoint(const Point3f &point) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = point.x();
    vector4[1] = point.y();
    vector4[2] = point.z();
    vector4[3] = 1;

    vector4 = m_matrix->inverse() * vector4;

    return Point3f(vector4[0], vector4[1], vector4[2]);
}

/// Multiplies \p vector by the inverse of the transform.
Vector3f GraphicsTransform::inverseMultiplyVector(const Vector3f &vector) const
{
    Eigen::Matrix<float, 4, 1> vector4;
    vector4[0] = vector.x();
    vector4[1] = vector.y();
    vector4[2] = vector.z();
    vector4[3] = 0;

    vector4 = m_matrix->inverse() * vector4;

    return Vector3f(vector4[0], vector4[1], vector4[2]);
}

Eigen::Matrix<float, 4, 1> GraphicsTransform::inverseMultiply(const Eigen::Matrix<float, 4, 1> &vector) const
{
    return m_matrix->inverse() * vector;
}

// --- Operators ----------------------------------------------------------- //
float GraphicsTransform::operator()(int row, int column) const
{
    return m_matrix->operator()(row, column);
}

float& GraphicsTransform::operator()(int row, int column)
{
    return m_matrix->operator()(row, column);
}

GraphicsRay GraphicsTransform::operator*(const GraphicsRay &ray) const
{
    return multiply(ray);
}

Point3f GraphicsTransform::operator*(const Point3f &point) const
{
    return multiplyPoint(point);
}

GraphicsTransform GraphicsTransform::operator*(const GraphicsTransform &transform) const
{
    return multiply(transform);
}

GraphicsTransform& GraphicsTransform::operator*=(const GraphicsTransform &transform)
{
    *m_matrix *= *transform.m_matrix;
    return *this;
}

GraphicsTransform& GraphicsTransform::operator=(const GraphicsTransform &transform)
{
    *m_matrix = *transform.m_matrix;
    return *this;
}

// --- Static Methods ------------------------------------------------------ //
/// Returns the identity transform.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       1 & 0 & 0 & 0 \\
///       0 & 1 & 0 & 0 \\
///       0 & 0 & 1 & 0 \\
///       0 & 0 & 0 & 1 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::identity()
{
    GraphicsTransform transform;
    transform.m_matrix->setIdentity();
    return transform;
}

/// Returns a transformation matrix that represents the translation by
/// \p vector.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       1 & 0 & 0 & vector_{x} \\
///       0 & 1 & 0 & vector_{y} \\
///       0 & 0 & 1 & vector_{z} \\
///       0 & 0 & 0 & 1 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::translation(const Vector3f &vector)
{
    GraphicsTransform transform = identity();

    transform(0, 3) = vector.x();
    transform(1, 3) = vector.y();
    transform(2, 3) = vector.z();

    return transform;
}

/// Returns a transform that represents a rotation by \p angle
/// degrees around \p axis.
GraphicsTransform GraphicsTransform::rotation(const Vector3f &axis, float angle)
{
    GraphicsTransform transform = identity();

    Vector3f v = axis.normalized();
    float c = cos(angle * chemkit::constants::DegreesToRadians);
    float s = sin(angle * chemkit::constants::DegreesToRadians);

    transform(0, 0) = v.x() * v.x() + (1 - v.x() * v.x()) * c;
    transform(0, 1) = v.x() * v.y() * (1 - c) - v.z() * s;
    transform(0, 2) = v.x() * v.z() * (1 - c) + v.y() * s;
    transform(1, 0) = v.x() * v.y() * (1 - c) + v.z() * s;
    transform(1, 1) = v.y() * v.y() + (1 - v.y() * v.y()) * c;
    transform(1, 2) = v.y() * v.z() * (1 - c) - v.x() * s;
    transform(2, 0) = v.x() * v.z() * (1 - c) - v.y() * s;
    transform(2, 1) = v.y() * v.z() * (1 - c) + v.x() * s;
    transform(2, 2) = v.z() * v.z() + (1 - v.z() * v.z()) * c;

    return transform;
}

/// Returns a perspective transform.
///
/// The transformation returned is the following:
/// \f[ f = cot(\frac{angle}{2}) \f]
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       \frac{f}{aspectRatio} & 0 & 0 & 0 \\
///       0 & f & 0 & 0 \\
///       0 & 0 & \frac{nearDistance+farDistance}{nearDistance-farDistance} & \frac{2 \cdot nearDistance \cdot farDistance}{nearDistance-farDistance} \\
///       0 & 0 & -1 & 0 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::perspective(float angle, float aspectRatio, float nearDistance, float farDistance)
{
    GraphicsTransform transform;

    float f = 1.0 / tan(angle / 2.0);

    transform(0, 0) = f / aspectRatio;
    transform(1, 1) = f;
    transform(2, 2) = (nearDistance + farDistance) / (nearDistance - farDistance);
    transform(2, 3) = (2 * nearDistance * farDistance) / (nearDistance - farDistance);
    transform(3, 2) = -1;

    return transform;
}

/// Returns a frustum transform.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       \frac{2 \cdot nearDistance}{right-left} & 0 & \frac{right+left}{right-left} & 0 \\
///       0 & \frac{2 \cdot nearDistance}{top-bottom} & \frac{top+bottom}{top-bottom} & 0 \\
///       0 & 0 & -\frac{farDistance+nearDistance}{farDistance-nearDistance} & -\frac{2 \cdot farDistance \cdot nearDistance}{farDistance-nearDistance} \\
///       0 & 0 & -1 & 0 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::frustum(float left, float right, float top, float bottom, float nearDistance, float farDistance)
{
    GraphicsTransform transform;

    transform(0, 0) = (2 * nearDistance) / (right - left);
    transform(1, 1) = (2 * nearDistance) / (top - bottom);
    transform(2, 0) = (right + left) / (right - left);
    transform(2, 1) = (top + bottom) / (top - bottom);
    transform(2, 2) = -(farDistance + nearDistance) / (farDistance - nearDistance);
    transform(2, 3) = -(2 * farDistance * nearDistance) / (farDistance - nearDistance);
    transform(3, 2) = -1;

    return transform;
}

/// Returns a orthographic transform.
///
/// The transformation returned is the following:
/** \f[
///   \left[
///   {
///     \begin{array}{cccc}
///       \frac{2}{right-left} & 0 & 0 & -\frac{right+left}{right-left} \\
///       0 & \frac{2}{top-bottom} & 0 & -\frac{top+bottom}{top-bottom} \\
///       0 & 0 & -\frac{2}{farDistance-nearDistance} & -\frac{farDistance+nearDistance}{farDistance-nearDistance} \\
///       0 & 0 & 0 & 1 \\
///     \end{array}
///   }
///   \right]
/// \f]
**/
GraphicsTransform GraphicsTransform::orthographic(float left, float right, float top, float bottom, float nearDistance, float farDistance)
{
    GraphicsTransform transform;

    transform(0, 0) = 2.0 / (right - left);
    transform(0, 3) = -(right + left) / (right - left);
    transform(1, 1) = 2.0 / (top - bottom);
    transform(1, 3) = -(top + bottom) / (top - bottom);
    transform(2, 2) = -2.0 / (farDistance - nearDistance);
    transform(2, 3) = -(farDistance + nearDistance) / (farDistance - nearDistance);
    transform(3, 3) = 1;

    return transform;
}

} // end chemkit namespace