#include "openrave_rrtplugin/rrtHelper.h"

/*
#include <openrave/openrave.h>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <openrave/plugin.h>
#include <boost/bind.hpp>
#include <time.h>
#include <stdlib.h>

#define ERRORTHRESHOLD 0.002
#define GOALERROR 0.3
#define PI 3.141592653
#define STEPSIZE 0.2
#define SAMPLEBIAS 0.15
#define PERIOD 100
#define DOFCONFIG 6
#define STRETCHING_THRESHOLD 0
*/

using namespace std;
using namespace OpenRAVE;
using namespace Eigen;
using namespace EigenHelpers;

namespace orPlugin{

    configuration SE3toVconfig(Eigen::Affine3d tConfig)
    {
        configuration config;

        Vector3d vtrans = tConfig.translation();
        for (int i = 0; i< vtrans.size(); i++)
        {  config.push_back(vtrans(i));}

//        Matrix3d m = tConfig.linear();
//        Vector3d eulerAngle = m.eulerAngles(0, 1, 2);
        Vector3d eulerAngle = EulerAnglesFromAffine3d(tConfig);

        for (int i = 0; i< eulerAngle.size(); i++)
        {  config.push_back(eulerAngle(i)); }

        return config;
    }

    configSet SetSE3toVconfig(tConfigSet tConfigSetIn)
    {
        configSet config;
        config.push_back(SE3toVconfig(tConfigSetIn.first));
        config.push_back(SE3toVconfig(tConfigSetIn.second));

        return config;
    }

    Eigen::Affine3d VtoSE3config(configuration config)
    {
        Vector3d tran;
        Vector3d eulerAngel;
        for (int i = 0; i<3; i++)
        {
            tran(i) = config.at(i);
            eulerAngel(i) = config.at(i+3);
        }

//        Matrix3f m;
//        m = AngleAxisf(angle1, Vector3f::UnitZ()) *AngleAxisf(angle2, Vector3f::UnitY()) *AngleAxisf(angle3, Vector3f::UnitZ());

        Eigen::Affine3d res = TransformFromRPY(tran,eulerAngel);

        return res;
    }

    tConfigSet SetVtoSE3config(configSet config)
    {
        tConfigSet res;
        res.first = VtoSE3config(config.at(0));
        res.second = VtoSE3config(config.at(1));

        return res;
    }

    RRTNode::RRTNode(){ parent_=NULL;  }

    RRTNode::RRTNode(configSet configSetIn):configSet_(configSetIn)
    {
        parent_=NULL;
//        children_.clear();
    }

    RRTNode::~RRTNode()
    {
        parent_ = NULL;
//        children_.clear();
    }

    void RRTNode::SetConfig(configSet config)
    {
        configSet_ = config;
    }

    configSet RRTNode::GetConfig()
    {
        return configSet_;
    }

    void RRTNode::AddStep(configSet s)
    {
        configSet res;
        for (int i = 0; i<configSet_.size(); i++)
        {
            configuration sConfig = s.at(i);
            configuration config = configSet_.at(i);
            configuration sumres;
            for (int k = 0; k<config.size(); k++ )
            {
                sumres.push_back(sConfig.at(k)+config.at(k));
            }
            res.push_back(sumres);
        }
        configSet_.clear();
        for (int i = 0; i<res.size(); i++)
        {
            configSet_.push_back(res.at(i));
        }

    }

    RRTNode* RRTNode::GetParent()
    {
        return parent_;
    }

    void RRTNode::SetParent(RRTNodePtr parentNode)
    {
        parent_ = parentNode;

//        parentNode->SetChild(this);
    }
/*
    bool RRTNode::IsChild(RRTNodePtr nodePtr)
    {
        for (int i =0; i<children_.size(); i++)
        {
            if(SameNode(children_.at(i),nodePtr))
            {   return true;   }
        }
        return false;
    }

    void RRTNode::SetChild(RRTNodePtr nodePtr)
    {
        children_.push_back(nodePtr);
        nodePtr->SetParent(this);
    }

    std::vector<RRTNodePtr> RRTNode::GetChild()
    {
        return children_;
    }
*/

    ///////////////////// NodeTree Class ///////////////////////////
    /// \brief NodeTree::NodeTree
    /// \param initNode
    ///
    NodeTree::NodeTree(){}

    NodeTree::NodeTree(RRTNode *initNode):rootNode_(initNode)
    {
        tree_.push_back(rootNode_);
    }

    NodeTree::~NodeTree()
    {
        while(!tree_.empty()) {
               delete tree_.back();
               tree_.pop_back();
           }

        tree_.clear();
    }

    RRTNode* NodeTree::GetRoot()
    {
        return rootNode_;
    }

    void NodeTree::ResetTree()
    {
        while(!tree_.empty()) {
               delete tree_.back();
               tree_.pop_back();
           }

        tree_.clear();
    }

    bool NodeTree::InTree(RRTNodePtr testNode)
    {
        for (std::vector<RRTNode*>::iterator i=tree_.begin(); i!=tree_.end(); i++)
        {
            if(SameNode((*i),testNode)){ return true; }
        }
        return false;
    }

    // double check this function later, for parent pointer issue
    bool NodeTree::Add(RRTNodePtr growNode)
    {
        if (tree_.empty()) { rootNode_=growNode;  }
        if (!InTree(growNode))
        {
            tree_.push_back(growNode);
            return true;
        }
        return false;
    }

    bool NodeTree::Remove(RRTNode* remNode)
    {
        if (tree_.size()<=1)
        {
            tree_.clear();
            return false;
        }
        for(tree::iterator i = tree_.begin()+1; i!=tree_.end(); i++)
        {
            if(SameNode(((*i)->GetParent()),remNode))
                (*i)->SetParent(remNode->GetParent());
            if(SameNode(remNode,*i))
                tree_.erase(i);
        }
        return true;
    }

    RRTNode* NodeTree::GetNode(int index)
    {
        if(index<tree_.size())
            return tree_.at(index);
        return rootNode_;
    }

    int NodeTree::TreeSize()
    {
        return tree_.size();
    }

    int NodeTree::GetIndex(RRTNode * findNode)
    {
        for (int i = 0; i<tree_.size(); i++){
            if(SameNode(findNode, tree_.at(i)))
                return i;
        }
        return -1;
    }

    bool NodeTree::IsRoot(RRTNode *checkNode)
    {
        if(SameNode(checkNode, rootNode_))
            return true;
        return false;
    }

    std::vector<RRTNode*> NodeTree::GetPath(RRTNode* goal)
     {
         std::vector<RRTNode*> path;
         RRTNode* nodePtr = goal;
         while(!IsRoot(nodePtr)){
             path.insert(path.begin(),nodePtr);
             nodePtr = nodePtr->GetParent();
         }
         path.insert(path.begin(),nodePtr);
         return path;
     }

    /////////////////////// ParameterSet Class /////////////////////////////
    // the config here is in object configuration space, with x,y,z
    // geodesicConfig dimension: 3*n

    ParameterSet::ParameterSet()
    {
        weight.clear();
        weight.push_back(1);
        weight.push_back(1);
        weight.push_back(1);
        weight.push_back(0);
        weight.push_back(0);
        weight.push_back(0);
    }

    void ParameterSet::InitGeodesic(configSet initConfig, std::vector<RobotBasePtr> robots)
    {
        geodesicConfig=initConfig;

        if (numGrippers == 0) {numGrippers = initConfig.size();}

        for (int i=0; i<numGrippers; i++)
        {
            for (int j=0; j<numGrippers; j++)
            {
            //  get end-effector translation
                robots.at(i)->SetActiveDOFValues(ConfigToVec(initConfig.at(i)));
                robots.at(j)->SetActiveDOFValues(ConfigToVec(initConfig.at(j)));

                OpenRAVE::Vector iPosRave = GetObjectPos(robots.at(i));
                OpenRAVE::Vector jPosRave = GetObjectPos(robots.at(j));

                configuration iPos = RaveVecToConfig(iPosRave);
                configuration jPos = RaveVecToConfig(jPosRave);

            // initConfig is in p (object space)
                float dis = Distance(iPos,jPos) ;
                geodesic.push_back(dis);
            }
        }

    }

    void ParameterSet::SetStart(tConfigSet startSE3)
    {
        startSE3_ = startSE3;
        start_ = SetSE3toVconfig(startSE3);
    }

    void ParameterSet::SetStart(configSet start)
    {
        start_ = start;
        startSE3_ = SetVtoSE3config(start);
    }

    void ParameterSet::SetGoal(tConfigSet goalSE3)
    {
        goalSE3_ = goalSE3;
        goal_ = SetSE3toVconfig(goalSE3);
    }

    void ParameterSet::SetGoal(configSet goal)
    {
        goal_ = goal;
        goalSE3_ = SetVtoSE3config(goal);
    }

    ////////////////////////////////// Helper function //////////////////////////

    // euclidian distance
     float Distance(configuration A, configuration B)
     {
         float distance = 0;
         for(int i =0; i<A.size(); i++)
         {
             float ele_error;
             ele_error = std::pow(A.at(i)-B.at(i),2);
             distance = distance+ele_error;
         }
         return std::sqrt(distance);
     }

     bool SameNode(RRTNodePtr baseNode, RRTNodePtr checkNode)
     {
         configSet config = checkNode->GetConfig();
         configSet baseConfig = baseNode->GetConfig();

         float error = 0;

         for(int i =0; i<config.size(); i++)
         {
             float ele_error = Distance(config.at(i),baseConfig.at(i)) ;
             error = error+ele_error;
         }

         if (error<ERRORTHRESHOLD){ return true; }
         return false;
     }

     // Only for config.size = 3, Vector = tran
     OpenRAVE::Vector ConfigToRaveVec(configuration config)
     {
         OpenRAVE::Vector raveVec;
         raveVec.x = config.at(0);
         raveVec.y = config.at(1);
         raveVec.z = config.at(2);
     //    raveVec.w = 0.0;
         return raveVec;
     }

     // Get the end effector pos
     OpenRAVE::Vector GetObjectPos(OpenRAVE::RobotBasePtr robotPtr)
     {
         std::vector<KinBody::LinkPtr> iLPtr = robotPtr->GetLinks();
         OpenRAVE::Transform itrans = (iLPtr.at(6))->GetTransform();
         OpenRAVE::Vector ipos = itrans.trans;
         return ipos;
     }

     configuration RaveVecToConfig(OpenRAVE::Vector raveVec)
     {
         configuration pos;
         double x=raveVec.x;
         pos.push_back(x);
         double y=raveVec.y;
         pos.push_back(y);
         double z=raveVec.z;
         pos.push_back(z);
         return pos;
     }

     configuration VecToConfig(std::vector<double> vec)
     {
         configuration config;
         for (int i=0; i<vec.size(); i++)
         {
             config.push_back(vec.at(i));
         }
         return config;
     }

     std::vector<double> ConfigToVec(configuration config)
     {
         std::vector<double> vec;
         for (int i=0; i<config.size();i++)
             vec.push_back(config.at(i));
         return vec;
     }

     //////////////////////////////// RrtPlanner ///////////////////////////////

     void RrtPlanner::init(OpenRAVE::EnvironmentBasePtr env)
     {
         env_ = env;
         env->GetRobots(robots_);
     }

     void RrtPlanner::init(OpenRAVE::EnvironmentBasePtr env, ParameterSet parameterIn)
     {
         env_ = env;
         inputParameters_ = parameterIn;
         env->GetRobots(robots_);
     }

     void RrtPlanner::ResetPath()
     {
         treePtr_->ResetTree();
         path_.clear();
     }

     int RrtPlanner::RandNode(int size)
     {
         int index = rand()%size;
         return index;
     }

     configuration RrtPlanner::ScaleConfig(configuration v, float scale)
     {
         configuration a;
         for (int i =0; i<v.size(); i++)
         {
             a.push_back(v.at(i)/scale);
         }
         return a;
     }

     void RrtPlanner::ConfigPrintHelp(configuration config){
         cout << "config is " << endl;
         for (int i =0; i<config.size();i++)
         {
             cout << config.at(i) << endl;
         }
     }

     bool RrtPlanner::OutBound(configuration a)
     {
         for (int i=0; i<a.size(); i++)
         {
             if (a.at(i)<inputParameters_.lowerBound.at(i)||a.at(i)>inputParameters_.upperBound.at(i))
                 return true;
         }
         return false;
     }

     bool RrtPlanner::SameDirection(configuration a, configuration b){
        double result = 0;
        for (int i =1; i<a.size(); i++)
        {
            result = result +a.at(i)+b.at(i);
        }
        if(result > 0)
            return true;
        return false;
    }

     configuration RrtPlanner::SumConfig(configuration A, configuration B)
     {
         configuration sum;
         for (int i=0; i<A.size(); i++)
         {
             sum.push_back(A.at(i)+B.at(i));
         }
         return sum;
     }

     configuration RrtPlanner::SubtractConfig(configuration A, configuration B)
    {
        configuration sum;
        for (int i=0; i<A.size(); i++)
        {
            sum.push_back(A.at(i)-B.at(i));
        }
        return sum;
    }

     float RrtPlanner::WeightedDis(configuration A, configuration B)
     {
         float distance = 0;
         for(int i =0; i<A.size(); i++)
         {
             float ele_error;
             ele_error = inputParameters_.weight.at(i)*std::pow(A.at(i)-B.at(i),2);
             distance = distance+ele_error;
         }
         return std::sqrt(distance);

     }

     RRTNode* RrtPlanner::NearestNode(configSet config, NodeTreePtr treePtr)
     {
         RRTNode* nodePtr=treePtr->GetRoot();
         RRTNode* minNode = nodePtr;
         int index=0;
         float minDis=SetWeightedDis(nodePtr->GetConfig(),config);
         for(int i=0; i<treePtr->TreeSize(); i++)
         {
             nodePtr = treePtr->GetNode(i);
             float curDis = SetWeightedDis(nodePtr->GetConfig(),config);
             if(curDis<minDis)
             {
                 index =i;
                 minNode = nodePtr;
                 minDis = curDis;
             }
         }
         return treePtr->GetNode(index);
     }

     configuration RrtPlanner::RandSample()
     {
         configuration randConfig;
         for (int i=0; i<inputParameters_.lowerBound.size(); i++)
         {
             float randNum = (float)rand();
             randNum =randNum/(float)RAND_MAX *(inputParameters_.upperBound.at(i)-inputParameters_.lowerBound.at(i))+inputParameters_.lowerBound.at(i);
             randConfig.push_back(randNum);
         }
         return randConfig;
     }

     configuration RrtPlanner::UnitStep(configuration start, configuration goal, float stepSize)
     {
         configuration step;
         float length = Distance(start, goal);
         for (int i=0; i<start.size(); i++)
         {
             step.push_back((goal.at(i)-start.at(i))/length*stepSize*std::sqrt(inputParameters_.weight.at(i)));
         }
         return step;
     }

     //////////////////////////////////// New / Revised function from HW3 for Project
     ///

    void RrtPlanner::SetConfigPrintHelp(configSet config)
    {
        for (int i=0; i<config.size(); i++)
        {   ConfigPrintHelp(config.at(i));    }
    }

    bool RrtPlanner::SetOutBound(configSet a)
    {
        for(int i=0; i<a.size(); i++)
        { if(OutBound(a.at(i))) {return true;} }
        return false;
    }

    configSet RrtPlanner::SetScaleConfig(configSet v, float scale)
    {
        configSet config;
        for(int i=0; i<v.size(); i++)
        {
            config.push_back(ScaleConfig(v.at(i),scale));
        }
        return config;
    }

    configSet RrtPlanner::SetVecToConfig(std::vector<std::vector<double> > setVec)
    {
        configSet setConfig;
        for (int i=0; i<setVec.size(); i++)
        {
            setConfig.push_back(VecToConfig(setVec.at(i)));
        }
        return setConfig;
    }

    std::vector<std::vector<double> > RrtPlanner::SetConfigToVec(configSet setConfig)
    {
        std::vector<std::vector<double> > setVec;
        for (int i=0; i<setConfig.size(); i++)
        {
            setVec.push_back(ConfigToVec(setConfig.at(i)));
        }
        return setVec;
    }

    float RrtPlanner::SetWeightedDis(configSet A, configSet B)
    {
        float dis = 0;
        for (int i = 0; i< A.size(); i++)
        {
            dis = dis + WeightedDis(A.at(i), B.at(i));
        }
        return dis;
    }

    configSet RrtPlanner::SetSumConfig(configSet A, configSet B)
    {
        configSet setSum;
        for (int i =0; i< A.size(); i++)
        {
            setSum.push_back(SumConfig(A.at(i), B.at(i)));
        }
        return setSum;
    }

    // A-B
    configSet RrtPlanner::SetSubtractConfig(configSet A, configSet B)
    {
        configSet SetDiff;
        for (int i=0; i<A.size(); i++)
        {
            SetDiff.push_back(SubtractConfig(A.at(i), B.at(i)));
        }
        return SetDiff;
    }

    // Sampe configSet
    configSet RrtPlanner::SampleSet(int gripperSize)
    {
        configSet SetSample;
        for(int i =0; i<gripperSize; i++)
        {
            SetSample.push_back(RandSample());
        }
        return SetSample;
    }

    // No guarantee within violation still though
    configSet RrtPlanner::SampleSetWithBase(configuration baseSample, int index)
    {
        configSet setSample;
        for (int i=0; i<inputParameters_.numGrippers; i++)
        {
            if(i==index){setSample.push_back(baseSample);}
            else
            {
                // This sampling approach only works for the free gripper in space
                float radius = inputParameters_.geodesic.at(i*inputParameters_.numGrippers+index);
                configuration randConfig;
                for (int j = 0; j<baseSample.size(); j++)
                {
                    float randNum = (float)rand();
                    randNum =randNum/(float)RAND_MAX *2*radius+baseSample.at(j)-radius;
                    randConfig.push_back(randNum);
                }
                setSample.push_back(randConfig);
            }
        }
        return setSample;
    }

    // return unitstep vector, the step size is in #define, could be adjust later
    configSet RrtPlanner::SetUnitStep(configSet start, configSet goal, float stepSize)
    {
        configSet setStep;
        for (int i=0; i<start.size(); i++)
        {
            setStep.push_back(UnitStep(start.at(i), goal.at(i), stepSize));
        }
        return setStep;
    }

    ///////////// Rave related function

    bool RrtPlanner::ConstraintViolation(configSet sampleConfig)
    {
        for (int i=0; i<sampleConfig.size(); i++)
        {
            for (int j=i; j<sampleConfig.size(); j++)
            {

                float disMax = inputParameters_.geodesic.at(i*inputParameters_.numGrippers+j)+STRETCHING_THRESHOLD;
                // from sample config to object config
                robots_.at(i)->SetActiveDOFValues(ConfigToVec(sampleConfig.at(i)));
                robots_.at(j)->SetActiveDOFValues(ConfigToVec(sampleConfig.at(j)));

                OpenRAVE::Vector iPosRave = GetObjectPos(robots_.at(i));
                OpenRAVE::Vector jPosRave = GetObjectPos(robots_.at(j));

                configuration iPos = RaveVecToConfig(iPosRave);
                configuration jPos = RaveVecToConfig(jPosRave);

                // Geodesic Constraint checking
                if (Distance(iPos,jPos) > disMax)
                { return true; }

                // Checking whether obstacle appears in the space between the two robots, should be improved
                OpenRAVE::RAY rayCheck;
                // 0.2* ... some offset that the start point is not in contact
                rayCheck.pos = GetObjectPos(robots_.at(i))+0.2*(GetObjectPos(robots_.at(j))-(GetObjectPos(robots_.at(i))));
                rayCheck.dir= 0.5*(GetObjectPos(robots_.at(j))-(GetObjectPos(robots_.at(i))));
                vector<OpenRAVE::KinBodyPtr> kinbody;
                env_->GetBodies(kinbody);

                for (int kin = 0; kin < kinbody.size(); kin++ )
                {
                    if (env_->CheckCollision(rayCheck,kinbody.at(kin)))
                    {   return true;}
                }
            }
        }
        return false;
    }

    // Return true if collision happens
    bool RrtPlanner::SetCollisionCheck()
    {
        for (int i =0; i<robots_.size(); i++)
        {
            if(env_->CheckCollision(robots_.at(i))||robots_.at(i)->CheckSelfCollision())
            { return true; }
        }
        return false;
    }

     ///////////////////////////////////// RRT Planning //////////////////////////
     /// \brief RRTPlanning
     /// \param env
     /// \param startConfig
     /// \param goalConfig
     /// \return

     std::vector<RRTNodePtr> RrtPlanner::RRTPlanning(ParameterSet parameterIn)
     {
         inputParameters_ = parameterIn;

//         int numGripper = robots_.size();
//         inputParameters_.numGrippers=numGripper;

    //     OpenRAVE::RobotBasePtr robot = robots.at(0);

         if (inputParameters_.start_.empty())
         {

             for (int i =0; i<inputParameters_.numGrippers; i++)
             {
                 std::vector<double> startVec;
                 robots_.at(i)->GetActiveDOFValues(startVec);
                 inputParameters_.start_.push_back(startVec);
             }
             inputParameters_.startSE3_ = SetVtoSE3config(inputParameters_.start_);
         }

         configSet startSet = inputParameters_.start_;

         RRTNodePtr startPtr;
//         RRTNode startNode(SetVecToConfig(startSet));
         RRTNode startNode(startSet);
         startPtr = new RRTNode(startNode);
         RRTNodePtr nearestNode;

//         NodeTree *treePtr;
         ResetPath();
         treePtr_ = new NodeTree(startPtr);

         bool sampleGoal = false;

         RRTNode *tempNode;
         RRTNode* stopPtr;
         configSet stepConfig;
         float curStep = inputParameters_.stepSize;

         bool findSol=false;
         int count = 0;

         // Randseed
         srand(time(NULL));

         while((!findSol)&&(count<1000000))
         {
             curStep = inputParameters_.stepSize;
             float toss = (float)rand()/((float)RAND_MAX);
             configSet sampleConfig;
             configSet objectSampleConfig;

             if(toss<inputParameters_.sampleBias)
             {
                 sampleGoal = true;
                 sampleConfig = inputParameters_.goal_;
             }

             else
             {
                 sampleConfig = SampleSetWithBase(RandSample(),0);
             //     Constraint violation & collision status for ray across end effectors
                 while (ConstraintViolation(sampleConfig))
                 {  sampleConfig = SampleSetWithBase(RandSample(),0); }

             //    ConfigPrintHelp(sampleConfig.at(0));
                 sampleGoal=false;
             }

             nearestNode = NearestNode(sampleConfig, treePtr_);

             stopPtr = nearestNode;
             // if the sampling is at nearest node, return nearest node.

             if (!(SetWeightedDis(nearestNode->GetConfig(), sampleConfig)<ERRORTHRESHOLD))
             {

                 stepConfig = SetUnitStep(nearestNode->GetConfig(),sampleConfig, inputParameters_.stepSize);
                 bool withinStep = false;

                 if((SetWeightedDis(stopPtr->GetConfig(),sampleConfig))<STEPSIZE) {withinStep=true;}

                 int inner_count =0;
                 while(inner_count<30)
                 {
                     inner_count++;
                     tempNode = new RRTNode(stopPtr->GetConfig());
                     tempNode->AddStep(stepConfig);

                     if (SetOutBound(tempNode->GetConfig()))
                     {   delete tempNode;   tempNode = stopPtr;   break; }

                     if (ConstraintViolation(tempNode->GetConfig()))
                     {   delete tempNode;   tempNode = stopPtr;   break; }

                     for (int irob=0; irob<robots_.size(); irob++)
                     {   robots_.at(irob)->SetActiveDOFValues(ConfigToVec((tempNode->GetConfig()).at(irob))); }

                     if(SetCollisionCheck())
                     {
                         if (curStep>0.1)
                         {
                             curStep=curStep/2.0;
                             // should be scaled
                             stepConfig = SetScaleConfig(stepConfig,2.0);
                             delete tempNode;
                             continue;
                         }
                         else
                         {  delete tempNode;  tempNode = stopPtr;   break; }
                     }

                     // step; revise: no if before
                     if ((SetWeightedDis(stopPtr->GetConfig(),sampleConfig))>inputParameters_.stepSize)
                     {
                         tempNode->SetParent(stopPtr);
                         stopPtr = tempNode;
                         treePtr_->Add(tempNode);
                     }

                     if ((SetWeightedDis(stopPtr->GetConfig(),sampleConfig))<inputParameters_.stepSize)
                     {
                     //    cout << "distance smaller than step size!!" << endl;
                         if (sampleGoal)
                         {
                             RRTNode* show = NearestNode(inputParameters_.goal_, treePtr_);

                             tempNode = new RRTNode (inputParameters_.goal_);
                             tempNode->SetParent(stopPtr);
                             treePtr_->Add(tempNode);
                             findSol = true;

                             show = NearestNode(inputParameters_.goal_, treePtr_);
                         }  // if sample is goal, connect directly
                         else
                         {
                             for (int irob=0; irob<robots_.size(); irob++)
                             {
                                 robots_.at(irob)->SetActiveDOFValues(ConfigToVec((tempNode->GetConfig()).at(irob)));
                             }
                             if(!(SetCollisionCheck()))
                             {
                                 tempNode = new RRTNode (sampleConfig);
                                 tempNode->SetParent(stopPtr);
                                 treePtr_->Add(tempNode);
    //                             cout << "reach nearest point, successfully connect!" << endl;
                             }
                         } // if sample is not goal, check if not collision, connect
                         break;
                     }
                 } // loop to step
             }

             /// ///// connect end /////////////
             count++;
         }// End of large loop
         cout << "RRT sampling size:" ;
         cout << treePtr_->TreeSize() << endl;

//         std::vector<RRTNode*> path;  // debug line
         path_.clear();
         if (treePtr_->TreeSize()>1)
         { path_ = treePtr_->GetPath(tempNode);}
    //     else { std::vector<RRTNode*> path;}  // debug line

         float pathLength = 0;
         for (int i = 1; i<path_.size(); i++)
         { pathLength = pathLength+SetWeightedDis((path_.at(i-1))->GetConfig() ,(path_.at(i))->GetConfig()); }
         cout << "RRT path size : ";
         cout << path_.size() << endl;

         return path_;
     }

//    std::vector<RRTNode*> SmoothPath(EnvironmentBasePtr env, std::vector<RRTNodePtr> &path,float stepSize, int iteration)
    std::vector<RRTNode*> RrtPlanner::SmoothPath()
    {
        int index1, index2; // checking node index, 2 is closer to goal
        RRTNode* node1;
        RRTNode* node2;
        RRTNode* tempNode;
        RRTNode* stopNode;
        bool insert = true;

        cout << "treeSize before smooth:" << endl;
        cout << path_.size()<< endl;
        for (int it = 0; it<inputParameters_.iteration; it++ )
        {
    //        vector<OpenRAVE::RobotBasePtr> robots;
    //        env_->GetRobots(robots);
    //        OpenRAVE::RobotBasePtr robot = robots.at(0);

            index1 = RandNode(path_.size());
            index2 = RandNode(path_.size());
            while(index1==index2) { index2 = RandNode(path_.size()); }
            if(index1>index2) { int a = index1; index1 = index2; index2 = a; }

            node1=path_.at(index1);
            node2=path_.at(index2);
            configSet stepConfig = SetUnitStep(node1->GetConfig(),node2->GetConfig(),inputParameters_.stepSize);

            std::vector<RRTNode*> tempPath;
            tempPath.clear();
            tempPath.push_back(node1);
            stopNode = node1;
            insert = true;

            // step from node 1 to node 2
            while (SetWeightedDis(stopNode->GetConfig(),node2->GetConfig()) > inputParameters_.stepSize)
            {
                tempNode = new RRTNode(SetSumConfig(stopNode->GetConfig(),stepConfig));
                if (SetOutBound(tempNode->GetConfig())){
                    delete tempNode;
                    tempNode = stopNode;
                    insert = false;
                    break;
                }
                for (int irob=0; irob<robots_.size(); irob++)
                { robots_.at(irob)->SetActiveDOFValues(ConfigToVec((tempNode->GetConfig()).at(irob)));}

    //            if(env->CheckCollision(robot)||robot->CheckSelfCollision())
                if(SetCollisionCheck())
                {
                    delete tempNode;
                    tempNode=stopNode;
                    insert = false;
                    break;
                }

                tempPath.push_back(tempNode);
                stopNode = tempNode;
            }

        }
        cout << "tree size after smooth: +++++++++++++";
        cout << path_.size() <<endl;
        float pathLength = 0;
        for (int i = 1; i<path_.size(); i++)
        { pathLength = pathLength+SetWeightedDis((path_.at(i-1))->GetConfig(), (path_.at(i))->GetConfig()); }
        cout << "Smooth path length : ";
        cout << pathLength << endl;

        return path_;
    }

    std::vector<RRTNode*> RrtPlanner::BiRRTPlanning(ParameterSet parameterIn)
    {
//        vector<OpenRAVE::RobotBasePtr> robots;
//        env->GetRobots(robots);

        int numGripper = robots_.size();
        inputParameters_.numGrippers=numGripper;

        /*
        std::vector<std::vector<double> > startSet;

        for (int i =0; i<numGripper; i++)
        {
            std::vector<double> startVec;
            robots.at(i)->GetActiveDOFValues(startVec);
            startSet.push_back(startVec);
        }
        */

        treeA_->ResetTree();
        treeB_->ResetTree();

        RRTNode* startPtr;
        startPtr = new RRTNode(inputParameters_.start_);
        treeA_ = new NodeTree(startPtr);
        RRTNode* goalPtr;
        goalPtr = new RRTNode(inputParameters_.goal_);
        treeB_ = new NodeTree(goalPtr);
        NodeTree* treeStepPtr;  // for extend a step
        NodeTree* treeConPtr;   // for connect

        configSet stepConfig;
        configSet sampleConfig;
        bool connect=false;
        float curStep = inputParameters_.stepSize;
        bool sample=true;
        bool AisCon = true;

        RRTNode* stepStop;
        RRTNode* conStop;
        RRTNode* nearestNode;
        RRTNode* tempPtr;

        // Initialize rand seed
        srand(time(NULL));

        while(!connect)
        {
            if (treeA_->TreeSize()>treeB_->TreeSize())
            { treeStepPtr = treeA_; treeConPtr = treeB_; AisCon=false; }
            else { treeStepPtr = treeB_; treeConPtr = treeA_; AisCon=true; }
            sample=true;

            // Extend the stepping node for one step
            while(sample)
            {
                sampleConfig = SampleSet(inputParameters_.numGrippers);
    //            sampleConfig = SampleSetWithBase(RandSample(),0);
                // Constraint violation & collision status for ray across end effectors
                while (ConstraintViolation(sampleConfig))
                {
                     sampleConfig = SampleSet(inputParameters_.numGrippers);
    //                sampleConfig = SampleSetWithBase(RandSample(),0);
                }

                nearestNode = NearestNode(sampleConfig, treeStepPtr);
                stepConfig = SetUnitStep(nearestNode->GetConfig(),sampleConfig,inputParameters_.stepSize);
                stepStop = new RRTNode(SetSumConfig(nearestNode->GetConfig(), stepConfig));

                if(SetOutBound(stepStop->GetConfig()))
                {  delete stepStop;    continue; }

                for (int irob=0; irob<robots_.size(); irob++)
                {
                    robots_.at(irob)->SetActiveDOFValues(ConfigToVec((tempPtr->GetConfig()).at(irob)));
                }

                if(SetCollisionCheck())
                {   delete stepStop;   continue; }
                stepStop->SetParent(nearestNode);
                treeStepPtr->Add(stepStop);
                sample=false;
                break;
            }

            nearestNode = NearestNode(stepStop->GetConfig(), treeConPtr);
            conStop = nearestNode;

            if(SetWeightedDis(conStop->GetConfig(),stepStop->GetConfig())<ERRORTHRESHOLD)
            {connect =true; break;}

            // Connect the connecting tree to the stepping node
            stepConfig = SetUnitStep(conStop->GetConfig(),stepStop->GetConfig(),inputParameters_.stepSize);
            while(SetWeightedDis(conStop->GetConfig(),stepStop->GetConfig())>inputParameters_.stepSize)
            {
                tempPtr = new RRTNode(SetSumConfig(conStop->GetConfig(),stepConfig));

                for (int irob=0; irob<robots_.size(); irob++)
                {   robots_.at(irob)->SetActiveDOFValues(ConfigToVec((tempPtr->GetConfig()).at(irob)));  }

                if(SetCollisionCheck())
                {
                    if(curStep>0.2)
                    {
                        curStep = curStep/2;
                        stepConfig = SetScaleConfig(stepConfig,2);
                        delete tempPtr;
                        break;
                    }
                    else
                    {
                        delete tempPtr;
                        tempPtr = conStop;
                        break;
                    }
                }
                tempPtr->SetParent(conStop);
                conStop=tempPtr;
                treeConPtr->Add(tempPtr);
            }
            if(SetWeightedDis(conStop->GetConfig(),stepStop->GetConfig()) <= inputParameters_.stepSize)
            {  connect=true; }
        }

        cout << "Bi-RRT sampling size:" ;
        cout << treeStepPtr->TreeSize()+treeConPtr->TreeSize() << endl;

        path_.clear();
//        std::vector<RRTNode*> pathS;
        std::vector<RRTNode*> pathE;
        if (AisCon)
        {
            path_ = treeConPtr->GetPath(conStop);
            pathE = treeStepPtr->GetPath(stepStop);
        }
        else
        {
            path_ = treeStepPtr->GetPath(stepStop);
            pathE = treeConPtr->GetPath(conStop);
        }
        for (int i =pathE.size(); i>0; i--)
        { path_.push_back(pathE.at(i-1));  }

        float pathLength = 0;
        for (int i = 1; i<path_.size(); i++)
        { pathLength = pathLength+SetWeightedDis((path_.at(i-1))->GetConfig() ,(path_.at(i))->GetConfig()); }
        cout << "Bi-RRT path length : ";
        cout << pathLength << endl;
        cout << "path size : ";
        cout << path_.size() <<endl;

        return path_;

    }

}


// Boost Includes ==============================================================
//#define PYUBLAS_HAVE_BOOST_BINDINGS
#include "datatypes.hpp"
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/cstdint.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "profile.hpp"
#include <Eigen/Core>
USING_PART_OF_NAMESPACE_EIGEN

using namespace boost::python;
using namespace std;

namespace gd {
	class SchwOpt1 {
		public:
			void test() {
				printf("hello\n");
			}
			void test2(Profile * profile) {
				printf("hello: %f\n", profile->densityr(1));
			}
			void test3() {
				Matrix3f m3;
				m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
				Matrix4f m4 = Matrix4f::Identity();
				Vector4i v4(1, 2, 3, 4);

				std::cout << "m3\n" << m3 << "\nm4:\n"
						<< m4 << "\nv4:\n" << v4 << std::endl;

			}
			void test4(double_matrix _A, double_vector _b, double_vector _c) {
				MatrixXd A = MatrixXd::Map(_A.data().begin(), _A.size2(), _A.size1());
				std::cout << A << std::endl;
				std::cout << _A.size1() << std::endl;
				std::cout << _A.size2() << std::endl;
				std::cout << _b.size() << std::endl;
				//VectorXd b = VectorXd::Map(_b.data().begin(), b.size());
				//VectorXd b = Eigen::Map<VectorXd>(_b.data().begin(), 1, b.size());
				VectorXd b = VectorXd::Map(_b.data().begin(), _b.size());
				std::cout << b << std::endl;
				VectorXd c = VectorXd::Map(_c.data().begin(), _c.size());
				//c = A * b;
				VectorXd::Map(_c.data().begin(), _c.size()) = A.transpose() * b;

				/*double array[3];
				//VectorXd v = Eigen::Map<Vector3d>(array, 2);
				VectorXd v = VectorXd::Map(array, 2);
				v.fill(10.);
				double data[6] = {1., 2., 3., 4., 5., 6.};
				//Matrix2i mat2x2(data);
				//MatrixXi mat2x2 = Map<Matrix2i>(data);
				MatrixXd mat2x2 = Eigen::Map<MatrixXd>(data, 3, 2);
				cout << v << endl;
				cout << mat2x2 << endl;
				cout << (mat2x2 *v )<< endl;*/

			}
	};
	void py_export_schw_opt() {
		class_<SchwOpt1 >("SchwOpt1", init<>())
				.def("test", (&SchwOpt1::test))
				.def("test2", (&SchwOpt1::test2))
				.def("test3", (&SchwOpt1::test3))
				.def("test4", (&SchwOpt1::test4))

				;
	}
	void py_export_schw_opt2();
//void py_export_schw_df_grid();
}
// Using =======================================================================
using namespace gd;

BOOST_PYTHON_MODULE(gdfast_schw)
{
	py_export_schw_opt();
	py_export_schw_opt2();
	//py_export_schw_df_grid();
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * 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.
 *
 *
 * time_profiling.jpp
 *
 *  Created on: Aug 14, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab.
 */

#ifndef MATRIX_BASE_HPP_
#define MATRIX_BASE_HPP_

//#include <Eigen/Dense> // overkill including everything, Specific dependencies will be set at each module
#include <Eigen/Core>
//#include <Eigen/LU> // for inverse and determinant
#include <Eigen/Sparse>

#define EIGEN_DEFAULT_TO_ROW_MAJOR

// data types conventions
typedef double matData_t;
typedef unsigned int uint_t;

// Definition of squared matrices, by default column major
typedef Eigen::Matrix<matData_t, 1,1, Eigen::RowMajor> Mat1;
typedef Eigen::Matrix<matData_t, 2,2, Eigen::RowMajor> Mat2;
typedef Eigen::Matrix<matData_t, 3,3, Eigen::RowMajor> Mat3;
typedef Eigen::Matrix<matData_t, 4,4, Eigen::RowMajor> Mat4;
typedef Eigen::Matrix<matData_t, 5,5, Eigen::RowMajor> Mat5;
typedef Eigen::Matrix<matData_t, 6,6, Eigen::RowMajor> Mat6;
typedef Eigen::Matrix<matData_t, Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> MatX;

//Sparse Matrices
typedef Eigen::SparseMatrix<matData_t, Eigen::ColMajor> SMatCol;
typedef Eigen::SparseMatrix<matData_t, Eigen::RowMajor> SMatRow;
typedef Eigen::Triplet<matData_t> Triplet;

// Definition of column matrices (vectors)
typedef Eigen::Matrix<matData_t, 2,1> Mat21;
typedef Eigen::Matrix<matData_t, 3,1> Mat31;
typedef Eigen::Matrix<matData_t, 4,1> Mat41;
typedef Eigen::Matrix<matData_t, 5,1> Mat51;
typedef Eigen::Matrix<matData_t, 6,1> Mat61;
typedef Eigen::Matrix<matData_t, Eigen::Dynamic,1> MatX1;

// Definition of row matrices (vectors)
typedef Eigen::Matrix<matData_t, 1,2> Mat12;
typedef Eigen::Matrix<matData_t, 1,3> Mat13;
typedef Eigen::Matrix<matData_t, 1,4> Mat14;
typedef Eigen::Matrix<matData_t, 1,5> Mat15;
typedef Eigen::Matrix<matData_t, 1,6> Mat16;
typedef Eigen::Matrix<matData_t, 1,Eigen::Dynamic> Mat1X;

// Definition of templated-based fixed matrices using c'11 aliases
template<int D>
using Vect = Eigen::Matrix<matData_t, D,1, Eigen::RowMajor>;
template<int Rw,int Col>
using Mat = Eigen::Matrix<matData_t, Rw, Col, Eigen::RowMajor>;

#endif /* MATRIX_BASE_HPP_ */


//
// Created by James Noeckel on 9/10/20.
//

#include "test_problem.h"
#include "utils/printvec.h"
#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
struct Callable {
    explicit Callable(const problem &p, bool gradient=false) : p_(p), gradient_(gradient) {}
    vector_double operator()(const vector_double &x) {
        if (gradient_) return p_.gradient(x);
        else return p_.fitness(x);
    }
private:
    const problem &p_;
    bool gradient_;
};

double testGradient(const problem &p, const vector_double &x, double dx) {
    vector_double estimate = estimate_gradient(Callable(p), x, dx);
    std::cout << "estimate: " << std::endl << estimate << std::endl;
    vector_double actual = p.gradient(x);
    std::cout << "actual: " << std::endl << actual << std::endl;
    vector_double diff(estimate.size());
    double maxError = 0.0;
    for (size_t i=0; i<diff.size(); ++i) {
        diff[i] = estimate[i]-actual[i];
        maxError = std::max(maxError, std::abs(diff[i]));
    }
    std::cout << "diff: " << diff << std::endl;
    return maxError;
}

double testHessian(const problem &p, const vector_double &x, double dx) {
    auto sparsity = p.hessians_sparsity();
    auto hessians = p.hessians(x);
    std::vector<MatrixXd> denseHessians(sparsity.size(), MatrixXd::Zero(x.size(), x.size()));
    for (size_t i=0; i<sparsity.size(); ++i) {
        for (size_t l=0; l<sparsity[i].size(); ++l) {
            size_t row = sparsity[i][l].first;
            size_t col = sparsity[i][l].second;
            denseHessians[i](row, col) = hessians[i][l];
            denseHessians[i](col, row) = hessians[i][l];
        }
    }
    auto gradOfGrad = estimate_gradient(Callable(p, true), x, dx);
    std::vector<MatrixXd> denseApproxHessians(sparsity.size(), MatrixXd(x.size(), x.size()));
    for (size_t f=0; f<sparsity.size(); ++f) {
        for (size_t i=0; i < x.size(); ++i) {
            for (size_t j=0; j < x.size(); ++j) {
                denseApproxHessians[f](i, j) = gradOfGrad[f * x.size() * x.size() + i*x.size() + j];
            }
        }
    }
    std::vector<MatrixXd> diffs(sparsity.size());
    double maxError = 0.0;
    for (size_t f=0; f<sparsity.size(); ++f) {
        std::cout << "hessian " << f << ": " << std::endl;
        std::cout << denseHessians[f] << std::endl;
        std::cout << "approx hessian " << f << ": " << std::endl;
        std::cout << denseApproxHessians[f] << std::endl;
        diffs[f] = denseApproxHessians[f] - denseHessians[f];
        std::cout << "diff " << f << ": " << std::endl;
        std::cout << diffs[f] << std::endl;
        maxError = std::max(maxError, diffs[f].array().abs().maxCoeff());
    }
    return maxError;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cos.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/fast.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

STF_CASE_TPL (" cos",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cos;

  using r_t = decltype(cos(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cos(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cos(-bs::Pi<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(-bs::Pio_2<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(-bs::Pio_4<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pi<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pio_2<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pio_4<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cos fast_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cos;

  using r_t = decltype(cos(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::fast_(cos)(-bs::Pi<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(-bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(-bs::Pio_4<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Pi<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Pio_4<T>()), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cos)(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cos clipped_small_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::cos;

  using r_t = decltype(cos(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cos(bs::Inf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Minf<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Nan<T>(), bst::clipped_small_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cos(-bs::Pi<T>(), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(-bs::Pio_2<T>(), bst::clipped_small_), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(-bs::Pio_4<T>(), bst::clipped_small_), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pi<T>(), bst::clipped_small_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pio_2<T>(), bst::clipped_small_), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pio_4<T>(), bst::clipped_small_), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Zero<T>(), bst::clipped_small_), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cos clipped_medium_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bst = bs::tag;
  namespace bd = boost::dispatch;
  using bs::cos;

  using r_t = decltype(cos(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cos(bs::Inf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Minf<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Nan<T>(), bst::clipped_medium_), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cos(-bs::Pi<T>(), bst::clipped_medium_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(-bs::Pio_2<T>(), bst::clipped_medium_), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(-bs::Pio_4<T>(), bst::clipped_medium_), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pi<T>(), bst::clipped_medium_), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pio_2<T>(), bst::clipped_medium_), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Pio_4<T>(), bst::clipped_medium_), bs::Sqrt_2o_2<r_t>(), 0.5);
  STF_ULP_EQUAL(cos(bs::Zero<T>(), bst::clipped_medium_), bs::One<r_t>(), 0.5);
}


/* Copyright 2019 The fast_sparse_interpolation Authors. All Rights Reserved.

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.
==============================================================================*/

#pragma once

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <iostream>
#include <vector>
#include "Iterators.hpp"

namespace fsi {

/**
 * Represents a vector indexed by multi-indices which are specified by an iterator of type It.
 */
template <typename It>
class MultiDimVector {
  It it;

 public:
  // put the first dimension into an outer vector for processing reasons
  // data[i] contains all entries whose multi-index starts with i, ordered lexicographically.
  std::vector<std::vector<double>> data;

  /**
   * Iterator class used for begin() / end()
   */
  class Iterator {
    MultiDimVector<It> &v;
    StepIterator<It> stepIt;

   public:
    Iterator(MultiDimVector<It> &v, It it) : v(v), stepIt(it){};

    typedef Iterator self_type;
    typedef double value_type;
    typedef double &reference;
    typedef double *pointer;
    typedef std::forward_iterator_tag iterator_category;
    typedef int difference_type;

    std::vector<size_t> index() { return stepIt.index(); }

    // post-increment
    self_type operator++() {
      self_type i = *this;
      stepIt.next();
      return i;
    }

    // pre-increment
    self_type operator++(int junk) {
      stepIt.next();
      return *this;
    }
    reference operator*() { return v.data[stepIt.firstIndex()][stepIt.tailDimsCounter()]; }
    pointer operator->() { return &(*(*this)); }
    bool operator==(const self_type &rhs) {
      if (stepIt.valid() != rhs.stepIt.valid()) {
        return false;
      }

      return stepIt.valid() or (stepIt.firstIndex() == rhs.stepIt.firstIndex() &&
                                stepIt.tailDimsCounter() == rhs.stepIt.tailDimsCounter());
    }
    bool operator!=(const self_type &rhs) { return !(*this == rhs); }
  };

  MultiDimVector(It it) : it(it), data(it.firstIndexBound()) {
    auto sizes = it.numValuesPerFirstIndex();
    for (size_t i = 0; i < sizes.size(); ++i) {
      data[i].resize(sizes[i]);
    }
  };

  void swap(MultiDimVector<It> &other) {
    data.swap(other.data);
    std::swap(it, other.it);
  }

  /**
   * Returns the associated iterator object.
   */
  It getJumpIterator() const { return it; }

  /**
   * Changes the associated iterator object and adjusts the structure of the data vectors (without
   * initializing them).
   */
  void resetWithJumpIterator(It const &other_it) {
    it = other_it;
    size_t n_1 = it.firstIndexBound();
    data.resize(n_1);
    auto sizes = it.numValuesPerFirstIndex();
    for (size_t i = 0; i < n_1; ++i) {
      data[i].resize(sizes[i]);
    }
  }

  Iterator begin() { return Iterator(*this, it); }

  Iterator end() {
    It end_it = it;
    end_it.goToEnd();
    return Iterator(*this, end_it);
  }

  MultiDimVector<It> &operator+=(MultiDimVector<It> const &other) {
    for (size_t i = 0; i < data.size(); ++i) {
      for (size_t j = 0; j < data[i].size(); ++j) {
        data[i][j] += other.data[i][j];
      }
    }
    return *this;
  }

  MultiDimVector<It> &operator-=(MultiDimVector<It> const &other) {
    for (size_t i = 0; i < data.size(); ++i) {
      for (size_t j = 0; j < data[i].size(); ++j) {
        data[i][j] -= other.data[i][j];
      }
    }
    return *this;
  }
};

/**
 * Helper for printing a matrix
 */
inline std::ostream &operator<<(std::ostream &os, boost::numeric::ublas::matrix<double> matrix) {
  os << "[";
  for (size_t i = 0; i < matrix.size1(); ++i) {
    for (size_t j = 0; j < matrix.size2(); ++j) {
      os << matrix(i, j) << "   ";
    }
    os << "\n";
  }
  os << " ]";
  return os;
}

/**
 * Helper for printing a std::vector
 */
template <class T>
inline std::ostream &operator<<(std::ostream &os, const std::vector<T> &v) {
  os << "[";
  for (auto ii = v.begin(); ii != v.end(); ++ii) {
    os << " " << *ii;
  }
  os << " ]";
  return os;
}

template <typename It>
MultiDimVector<It> operator+(MultiDimVector<It> first, MultiDimVector<It> const &second) {
  first += second;
  return first;
}

template <typename It>
MultiDimVector<It> operator-(MultiDimVector<It> first, MultiDimVector<It> const &second) {
  first -= second;
  return first;
}

template <typename It>
double squared_l2_norm(MultiDimVector<It> const &v) {
  double sum = 0.0;
  for (size_t i = 0; i < v.data.size(); ++i) {
    for (size_t j = 0; j < v.data[i].size(); ++j) {
      double value = v.data[i][j];
      sum += value * value;
    }
  }
  return sum;
}

// ----- The next block of methods contains methods for efficient multi-indexed matrix-vector
// products.

/**
 * Multiplies the multi-indexed vector v with the matrix \widehat{I \otimes \hdots \otimes I \otimes
 * L}, according to the notation from the paper. Saves the result in v. The vector buffer is used
 * for storing intermediate results. It should be provided externally and can be used for multiple
 * matrix-vector products to omit allocating new memory each time.
 * Note that this method permutes indices, specifically it moves the last index to the front. This
 * means e.g. that an element at index (0, 1, 2, 3) of the resulting vector would have been at index
 * (1, 2, 3, 0) if the indices hadn't been permuted.
 * We cycle indices because it allows for almost continuous memory access during each
 * multiplication, hence likely accelerating the method due to better cache performance and better
 * iterator performance.
 */
template <typename It>
void multiply_single_lower_triangular_inplace(boost::numeric::ublas::matrix<double> const &L,
                                              MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  It it = v.getJumpIterator();
  It cycled_it = it.cycle();
  buffer.resetWithJumpIterator(cycled_it);

  std::vector<size_t> indexes(buffer.data.size(), 0);

  it.reset();

  size_t first_v_index = 0;
  size_t second_v_index = 0;

  double *data_pointer = &v.data[0][0];
  size_t data_size = v.data[0].size();

  while (it.valid()) {
    size_t last_dim_count = it.lastDimensionCount();
    double *offset_data_pointer = data_pointer + second_v_index;
    for (size_t i = 0; i < last_dim_count; ++i) {
      double sum = 0.0;
      for (size_t j = 0; j <= i; ++j) {
        sum += L(i, j) * (*(offset_data_pointer + j));
      }
      buffer.data[i][indexes[i]++] = sum;
    }
    second_v_index += last_dim_count;
    if (second_v_index >= data_size) {
      second_v_index = 0;
      first_v_index += 1;
      data_pointer = &v.data[first_v_index][0];
      data_size = v.data[first_v_index].size();
    }

    it.next();
  }

  v.swap(buffer);
}

/**
 * Same as above, but with an upper triangular matrix U instead of L.
 */
template <typename It>
void multiply_single_upper_triangular_inplace(boost::numeric::ublas::matrix<double> const &U,
                                              MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  It it = v.getJumpIterator();
  It cycled_it = it.cycle();
  buffer.resetWithJumpIterator(cycled_it);

  std::vector<size_t> indexes(buffer.data.size(), 0);

  it.reset();

  size_t first_v_index = 0;
  size_t second_v_index = 0;

  double *data_pointer = &v.data[0][0];
  size_t data_size = v.data[0].size();

  while (it.valid()) {
    size_t last_dim_count = it.lastDimensionCount();
    double *offset_data_pointer = data_pointer + second_v_index;
    for (size_t i = 0; i < last_dim_count; ++i) {
      double sum = 0.0;
      for (size_t j = i; j < last_dim_count; ++j) {
        sum += U(i, j) * (*(offset_data_pointer + j));
      }
      buffer.data[i][indexes[i]++] = sum;
    }
    second_v_index += last_dim_count;
    if (second_v_index >= data_size) {
      second_v_index = 0;
      first_v_index += 1;
      data_pointer = &v.data[first_v_index][0];
      data_size = v.data[first_v_index].size();
    }

    it.next();
  }

  v.swap(buffer);
}

/**
 * Same as above, but with an arbitrary Matrix M instead of L.
 */
template <typename It>
void multiply_single_arbitrary_inplace(boost::numeric::ublas::matrix<double> const &M,
                                       MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  It it = v.getJumpIterator();
  It cycled_it = it.cycle();
  buffer.resetWithJumpIterator(cycled_it);

  std::vector<size_t> indexes(buffer.data.size(), 0);

  it.reset();

  size_t first_v_index = 0;
  size_t second_v_index = 0;

  double *data_pointer = &v.data[0][0];
  size_t data_size = v.data[0].size();

  while (it.valid()) {
    size_t last_dim_count = it.lastDimensionCount();
    double *offset_data_pointer = data_pointer + second_v_index;
    for (size_t i = 0; i < last_dim_count; ++i) {
      double sum = 0.0;
      for (size_t j = 0; j < last_dim_count; ++j) {
        sum += M(i, j) * (*(offset_data_pointer + j));
      }
      buffer.data[i][indexes[i]++] = sum;
    }
    second_v_index += last_dim_count;
    if (second_v_index >= data_size) {
      second_v_index = 0;
      first_v_index += 1;
      data_pointer = &v.data[first_v_index][0];
      data_size = v.data[first_v_index].size();
    }

    it.next();
  }

  v.swap(buffer);
}

/**
 * Same as above, but multiplies with the identity matrix, i.e. it only cycles indices. This should
 * be somewhat faster than explicitly multiplying with the identity matrix using one of the other
 * three methods.
 */
template <typename It>
void multiply_single_identity_inplace(MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  It it = v.getJumpIterator();
  It cycled_it = it.cycle();
  buffer.resetWithJumpIterator(cycled_it);

  std::vector<size_t> indexes(buffer.data.size(), 0);

  it.reset();

  size_t first_v_index = 0;
  size_t second_v_index = 0;

  double *data_pointer = &v.data[0][0];
  size_t data_size = v.data[0].size();

  while (it.valid()) {
    size_t last_dim_count = it.lastDimensionCount();
    double *offset_data_pointer = data_pointer + second_v_index;
    for (size_t i = 0; i < last_dim_count; ++i) {
      buffer.data[i][indexes[i]++] = (*(offset_data_pointer + i));
    }
    second_v_index += last_dim_count;
    if (second_v_index >= data_size) {
      second_v_index = 0;
      first_v_index += 1;
      data_pointer = &v.data[first_v_index][0];
      data_size = v.data[first_v_index].size();
    }

    it.next();
  }

  v.swap(buffer);
}

/**
 * Repeats calls to the identity multiplication to cycle the indices several times.
 */
template <typename It>
void cycle_vector_inplace(size_t num_times, MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  for (size_t i = 0; i < num_times; ++i) {
    multiply_single_identity_inplace(v, buffer);
  }
}

/**
 * Repeats calls to the identity multiplication to cycle the indices several times. Uses its own
 * buffer (potentially slower than reusing an already created buffer).
 */
template <typename It>
void cycle_vector_inplace(size_t num_times, MultiDimVector<It> &v) {
  MultiDimVector<It> buffer(v.getJumpIterator());
  for (size_t i = 0; i < num_times; ++i) {
    multiply_single_identity_inplace(v, buffer);
  }
}

/**
 * Performs a multiplication with a tensor product of lower triangular matrices \widehat{L[1]
 * \otimes \hdots \otimes L[k]}.
 */
template <typename It>
void multiply_lower_triangular_inplace(std::vector<boost::numeric::ublas::matrix<double>> L,
                                       MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  // the multiplication is based on a cyclic permutation of the indices: the last index of v becomes
  // the first index of w
  size_t d = L.size();
  It it = v.getJumpIterator();

  for (int k = d - 1; k >= 0; --k) {
    multiply_single_lower_triangular_inplace(L[k], v, buffer);
  }
}

/**
 * Performs a multiplication with a tensor product of upper triangular matrices \widehat{U[1]
 * \otimes \hdots \otimes U[k]}.
 */
template <typename It>
void multiply_upper_triangular_inplace(std::vector<boost::numeric::ublas::matrix<double>> U,
                                       MultiDimVector<It> &v, MultiDimVector<It> &buffer) {
  // the multiplication is based on a cyclic permutation of the indices: the last index of v becomes
  // the first index of w
  size_t d = U.size();
  It it = v.getJumpIterator();

  for (int k = d - 1; k >= 0; --k) {
    multiply_single_upper_triangular_inplace(U[k], v, buffer);
  }
}

/**
 * Represents a sparse linear tensor product operator defined by a matrix for each dimension and an
 * iterator that defines the multi-index set.
 */
template <typename It>
class SparseTPOperator {
  // Iterator that defines the multi-index set
  It it;

  // Matrices associated with the operator.
  std::vector<boost::numeric::ublas::matrix<double>> M;

  // Matrices containing the L and U parts of the LU decompositions
  std::vector<boost::numeric::ublas::matrix<double>> LU;

  // Matrices containing the L parts of the LU decompositions
  std::vector<boost::numeric::ublas::matrix<double>> L;

  // Matrices containing the U parts of the LU decomposition
  std::vector<boost::numeric::ublas::matrix<double>> U;

  // Inverses of the L matrices
  std::vector<boost::numeric::ublas::matrix<double>> Linv;

  // Inverses of the U matrices
  std::vector<boost::numeric::ublas::matrix<double>> Uinv;

  // is used to store intermediate data
  MultiDimVector<It> buffer;

  // dimension of the indices
  size_t d;

 public:
  SparseTPOperator(It it, std::vector<boost::numeric::ublas::matrix<double>> matrices)
      : it(it), M(matrices), buffer(it), d(matrices.size()){};

  /**
   * This method performs preparations common to apply() and solve() if they haven't already been
   * performed. The preparations are the LU decompositions of the M matrices.
   */
  void prepareCommon() {
    if (LU.size() > 0) {
      return;  // already prepared
    }

    namespace ublas = boost::numeric::ublas;
    typedef ublas::matrix<double> Matrix;

    for (size_t k = 0; k < d; ++k) {
      // std::cout << "Matrix creation loop\n";
      Matrix Mk = M[k];

      ublas::lu_factorize(Mk);

      LU.push_back(Mk);
    }
  }

  /**
   * This method performs preparations specific to apply() if they haven't already been performed.
   * These are the (cheap) computation of L and U from LU.
   */
  void prepareApply() {
    if (L.size() > 0) {
      return;  // already prepared
    }

    prepareCommon();

    namespace ublas = boost::numeric::ublas;
    typedef ublas::matrix<double> Matrix;

    for (size_t k = 0; k < d; ++k) {
      size_t nk = M[k].size1();
      Matrix &LUk = LU[k];
      Matrix Lk(nk, nk);
      Matrix Uk(nk, nk);

      for (size_t i = 0; i < nk; ++i) {
        for (size_t j = 0; j < i; ++j) {
          Lk(i, j) = LUk(i, j);
        }

        Lk(i, i) = 1.0;

        for (size_t j = i; j < nk; ++j) {
          Uk(i, j) = LUk(i, j);
        }
      }

      L.push_back(Lk);
      U.push_back(Uk);
    }
  }

  /**
   * This method performs preparations specific to solve() if they haven't already been performed.
   * This is the inversion of the L and U matrices.
   */
  void prepareSolve() {
    if (Linv.size() > 0) {
      return;  // already prepared
    }

    prepareCommon();

    namespace ublas = boost::numeric::ublas;
    typedef ublas::matrix<double> Matrix;

    for (size_t k = 0; k < d; ++k) {
      Matrix Lkinv = ublas::identity_matrix<double>(M[k].size1());
      Matrix Ukinv = ublas::identity_matrix<double>(M[k].size1());

      ublas::inplace_solve(LU[k], Lkinv, ublas::unit_lower_tag());
      ublas::inplace_solve(LU[k], Ukinv, ublas::upper_tag());

      Linv.push_back(Lkinv);
      Uinv.push_back(Ukinv);
    }
  }

  /**
   * Performs a matrix-vector product with the matrix that is implicitly defined by this operator.
   */
  MultiDimVector<It> apply(MultiDimVector<It> input) {
    prepareApply();
    multiply_upper_triangular_inplace(U, input, buffer);
    multiply_lower_triangular_inplace(L, input, buffer);
    return input;
  }

  /**
   * Performs a matrix-vector product with the inverse of the matrix that is implicitly defined by
   * this operator.
   */
  MultiDimVector<It> solve(MultiDimVector<It> rhs) {
    prepareSolve();
    multiply_lower_triangular_inplace(Linv, rhs, buffer);
    multiply_upper_triangular_inplace(Uinv, rhs, buffer);
    return rhs;
  }
};

/**
 * Creates a SparseTPOperator corresponding to the interpolation problem. That means its
 * one-dimensional matrices contain the entries phi[k](j)(x[k](i)). The type of phi and x must be
 * such that this expression is defined.
 */
template <typename It, typename X, typename Phi>
SparseTPOperator<It> createInterpolationOperator(It it, Phi phi, X x) {
  auto n = it.indexBounds();
  size_t d = it.dim();

  namespace ublas = boost::numeric::ublas;
  typedef ublas::matrix<double> Matrix;

  std::vector<Matrix> matrices;

  // create matrices and inverted LU decompositions
  for (size_t k = 0; k < d; ++k) {
    // std::cout << "Matrix creation loop\n";
    Matrix Mk(n[k], n[k]);
    for (size_t i = 0; i < n[k]; ++i) {
      for (size_t j = 0; j < n[k]; ++j) {
        Mk(i, j) = phi[k](j)(x[k](i));
      }
    }

    matrices.push_back(Mk);
  }

  return SparseTPOperator<It>(it, matrices);
}

/**
 * Creates a MultiDimVector containing the values of the function f at the grid points specified by
 * x and the multi-index iterator it. The function f should take a std::vector<double> as an
 * argument. The point object x should have a type such that the expression x[k](i) yields a double
 * specifying the i-th point (starting from i=0) in dimension k (also starting from k=0).
 */
template <typename It, typename Func, typename X>
MultiDimVector<It> evaluateFunction(It it, Func f, X x) {
  size_t d = it.dim();
  auto n = it.indexBounds();
  MultiDimVector<It> v(it);
  std::vector<size_t> indexes(v.data.size(), 0);

  it.reset();
  std::vector<double> point(d);
  while (it.valid()) {
    size_t last_dim_count = it.lastDimensionCount();
    for (size_t dim = 0; dim < d - 1; ++dim) {
      point[dim] = x[dim](it.indexAt(dim));
    }

    for (size_t last_dim_idx = 0; last_dim_idx < last_dim_count; ++last_dim_idx) {
      point[d - 1] = x[d - 1](last_dim_idx);

      double function_value = f(point);

      size_t first_index = it.firstIndex();
      v.data[first_index][indexes[first_index]++] = function_value;
    }

    it.next();
  }

  return v;
}

/**
 * Convenience function that computes a vector of interpolation coefficients for the sparse grid
 * basis given by phi and it on the points given by phi and it. The arguments f, it, phi, x need to
 * satisfy the same requirements as explained in the documentation of the functions above.
 */
template <typename Func, typename It, typename Phi, typename X>
MultiDimVector<It> interpolate(Func f, It it, Phi phi, X x) {
  auto rhs = evaluateFunction(it, f, x);
  auto op = createInterpolationOperator(it, phi, x);
  return op.solve(rhs);
}

} /* namespace fsi */


#pragma once
#include <boost/rational.hpp>
#include "../elements/time.hpp"

namespace adm {
  // utilities for working with times as boost::rational
  using RationalTime = boost::rational<int64_t>;

  inline RationalTime asRational(const FractionalTime &t) {
    return {t.numerator(), t.denominator()};
  }

  inline RationalTime asRational(const Time &t) {
    FractionalTime fractional = t.asFractional();
    return {fractional.numerator(), fractional.denominator()};
  }

  inline FractionalTime asFractionalTime(const RationalTime &t) {
    return {t.numerator(), t.denominator()};
  }

  inline Time asTime(const RationalTime &t) { return asFractionalTime(t); }
}  // namespace adm


/***********************************************************************************
 * Copyright (c) 2017, UT-Battelle
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the xacc 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 <COPYRIGHT HOLDER> 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.
 *
 * Contributors:
 *   Initial API and implementation - Alex McCaskey
 *
 **********************************************************************************/
#include "FCIDumpPreprocessor.hpp"
#include <boost/tokenizer.hpp>
#include "XACC.hpp"
//#include "MPIProvider.hpp"

#include "chemps2/Hamiltonian.h"
#include "unsupported/Eigen/CXX11/Tensor"

using namespace CheMPS2;

namespace xacc {

namespace vqe {

const std::string FCIDumpPreprocessor::process(const std::string& source,
		std::shared_ptr<Compiler> compiler,
		std::shared_ptr<Accelerator> accelerator) {

	if (boost::contains(source, "FCI") && boost::contains(source, "NELEC")) {

//		std::shared_ptr<MPIProvider> provider;
//		if (xacc::hasService<MPIProvider>("boost-mpi")) {
//			provider = xacc::getService<MPIProvider>("boost-mpi");
//		} else {
//			provider = xacc::getService<MPIProvider>("no-mpi");
//		}

//		provider->initialize();
//		auto world = provider->getCommunicator();

		std::string kernelString = "";

//		if (world->rank() == 0) {
			xacc::info("Running FCIDump Preprocessor.");
			int symGroup = 7;
			if (xacc::optionExists("vqe-fcidump-symmetry")) {
				symGroup = std::stoi(xacc::getOption("vqe-fcidump-symmetry"));
				xacc::info("Setting Symmetry Group to " + std::to_string(symGroup));
			}

			if (!xacc::optionExists("n-electrons")) {
				std::vector<std::string> split, splitCommas, splitEquals;
				boost::split(split, source, boost::is_any_of("\n"));
				for (auto& line : split) {
					if (boost::contains(line, "NELEC")) {

						boost::split(splitCommas, line, boost::is_any_of(","));
						for (auto& s : splitCommas) {
							if (boost::contains(s, "NELEC")) {
								boost::split(splitEquals, s,
										boost::is_any_of("="));
								std::string val = splitEquals[1];
								boost::trim(val);
								xacc::setOption("n-electrons", val);
								xacc::info(
										"FCIDump Preprocessor Number of Electrons = "
												+ val);
								break;
							}
						}

						break;
					}
				}
			}

			std::ofstream tmpFile(".tmp.fcidump");
			tmpFile << source;
			tmpFile.close();

			Hamiltonian h(".tmp.fcidump", symGroup);

			auto nOrbitals = h.getL();
			auto econst = h.getEconst();

			xacc::setOption("n-qubits", std::to_string(2*nOrbitals));

			Eigen::Tensor<double, 2> hpq(2 * nOrbitals, 2 * nOrbitals);
			Eigen::Tensor<double, 4> hpqrs(2 * nOrbitals, 2 * nOrbitals,
					2 * nOrbitals, 2 * nOrbitals);

			hpq.setZero();
			hpqrs.setZero();

			for (int p = 0; p < nOrbitals; p++) {
				for (int q = 0; q < nOrbitals; q++) {
					hpq(2 * p, 2 * q) = h.getTmat(p, q);
					hpq(2 * p + 1, 2 * q + 1) = h.getTmat(p, q);

					for (int r = 0; r < nOrbitals; r++) {
						for (int s = 0; s < nOrbitals; s++) {

							hpqrs(2 * p, 2 * q + 1, 2 * r + 1, 2 * s) =
									h.getVmat(p, q, s, r) / 2.0;
							hpqrs(2 * p + 1, 2 * q, 2 * r, 2 * s + 1) =
									h.getVmat(p, q, s, r) / 2.0;

							if (p != q && r != s) {
								hpqrs(2 * p, 2 * q, 2 * r, 2 * s) = h.getVmat(p,
										q, s, r) / 2.0;
								hpqrs(2 * p + 1, 2 * q + 1, 2 * r + 1,
										2 * s + 1) = h.getVmat(p, q, s, r)
										/ 2.0;
							}
						}
					}
				}
			}

			std::remove(".tmp.fcidump");

			bool genOfScript = xacc::optionExists("generate-openfermion-transform-script");
			std::stringstream zz, ofscript;
			zz << "__qpu__ kernel() {\n" << "   " << std::setprecision(16)
					<< econst << "\n";
			kernelString = zz.str();

			if (genOfScript) {
				ofscript << "from openfermion.ops import FermionOperator\n";
				ofscript << "from openfermion.ops import QubitOperator\n";
				ofscript << "from openfermion.utils import eigenspectrum\n";
				ofscript << "from openfermion.transforms import jordan_wigner, bravyi_kitaev\n";
				ofscript << "op = FermionOperator('', " << econst << ") + \\\n";
			}

			for (int p = 0; p < 2 * nOrbitals; p++) {
				for (int q = 0; q < 2 * nOrbitals; q++) {
					if (std::fabs(hpq(p, q)) > 1e-12) {
						std::stringstream ss;
						ss << std::setprecision(16) << "   " << hpq(p, q) << " "
								<< p << " 1 " << q << " 0\n";
						kernelString += ss.str();

						if (genOfScript) {
							if (q == 2 * nOrbitals - 1
									&& p == 2 * nOrbitals - 1) {
								ofscript << "   FermionOperator('" << p << "^ "
										<< q << "', " << hpq(p, q) << ")\n";
							} else {
								ofscript << "   FermionOperator('" << p << "^ "
										<< q << "', " << hpq(p, q)
										<< ") + \\\n";
							}
						}
					}

					for (int r = 0; r < 2 * nOrbitals; r++) {
						for (int s = 0; s < 2 * nOrbitals; s++) {
							if (std::fabs(hpqrs(p, q, r, s)) > 1e-12) {
								std::stringstream ss;
								ss << std::setprecision(16) << "   "
										<< hpqrs(p, q, r, s) << " " << p
										<< " 1 " << q << " 1 " << r << " 0 "
										<< s << " 0\n";
								kernelString += ss.str();

								if (genOfScript) {
									if (q == 2 * nOrbitals - 1
											&& p == 2 * nOrbitals - 1 && r == 2*nOrbitals-1 && s == 2*nOrbitals-1) {
										ofscript << "   FermionOperator('" << p
												<< "^ " << q << "^ " << r << " " << s << "', "
												<< hpqrs(p, q, r, s) << ")\n";
									} else {
										ofscript << "   FermionOperator('" << p
												<< "^ " << q << "^ " << r << " "
												<< s << "', "
												<< hpqrs(p, q, r, s) << ") + \\\n";
									}
								}
							}
						}
					}
				}
			}

			kernelString += "}\n";

			if (genOfScript) {
				std::string transformStr;
				if (xacc::optionExists("fermion-transformation")) {
					transformStr = xacc::getOption("fermion-transformation");
				} else {
					transformStr = "jw";
				}

				ofscript << "transformedOp = "
						<< (transformStr == "jw" || transformStr == "ejw" ?
								"jordan_wigner" : "bravyi_kitaev") << "(op)\n";
				ofscript << "es = eigenspectrum(op)\n";
				ofscript << "print('Energies = ', es)\n";

				std::ofstream out("gen_openfermion_transform_script.py");
				out << ofscript.str();
				out.close();
			}

//		}

//		world->broadcast(kernelString, 0);
		xacc::info("Done running FCIDump Preprocessor.");

//		std::cout << kernelString << "\n";
		return kernelString;

	} else {

		return source;

	}

}

}

}


#include <iostream>
#include <cassert>
#include <Eigen/Dense>
#include <Magick++.h>

#include "csv.h"

#include "startree.h"

using namespace startree;
using namespace Eigen;
using namespace Magick;
using namespace std;

const uint32_t kMaxLeafSize = 8;

const uint32_t kImageWidth = 800;
const uint32_t kImageHeight = 600;

// Brightness multiplication factor ("exposure time")
//const double A = 30000;
const double A = 10000;

const float numSeconds = 4.0;
const uint32_t fps = 1;
const double frame_period_s = 1 / fps;
const uint32_t numFrames = numSeconds * fps;
const double radsPerFrame = (2 * M_PI) / numFrames;

Vector3d cameraPosition(0, -1, 0);
Vector3d cameraDirection(0, 1, 0);
const Vector3d upDirection(0, 0, 1);
Vector3d rightDirection(cameraDirection.cross(upDirection));

static Vector3d basisProjection(const Vector3d& vec,
                         const Vector3d& basis_a,
                         const Vector3d& basis_b,
                         const Vector3d& basis_c) {
    return Vector3d(vec.dot(basis_a),
                    vec.dot(basis_b),
                    vec.dot(basis_c));
}

int render_count = 0;
int overlap_count = 0;
static void renderStar(Image& image,
                       const uint32_t sx,
                       const uint32_t sy,
                       const double brightness,
                       const Vector3d& color) {
    ColorRGB pxColor = image.pixelColor(sx, sy);
    pxColor.red(1.0);
    pxColor.green(1.0);
    pxColor.blue(1.0);
    double alpha = pxColor.alpha();
    if (alpha != 1.0)
        overlap_count++;
    pxColor.alpha(fmax(alpha - (A*brightness), 0.0));
    image.pixelColor(sx, sy, pxColor);

    render_count++;
}

int main(int argc, char* argv[]) {
    //Initialize ImageMagick
    InitializeMagick(*argv);

    cout << "Seconds: " << numSeconds << endl;
    cout << "FPS: " << fps << endl;
    cout << "Period: " << frame_period_s << endl;
    cout << "Frames: " << numFrames << endl;
    cout << "rads/frame: " << radsPerFrame << endl;

    // Read CSV file and create Star objects
    //io::CSVReader<8> star_reader("data/hygdata_min3.csv");
    io::CSVReader<8> star_reader("data/thc.csv");

    uint64_t id = 0;
    string idstr;
    double x, y, z;
    double absmag;
    double lum;
    double r, g, b;

    double minx,miny,minz,maxx,maxy,maxz;

    vector<Star> stars;
    while(star_reader.read_row(idstr, x, y, z, absmag, r, g, b)) {
        minx = min(x, minx);
        miny = min(y, miny);
        minz = min(z, minz);
        maxx = max(x, maxx);
        maxy = max(y, maxy);
        maxz = max(z, maxz);
        lum = exp(-0.4 * (absmag - 4.85));
        stars.push_back(Star(id++, x, y , z, lum, r, g, b));
    }
    cout << "Min x: " << minx << " Min y: " << miny << " Min z: " << minz
         << endl
         << "Max x: " << maxx << " Max y: " << maxy << " Max z: " << maxz
         << endl;

    vector<Image> frameImages;
    Matrix3d rotM =
        AngleAxisd(radsPerFrame, upDirection).toRotationMatrix();
    for (unsigned int i = 0; i < numFrames; i++) {
        cout << "Starting frame: " << i << endl;

        // Background image (black)
        frameImages.push_back(Image(Geometry(kImageWidth, kImageHeight),
                                    Color("black")));

        // Create clear image for stars
        Image starImage(Geometry(kImageWidth, kImageHeight), Color("black"));
        starImage.matte(true);
        starImage.transparent(Color("black"));
        assert(((ColorRGB)(starImage.pixelColor(0,0))).red() == 0.0);
        assert(((ColorRGB)(starImage.pixelColor(0,0))).green() == 0.0);
        assert(((ColorRGB)(starImage.pixelColor(0,0))).blue() == 0.0);
        assert(starImage.pixelColor(0,0).alpha() == 1.0);

        for (vector<Star>::iterator star = stars.begin();
             star != stars.end(); ++star) {
            const Vector3d& position = star->position();
            const Vector3d translated = position - cameraPosition;
            const Vector3d projected = basisProjection(translated,
                                                       rightDirection,
                                                       cameraDirection,
                                                       upDirection);
            if (projected.y() < 0.00001) {
                continue;
            }
            const uint32_t sx = (kImageWidth / 2.0) +
                (kImageWidth / 2.0) * projected.x() / projected.y();
            const uint32_t sy = (kImageHeight / 2.0) +
                (kImageWidth / 2.0) * projected.z() / projected.y();
            if (sx < 0 || sx >= kImageWidth) continue;
            if (sy < 0 || sy >= kImageHeight) continue;
            const double brightness = star->lum() /
                translated.squaredNorm();
            renderStar(starImage, sx, sy, brightness, star->color());
        }

        frameImages.back().composite(starImage, 0, 0, PlusCompositeOp);
        frameImages.back().animationDelay(frame_period_s * 100);

        // Rotate camera position about up axis through origin
        Vector3d tmp = rotM * cameraPosition;
        cameraPosition = tmp;
        // Set camera direction towards the origin
        tmp = cameraPosition;
        cameraDirection = -(tmp.normalized());
        rightDirection = cameraDirection.cross(upDirection);

        cout << "New camera position: " << cameraPosition << endl;
        cout << "New camera direction: " << cameraDirection << endl;
    }

    writeImages(frameImages.begin(), frameImages.end(), "animation.gif");

    return 0;
}


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_COMPLEX_ATANH_INCLUDED
#define BOOST_MATH_COMPLEX_ATANH_INCLUDED

#ifndef BOOST_MATH_COMPLEX_DETAILS_INCLUDED
#  include <boost/math/complex/details.hpp>
#endif
#ifndef BOOST_MATH_LOG1P_INCLUDED
#  include <boost/math/special_functions/log1p.hpp>
#endif
#include <boost/assert.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; using ::fabs; using ::acos; using ::asin; using ::atan; using ::atan2; }
#endif

namespace boost{ namespace math{

template<class T>
std::complex<T> atanh(const std::complex<T>& z)
{
   //
   // References:
   //
   // Eric W. Weisstein. "Inverse Hyperbolic Tangent."
   // From MathWorld--A Wolfram Web Resource.
   // http://mathworld.wolfram.com/InverseHyperbolicTangent.html
   //
   // Also: The Wolfram Functions Site,
   // http://functions.wolfram.com/ElementaryFunctions/ArcTanh/
   //
   // Also "Abramowitz and Stegun. Handbook of Mathematical Functions."
   // at : http://jove.prohosting.com/~skripty/toc.htm
   //

   static const T half_pi = static_cast<T>(1.57079632679489661923132169163975144L);
   static const T pi = static_cast<T>(3.141592653589793238462643383279502884197L);
   static const T one = static_cast<T>(1.0L);
   static const T two = static_cast<T>(2.0L);
   static const T four = static_cast<T>(4.0L);
   static const T zero = static_cast<T>(0);
   static const T a_crossover = static_cast<T>(0.3L);

   T x = std::fabs(z.real());
   T y = std::fabs(z.imag());

   T real, imag;  // our results

   T safe_upper = detail::safe_max(two);
   T safe_lower = detail::safe_min(static_cast<T>(2));

   //
   // Begin by handling the special cases specified in C99:
   //
   if(detail::test_is_nan(x))
   {
      if(detail::test_is_nan(y))
         return std::complex<T>(x, x);
      else if(std::numeric_limits<T>::has_infinity && (y == std::numeric_limits<T>::infinity()))
         return std::complex<T>(0, ((z.imag() < 0) ? -half_pi : half_pi));
      else
         return std::complex<T>(x, x);
   }
   else if(detail::test_is_nan(y))
   {
      if(x == 0)
         return std::complex<T>(x, y);
      if(std::numeric_limits<T>::has_infinity && (x == std::numeric_limits<T>::infinity()))
         return std::complex<T>(0, y);
      else
         return std::complex<T>(y, y);
   }
   else if((x > safe_lower) && (x < safe_upper) && (y > safe_lower) && (y < safe_upper))
   {

      T xx = x*x;
      T yy = y*y;
      T x2 = x * two;

      ///
      // The real part is given by:
      //
      // real(atanh(z)) == log((1 + x^2 + y^2 + 2x) / (1 + x^2 + y^2 - 2x))
      //
      // However, when x is either large (x > 1/E) or very small
      // (x < E) then this effectively simplifies
      // to log(1), leading to wildly inaccurate results.
      // By dividing the above (top and bottom) by (1 + x^2 + y^2) we get:
      //
      // real(atanh(z)) == log((1 + (2x / (1 + x^2 + y^2))) / (1 - (-2x / (1 + x^2 + y^2))))
      //
      // which is much more sensitive to the value of x, when x is not near 1
      // (remember we can compute log(1+x) for small x very accurately).
      //
      // The cross-over from one method to the other has to be determined
      // experimentally, the value used below appears correct to within a
      // factor of 2 (and there are larger errors from other parts
      // of the input domain anyway).
      //
      T alpha = two*x / (one + xx + yy);
      if(alpha < a_crossover)
      {
         real = boost::math::log1p(alpha) - boost::math::log1p(-alpha);
      }
      else
      {
         T xm1 = x - one;
         real = boost::math::log1p(x2 + xx + yy) - std::log(xm1*xm1 + yy);
      }
      real /= four;
      if(z.real() < 0)
         real = -real;

      imag = std::atan2((y * two), (one - xx - yy));
      imag /= two;
      if(z.imag() < 0)
         imag = -imag;
   }
   else
   {
      //
      // This section handles exception cases that would normally cause
      // underflow or overflow in the main formulas.
      //
      // Begin by working out the real part, we need to approximate
      //    alpha = 2x / (1 + x^2 + y^2)
      // without either overflow or underflow in the squared terms.
      //
      T alpha = 0;
      if(x >= safe_upper)
      {
         // this is really a test for infinity,
         // but we may not have the necessary numeric_limits support:
         if((x > (std::numeric_limits<T>::max)()) || (y > (std::numeric_limits<T>::max)()))
         {
            alpha = 0;
         }
         else if(y >= safe_upper)
         {
            // Big x and y: divide alpha through by x*y:
            alpha = (two/y) / (x/y + y/x);
         }
         else if(y > one)
         {
            // Big x: divide through by x:
            alpha = two / (x + y*y/x);
         }
         else
         {
            // Big x small y, as above but neglect y^2/x:
            alpha = two/x;
         }
      }
      else if(y >= safe_upper)
      {
         if(x > one)
         {
            // Big y, medium x, divide through by y:
            alpha = (two*x/y) / (y + x*x/y);
         }
         else
         {
            // Small x and y, whatever alpha is, it's too small to calculate:
            alpha = 0;
         }
      }
      else
      {
         // one or both of x and y are small, calculate divisor carefully:
         T div = one;
         if(x > safe_lower)
            div += x*x;
         if(y > safe_lower)
            div += y*y;
         alpha = two*x/div;
      }
      if(alpha < a_crossover)
      {
         real = boost::math::log1p(alpha) - boost::math::log1p(-alpha);
      }
      else
      {
         // We can only get here as a result of small y and medium sized x,
         // we can simply neglect the y^2 terms:
         BOOST_ASSERT(x >= safe_lower);
         BOOST_ASSERT(x <= safe_upper);
         //BOOST_ASSERT(y <= safe_lower);
         T xm1 = x - one;
         real = std::log(1 + two*x + x*x) - std::log(xm1*xm1);
      }

      real /= four;
      if(z.real() < 0)
         real = -real;

      //
      // Now handle imaginary part, this is much easier,
      // if x or y are large, then the formula:
      //    atan2(2y, 1 - x^2 - y^2)
      // evaluates to +-(PI - theta) where theta is negligible compared to PI.
      //
      if((x >= safe_upper) || (y >= safe_upper))
      {
         imag = pi;
      }
      else if(x <= safe_lower)
      {
         //
         // If both x and y are small then atan(2y),
         // otherwise just x^2 is negligible in the divisor:
         //
         if(y <= safe_lower)
            imag = std::atan2(two*y, one);
         else
         {
            if((y == zero) && (x == zero))
               imag = 0;
            else
               imag = std::atan2(two*y, one - y*y);
         }
      }
      else
      {
         //
         // y^2 is negligible:
         //
         if((y == zero) && (x == one))
            imag = 0;
         else
            imag = std::atan2(two*y, 1 - x*x);
      }
      imag /= two;
      if(z.imag() < 0)
         imag = -imag;
   }
   return std::complex<T>(real, imag);
}

} } // namespaces

#endif // BOOST_MATH_COMPLEX_ATANH_INCLUDED


#include <CGAL/Surface_mesh.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>
#include <CGAL/Simple_cartesian.h>

#include <boost/graph/graph_concepts.hpp>
#include <CGAL/boost/graph/graph_concepts.h>

typedef CGAL::Simple_cartesian<double> K;
typedef CGAL::Surface_mesh<K::Point_3> SM;
typedef CGAL::Face_filtered_graph<SM, SM::Property_map<boost::graph_traits<SM>::face_descriptor , std::size_t> > Adapter;
typedef boost::graph_traits< Adapter > Traits;
typedef Traits::edge_descriptor edge_descriptor;
typedef Traits::halfedge_descriptor halfedge_descriptor;
typedef Traits::vertex_descriptor vertex_descriptor;
typedef Traits::face_descriptor face_descriptor;

void concept_check_adapter()
{
  boost::function_requires< boost::GraphConcept<Adapter> >();
  boost::function_requires< boost::VertexListGraphConcept<Adapter> >();
  boost::function_requires< boost::EdgeListGraphConcept<Adapter> >();
  boost::function_requires< boost::IncidenceGraphConcept<Adapter> >();
  boost::function_requires< boost::AdjacencyMatrixConcept<Adapter> >();
  boost::function_requires< boost::BidirectionalGraphConcept<Adapter> >();
  boost::function_requires< CGAL::HalfedgeGraphConcept<Adapter> >();
  boost::function_requires< CGAL::HalfedgeListGraphConcept<Adapter> >();
  boost::function_requires< CGAL::FaceGraphConcept<Adapter> >();
  boost::function_requires< CGAL::FaceListGraphConcept<Adapter> >();

  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Adapter, halfedge_descriptor, CGAL::halfedge_index_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Adapter, edge_descriptor, boost::edge_index_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Adapter, edge_descriptor, boost::edge_weight_t> >();
  boost::function_requires< boost::concepts::PropertyGraph<
    Adapter, vertex_descriptor, CGAL::vertex_point_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Adapter, vertex_descriptor, boost::vertex_index_t> >();
  boost::function_requires< boost::concepts::ReadablePropertyGraph<
    Adapter, face_descriptor, CGAL::face_index_t> >();

  // null
  boost::graph_traits<Adapter>::null_vertex();
  boost::graph_traits<Adapter>::null_halfedge();
  boost::graph_traits<Adapter>::null_face();
}

int main()
{
  concept_check_adapter();
  return 0;
}