/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Texas A&M University
*  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 Texas A&M University 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.
*********************************************************************/

/* Author: Saurav Agarwal */

#include <numeric>
#include <boost/thread.hpp>
#include <boost/foreach.hpp>
#include "Planner/NBM3P.h"
#include "Visualization/Visualizer.h"
#include "Utils/FIRMUtils.h"
#include "ObservationModels/CamAruco2DObservationModel.h"
#include "LinearSystem/LinearSystem.h"

#define foreach BOOST_FOREACH
#define foreach_reverse BOOST_REVERSE_FOREACH

namespace ompl
{
    namespace magic
    {
        static const double COLISSION_FAILURE_COST  = 1e6;// cost of colliding

        static const double RRT_PLAN_MAX_TIME = 1.0; // maximum time allowed for RRT to plan

        static const double RRT_FINAL_PROXIMITY_THRESHOLD = 1.0; // maximum distance for RRT to succeed

        static const double NEIGHBORHOOD_RANGE = 20.0 ; // 20(6cw), 12 (4cw) range within which to find neighbors

        static const float MIN_ROBOT_CLEARANCE = 0.10;

        static const double SIGMA_RANGE = 0.5;// meters

        static const double SIGMA_THETA = 0.2; // radians

        static const double MODE_DELETION_THRESHOLD = 1e-4; // the percentage of weight a mode should hold, below which it gets deleted

        static const double ZERO_CONTROL_UPDATE_TIME = 1.0 ;

        static const double SAMPLING_ROTATION_SPACING = 5.0; // degrees

        static const double SAMPLING_GRID_SIZE = 0.50 ; // meters
    }
}

void NBM3P::sampleNewBeliefStates()
{
    /**
    Logic:
    1. Grid the environment
    2. At each point in grid, put a robot with periodic orientations (10 degree spacing)
    */
    //Make sure the current states and weights are cleared out
    currentBeliefStates_.clear();
    weights_.clear();

    ompl::base::StateSpacePtr sp = si_->getStateSpace();
    ompl::base::RealVectorBounds bounds = sp->as<SE2BeliefSpace>()->getBounds();

    //Get the environment boundaries
    double X_1 = bounds.low[0];
    double X_2 = bounds.high[0];
    double Y_1 = bounds.low[1];
    double Y_2 = bounds.high[1];

    double spacing = ompl::magic::SAMPLING_GRID_SIZE;

    // grid size
    int gridSizeX = std::ceil( (X_2-X_1) / spacing);
    int gridSizeY = std::ceil( (Y_2-Y_1) / spacing);

    double rotationSpacing = FIRMUtils::degree2Radian(ompl::magic::SAMPLING_ROTATION_SPACING);// radians

    int numHeadings = std::floor(2*boost::math::constants::pi<double>()/rotationSpacing);

    arma::mat cov = arma::eye(3,3);
    cov(0,0) = 0.25;
    cov(1,1) = 0.25;
    cov(2,2) = 0.01;

    OMPL_INFORM("NBM3P: Sampling start beliefs");

    for(int i = 0; i <= gridSizeX; i++)
    {
        for(int j=0; j <= gridSizeY; j++)
        {
            double newX = X_1 + i*spacing;
            double newY = Y_1 + j*spacing;

            for(int k =0; k < numHeadings; k++ )
            {
                double newYaw = -boost::math::constants::pi<double>() + k*rotationSpacing;

                ompl::base::State *newState = si_->allocState();

                newState->as<SE2BeliefSpace::StateType>()->setXYYaw(newX, newY, newYaw);
                newState->as<SE2BeliefSpace::StateType>()->setCovariance(cov);

                if(si_->isValid(newState))
                {
                    currentBeliefStates_.push_back(si_->cloneState(newState));
                    weights_.push_back(0.0); // push zero weight
                    timeSinceDivergence_.push_back(0.0);
                }
                si_->freeState(newState);
            }
        }
    }

    OMPL_INFORM("NBM3P: Sampling beliefs Completed");

    assignUniformWeight();

    OMPL_INFORM("NBM3P: Sample Belief Weights Added");

    arma::colvec obs = policyExecutionSI_->getObservation();

    OMPL_INFORM("NBM3P: Updating the weights before proceeding");

    unsigned int numModes = currentBeliefStates_.size();
    unsigned int updatedNumModes = 0;

    // We will update weights till we converge to most likely modes
    while( numModes != updatedNumModes )
    {
        numModes = currentBeliefStates_.size();

        this->updateWeights(obs);

        updatedNumModes = currentBeliefStates_.size();

    }

    // show the beliefs
    this->drawBeliefs();

    this->printWeights();

    // After converging to most likely, assign uniform weights
    assignUniformWeight();

    double t = 0;

    // Run filter for a few seconds to update initial samples, zero control
    while( t < ompl::magic::ZERO_CONTROL_UPDATE_TIME )
    {

        this->propagateBeliefs(si_->getMotionModel()->getZeroControl());

        t += si_->getMotionModel()->getTimestepSize();

    }

    // Before beginning the strategy, assign all modes the same weight
    assignUniformWeight();

    this->printWeights();

    // Print the states
    OMPL_INFORM("NBM3P: The final sampled beliefs are :");
    for(int i=0; i< currentBeliefStates_.size(); i++)
    {
        si_->printState(currentBeliefStates_[i]);
    }

}

void NBM3P::generatePolicy(std::vector<ompl::control::Control*> &policy)
{
    si_->showRobotVisualization(false);

    Visualizer::clearOpenLoopRRTPaths();

    //container to store the sequence of controls for each mode/target pair
    std::vector<std::vector<ompl::control::Control*> > openLoopPolicies;

    std::vector<ompl::geometric::PathGeometric> rrtPaths;

    // Iterate over the mode/target pairs and generate open loop controls

    auto start_time_policygen = std::chrono::high_resolution_clock::now();

    for(unsigned int i = 0; i < currentBeliefStates_.size(); i++)
    {

        //Generate a path for the mode/target pair
        if( si_->isValid(currentBeliefStates_[i]) )
        {

            ompl::base::PlannerPtr planner(new ompl::geometric::RRTstar(si_));

            ompl::base::ProblemDefinitionPtr pdef(new ompl::base::ProblemDefinition(si_));

            Vertex targetVertex = findTarget(i);

            pdef->setStartAndGoalStates(currentBeliefStates_[i], stateProperty_[targetVertex], ompl::magic::RRT_FINAL_PROXIMITY_THRESHOLD);

            planner->as<ompl::geometric::RRT>()->setRange(1.0);

            planner->setProblemDefinition(pdef);

            planner->setup();

            ompl::base::PlannerStatus solved = planner->solve(ompl::magic::RRT_PLAN_MAX_TIME);

            if(solved)
            {
                const ompl::base::PathPtr &path = pdef->getSolutionPath();

                ompl::geometric::PathGeometric gpath = static_cast<ompl::geometric::PathGeometric&>(*path);

                rrtPaths.push_back(gpath);

                Visualizer::addOpenLoopRRTPath(gpath);

                //boost::this_thread::sleep(boost::posix_time::milliseconds(50));

                std::vector<ompl::control::Control*> olc;

                si_->getMotionModel()->generateOpenLoopControlsForPath(gpath, olc);

                openLoopPolicies.push_back(olc);

            }

            planner->clear();
        }
    }

    // Now that you have the open loop controls, need to execute them on all modes to see which is best
    std::vector<ompl::base::Cost> policyInfGains;

    double maxGain = -1e10;

    int maxGainPolicyIndx=-1;

    OMPL_INFORM("Evaluating the Open Loop policies on all the modes");

    //boost::this_thread::sleep(boost::posix_time::milliseconds(100));

    Visualizer::doSaveVideo(false);

    for(unsigned int i = 0; i < openLoopPolicies.size(); i++)
    {
        ompl::base::Cost pGain(0);

        pGain = ompl::base::Cost(0);

        for(unsigned int j = 0; j < currentBeliefStates_.size(); j++)
        {

            OMPL_INFORM("NBM3P: Evaluating Policy Number #%u  on Mode Number #%u",i,j);

            ompl::base::Cost c = executeOpenLoopPolicyOnMode(openLoopPolicies[i],currentBeliefStates_[j]);

            //pGain.v += c.v;

            pGain = ompl::base::Cost(pGain.value() + c.value());

        }

        //pGain.v = weights_[i]*pGain.v;
        pGain = ompl::base::Cost(weights_[i]*pGain.value());

        OMPL_INFORM("NBM3P: Weighted Information Gain for Policy #%u = %f",i,pGain.value());

        policyInfGains.push_back(pGain);

        if(pGain.value() >= maxGain)
        {
            maxGain = pGain.value();

            maxGainPolicyIndx = i;
        }
    }

    OMPL_INFORM("NBM3P: Max gain policy index = %u", maxGainPolicyIndx);

    //std::cin.get();

    Visualizer::clearOpenLoopRRTPaths();

    //TODO: If all policies have same expected gain, then pick one randomly
    if( maxGainPolicyIndx >= 0)
    {
        OMPL_INFORM("NBM3P: A minimum cost policy was found");

        policy = openLoopPolicies[maxGainPolicyIndx];

        previousPolicy_ = policy;

        Visualizer::addOpenLoopRRTPath(rrtPaths[maxGainPolicyIndx]);

    }
    else
    {
        policy = previousPolicy_;
    }

    auto end_time_policygen = std::chrono::high_resolution_clock::now();

    std::cout << "Time to evaluate policy: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_time_policygen - start_time_policygen).count() << " milli seconds."<<std::endl;

    //std::cin.get();

    si_->showRobotVisualization(true);

}

ompl::base::Cost NBM3P::executeOpenLoopPolicyOnMode(std::vector<ompl::control::Control*> controls,
                                                                const ompl::base::State* state)
{

    double I1 = 0;
    //get weighted sum of trace of covariance
    for(int i = 0; i < currentBeliefStates_.size(); i++)
    {
         I1 +=   weights_[i]/arma::trace(currentBeliefStates_[i]->as<SE2BeliefSpace::StateType>()->getCovariance());
    }

    std::vector<float> weightsCopy = weights_;
    std::vector<ompl::base::State*> currentBeliefStatesCopy;

    for(int i=0; i < currentBeliefStates_.size(); i++)
    {
        currentBeliefStatesCopy.push_back(si_->cloneState(currentBeliefStates_[i]));
    }

    ompl::base::State *currentTrueState = si_->allocState();
    si_->getTrueState(currentTrueState);

    si_->setTrueState(state);

    ompl::base::Cost olpInfGain(0);

    for(int i=0; i < controls.size() ; i++)
    {
        si_->applyControl(controls[i],false);

        propagateBeliefs(controls[i], true);

        if(!si_->checkTrueStateValidity()/*!areCurrentBeliefsValid()*/)
        {
            //olpInfGain.v -= ompl::magic::COLISSION_FAILURE_COST/(i+1);
            olpInfGain = ompl::base::Cost(olpInfGain.value() - ompl::magic::COLISSION_FAILURE_COST/(i+1));
            OMPL_INFORM("NBM3P: Collided in sim");
            //std::cin.get();
            break;
        }

    }

    double I2 = 0;

    //get weighted sum of trace of covariance
    for(int i = 0; i < currentBeliefStates_.size(); i++)
    {
         I2 +=   weights_[i]/arma::trace(currentBeliefStates_[i]->as<SE2BeliefSpace::StateType>()->getCovariance());
    }

    double changeInNumberOfModes = (double)(currentBeliefStatesCopy.size() - currentBeliefStates_.size());

    OMPL_INFORM("The discrete change in number of modes: %f",  changeInNumberOfModes);

    //olpInfGain.v +=  changeInNumberOfModes;
    olpInfGain = ompl::base::Cost(olpInfGain.value() + changeInNumberOfModes);

    // reset old weight values
    weights_ = weightsCopy;

    // free unneeded states
    for(int i = 0; i < currentBeliefStates_.size(); i++)
    {
         si_->freeState(currentBeliefStates_[i]);
    }

    // set back to old states
    currentBeliefStates_ = currentBeliefStatesCopy;

    // set back true state
    si_->setTrueState(currentTrueState);

    return olpInfGain;

}

void NBM3P::propagateBeliefs(const ompl::control::Control *control, bool isSimulation)
{
    // To propagate beliefs we need to apply the control to the true state, get observations and update the beliefs.
    ExtendedKF kf(si_);

    LinearSystem dummy;

    arma::colvec obs = policyExecutionSI_->getObservation();

    for(unsigned int i = 0; i < currentBeliefStates_.size(); i++)
    {
        ompl::base::State *kfEstimate = si_->allocState();
        ompl::base::State *kfEstimateUpdated = si_->allocState();

        si_->copyState(kfEstimate, currentBeliefStates_[i]);

        kf.Evolve(kfEstimate, control, obs, dummy, dummy, kfEstimateUpdated);

        si_->copyState(currentBeliefStates_[i], kfEstimateUpdated);

        si_->freeState(kfEstimate);

        si_->freeState(kfEstimateUpdated);

    }

    /*
    if(!isSimulation)
    {
        OMPL_INFORM("NBM3P: BEFORE updating weights in propogate: ");
        this->printWeights();
    }
    */

    this->updateWeights(obs);

    /*
    if(!isSimulation)
    {
        OMPL_INFORM("NBM3P: AFTER updating weights in propogate: ");
        this->printWeights();
    }
    */

    this->removeDuplicateModes();

    if(!isSimulation)
        this->drawBeliefs();

}

bool NBM3P::areSimilarWeights()
{

    arma::colvec w(weights_.size(),1);

    for(unsigned int i = 0; i < weights_.size(); i++)
    {
        w[i] = weights_[i];
    }

    double sd = arma::stddev(w);

    if(sd < 1e-6)
    {
        return true;
    }

    return false;
}

void NBM3P::updateWeights(const arma::colvec trueObservation)
{

    arma::colvec sigma(2);

    sigma(0) = ompl::magic::SIGMA_RANGE;
    sigma(1) = ompl::magic::SIGMA_THETA;

    arma::colvec variance = arma::pow(sigma,2);

    float totalWeight = 0.0;

    for(unsigned int i = 0; i < currentBeliefStates_.size(); i++)
    {

        double weightFactor= 1.0;

        arma::colvec innov = this->computeInnovation(i,trueObservation,weightFactor);

        float w;

        // robot and mode see something in common
        if(innov.n_rows != 0)
        {
            arma::mat covariance = arma::diagmat(arma::repmat(arma::diagmat(variance), innov.n_rows/2, innov.n_rows/2)) ;

            arma::mat t = -0.5*trans(innov)*covariance.i()*innov;

            w = std::exp(t(0,0)) * weightFactor;

        }
        // if there is no innov it means there is no common observation
        else
        {
            // case 1: robot also doesnt see anything
            if(trueObservation.n_rows ==0)
            {
                // mode could be seeing something
                w = weightFactor;
            }
            // robot is seeing something and mode sees nothing in common
            else
            {
                w = 0;
            }
        }

        weights_[i]  = weights_[i]*w;

        totalWeight += weights_[i];

    }

    // if totalWeight becomes 0, reset to uniform distribution
    if(totalWeight==0)
    {
        for(unsigned int i=0; i< weights_.size(); i++)
        {
            weights_[i] = 1.0/weights_.size();
        }
        return;
    }
    else
    {
        normalizeWeights();

    }

    std::vector<int> beliefsToRemove;

    for(unsigned int i = 0; i < weights_.size(); i++)
    {
        // if the weight of the mode is less than threshold, delete it
        if(weights_[i]/totalWeight < ompl::magic::MODE_DELETION_THRESHOLD || weights_[i] == 0.0 )
        {
            beliefsToRemove.push_back(i);
        }
    }

    removeBeliefs(beliefsToRemove);

}

/*
// Alternate implementation of calculating weight update
float NBM3P::computeWeightForMode(const int currentBeliefIndx,const arma::colvec trueObservation)
{
    const int singleObservationDim = 4;//CamAruco2DObservationModel::singleObservationDim;

    const int landmarkInfoDim = 2;//CamAruco2DObservationModel::landmarkInfoDim;

    // the true observation
    arma::colvec Zg = trueObservation;

    int landmarksActuallySeen = Zg.n_rows / singleObservationDim;

    // the belief's predicted observation for what the robot sees
    arma::colvec Zprd =  si_->getObservationModel()->getObservationPrediction(currentBeliefStates_[currentBeliefIndx], Zg);

    // get observation
    arma::colvec innov = si_->getObservationModel()->computeInnovation(currentBeliefStates_[currentBeliefIndx], obs);

    float weightUpdate = 1.0;

    // robot and mode see something in common
    if(innov.n_rows != 0)
    {
        // construct a linear system at where the mode is
        LinearSystem ls(si_,currentBeliefStates_[currentBeliefIndx],si_->getMotionModel()->getZeroControl(), Zprd, si_->getMotionModel(), si_->getObservationModel());

        arma::mat P = currentBeliefStates_[currentBeliefIndx]->as<SE2BeliefSpace::StateType>()->getCovariance();

        mat obsLikelihood = ls.getH() * P * trans(ls.getH()) + ls.getR();

        arma::mat t = -0.5*trans(innov)*obsLikelihood.i()*innov;

        weightUpdate = t(0,0);

    }
    // if there is no innov it means there is no common observation
    else
    {
        // case 1: robot also doesnt see anything
        if(Zg.n_rows ==0)
        {
            // mode could be seeing something
            weightUpdate = 1.0;
        }
        // robot is seeing something and mode sees nothing in common
        else
        {
            weightUpdate = 0;
        }
    }

     // the beliefs predicted observation for what is should see based on where it is
    arma::colvec Zmode =  si_->getObservationModel()->getObservation(currentBeliefStates_[currentBeliefIndx], false);

    int numIntersection=0;

    int predictedLandmarksSeen = Zmode.n_rows / singleObservationDim ;

    for(int j = 0 ; j < landmarksActuallySeen ; j++)
    {
        // match ids
        for(int k = 0 ; k < predictedLandmarksSeen ; k++)
        {
            if(Zprd(k*singleObservationDim) == Zg(j*singleObservationDim))
            {

                numIntersection++;

                break;
            }
        }

    }

    float weightReductionFactor=1.0;

    // there is a mismatch in what the robot sees and predicted
    if(numIntersection != landmarksActuallySeen || numIntersection != predictedLandmarksSeen )
    {

        //weightFactor = std::min(1.0 / abs(1 + landmarksActuallySeen - numIntersection) , 1.0 / abs(1 + predictedLandmarksSeen - numIntersection));
        float heuristicVal = std::max(abs(1 + landmarksActuallySeen - numIntersection) , abs(1 + predictedLandmarksSeen - numIntersection));

        weightReductionFactor = std::exp(-heuristicVal*timeSinceDivergence_[currentBeliefIndx]);

        timeSinceDivergence_[currentBeliefIndx] = timeSinceDivergence_[currentBeliefIndx] + std::pow(si_->getMotionModel()->getTimestepSize(),2);

    }
    else
    {
         timeSinceDivergence_[currentBeliefIndx] = timeSinceDivergence_[currentBeliefIndx] - 1.0;

         if(timeSinceDivergence_[currentBeliefIndx] < 0)
         {
            timeSinceDivergence_[currentBeliefIndx] = 0;
         }
    }

    return weightUpdate*weightReductionFactor;

}
*/

arma::colvec NBM3P::computeInnovation(const int currentBeliefIndx,const arma::colvec trueObservation, double &weightFactor)
{
    const int singleObservationDim = 4;//CamAruco2DObservationModel::singleObservationDim;

    const int landmarkInfoDim = 2;//CamAruco2DObservationModel::landmarkInfoDim;

    // the true observation
    arma::colvec Zg = trueObservation;

    int landmarksActuallySeen = Zg.n_rows / singleObservationDim;

    // the beliefs predicted observation
    arma::colvec Zprd =  si_->getObservationModel()->getObservation(currentBeliefStates_[currentBeliefIndx], false);

    int predictedLandmarksSeen = Zprd.n_rows / singleObservationDim ;

    arma::colvec innov;

    int numIntersection=0;

    //First we see if a landmark that is observed by robot is seen by mode:
    //1. If yes, then we compute the innov
    //2. If no, then we predict where the mode "would" see that particular landmark and then calculate the innov
    for(int j = 0 ; j < landmarksActuallySeen ; j++)
    {
        // match ids
        for(int k = 0 ; k < predictedLandmarksSeen ; k++)
        {
            if(Zprd(k*singleObservationDim) == Zg(j*singleObservationDim))
            {

                arma::colvec innov_for_landmark(2);

                innov_for_landmark(0) = Zg(j*singleObservationDim + 1) - Zprd(k*singleObservationDim + 1) ;

                double delta_theta = Zg(j*singleObservationDim + 2) - Zprd(k*singleObservationDim + 2) ;

                FIRMUtils::normalizeAngleToPiRange(delta_theta);

                assert(abs(delta_theta) <= 2*boost::math::constants::pi<double>()) ;

                innov_for_landmark(1) =  delta_theta;

                innov.insert_rows(numIntersection*landmarkInfoDim, innov_for_landmark);

                numIntersection++;

                break;
            }
        }

    }

    // there is a mismatch in what the robot sees and predicted
    if(numIntersection != landmarksActuallySeen || numIntersection != predictedLandmarksSeen )
    {

        //weightFactor = std::min(1.0 / abs(1 + landmarksActuallySeen - numIntersection) , 1.0 / abs(1 + predictedLandmarksSeen - numIntersection));
        float heuristicVal = std::max(abs(1 + landmarksActuallySeen - numIntersection) , abs(1 + predictedLandmarksSeen - numIntersection));

        weightFactor = std::exp(-heuristicVal*timeSinceDivergence_[currentBeliefIndx]*1e-4);

        timeSinceDivergence_[currentBeliefIndx] = timeSinceDivergence_[currentBeliefIndx] + std::pow(si_->getMotionModel()->getTimestepSize(),1);

    }
    else
    {
         //timeSinceDivergence_[currentBeliefIndx] = timeSinceDivergence_[currentBeliefIndx] - 1.0;

         //if(timeSinceDivergence_[currentBeliefIndx] < 0)
         //{
            timeSinceDivergence_[currentBeliefIndx] = 0;
         //}
    }

    return innov;
}

void NBM3P::removeBeliefs(const std::vector<int> Indxs)
{
    // Make a local copy
    std::vector<ompl::base::State*> currentBeliefStatesCopy = currentBeliefStates_;
    std::vector<float> weightsCopy = weights_;
    std::vector<double>  timeSinceDivergenceCopy = timeSinceDivergence_;

    weights_.clear();
    currentBeliefStates_.clear();
    timeSinceDivergence_.clear();

    for(int i = 0 ; i < currentBeliefStatesCopy.size(); i++)
    {
        // check if this index is in the list marked to be deleted. If not, then we keep it
        std::vector<int>::const_iterator it = std::find(Indxs.begin(), Indxs.end(), i) ;

        if(it == Indxs.end())
        {
            currentBeliefStates_.push_back(currentBeliefStatesCopy[i]);
            weights_.push_back(weightsCopy[i]);
            timeSinceDivergence_.push_back(timeSinceDivergenceCopy[i]);
        }
        // if it was marked to be deleted, then we free memory for that state
        else
        {
            si_->freeState(currentBeliefStatesCopy[i]);
        }
    }

    normalizeWeights();

}

bool NBM3P::isConverged()
{

    if(currentBeliefStates_.size()==1)
    {
        return true;
    }

    return false;
}

 bool NBM3P::doCurrentBeliefsSatisfyClearance(int currentStep)
{

    int clearanceHorizon  = 20; // check 10 steps ahead

    if(currentBeliefStates_.size()==1)
        return true;

    for(int i =0 ; i< currentBeliefStates_.size(); i++)
    {

        ompl::base::State* tempState = si_->cloneState(currentBeliefStates_[i]);

        for(int j = currentStep; j < currentStep + clearanceHorizon ; j++)
        {
            if(j < previousPolicy_.size())
            {
                si_->getMotionModel()->Evolve(tempState, previousPolicy_[j], si_->getMotionModel()->getZeroNoise(),tempState);

                if(!si_->isValid(tempState))
                {
                    return false;
                }
            }

        }

        si_->freeState(tempState);

        /*
        double clearance  = si_->getStateValidityChecker()->clearance(currentBeliefStates_[i]) ;

        if( clearance < ompl::magic::MIN_ROBOT_CLEARANCE)
        {
            OMPL_INFORM("NBM3P: Mode #%u  clearance: %f", i, clearance);

            //std::cin.get();

            return false;
        }
        */

    }

    return true;
}

 bool NBM3P::areCurrentBeliefsValid()
{
    if(currentBeliefStates_.size()==1)
        return true;

    for(int i =0 ; i< currentBeliefStates_.size(); i++)
    {
        if(!si_->isValid(currentBeliefStates_[i]))
            return false;

    }

    return true;
}

void NBM3P::removeDuplicateModes()
{
    std::vector<int> toDelete;

    for(int i = 0; i < currentBeliefStates_.size(); i++)
    {
        for(int j = i+1; j < currentBeliefStates_.size(); j++ )
        {
            if(i!=j)
            {
                arma::colvec xi = currentBeliefStates_[i]->as<SE2BeliefSpace::StateType>()->getArmaData();
                arma::colvec xj = currentBeliefStates_[j]->as<SE2BeliefSpace::StateType>()->getArmaData();

                double xd = xi(0) - xj(0);
                double yd = xi(1) - xj(1);
                double thetad = xi(2) - xj(2);

                if(std::abs(xd) < 0.01 && std::abs(yd) < 0.01 &&  std::abs(thetad) < FIRMUtils::degree2Radian(1.0) )
                {

                    if(weights_[i] >= weights_[j] )
                    {
                        toDelete.push_back(j);
                        weights_[i] = weights_[i] + weights_[j]; // transfer the weight to the more likely mode
                    }
                    else
                    {
                        toDelete.push_back(i);
                        weights_[j] = weights_[i] + weights_[j]; // transfer the weight to the more likely mode

                    }

                }
            }

        }
    }

    this->removeBeliefs(toDelete);

}

void NBM3P::drawBeliefs()
{

    Visualizer::clearBeliefModes();

    for(unsigned int i = 0; i < currentBeliefStates_.size(); i++)
    {
        Visualizer::addBeliefMode(currentBeliefStates_[i]);
    }

}

void NBM3P::getStateWithMaxWeight(ompl::base::State *state, float &weight)
{

    std::vector<float>::iterator biggest = std::max_element(weights_.begin(), weights_.end());

    weight = *biggest;

    int index = std::distance(weights_.begin(),biggest);

    si_->copyState(state, currentBeliefStates_[index]);

}

void NBM3P::normalizeWeights()
{
    float totalWeight = std::accumulate(weights_.begin(), weights_.end(), 0.0);

    // Normalize the weights
     for(unsigned int i = 0; i < weights_.size(); i++)
    {
        weights_[i] =  weights_[i]/totalWeight;

    }
}

void NBM3P::assignUniformWeight()
{
    int Nm = weights_.size();

    float Wu = 1.0 / Nm;

    for(unsigned int i = 0; i < Nm ; i++)
    {
        weights_[i] =  Wu;

    }

}

void NBM3P::addStateToObservationGraph(ompl::base::State *state)
{

    // Add state to graph
    Vertex m = boost::add_vertex(g_);

    stateProperty_[m] = state;

    // Iterate over existing nodes and add edges
    foreach (Vertex n, boost::vertices(g_))
    {
        if ( m!=n && stateProperty_[m]!=stateProperty_[n])
        {
            addEdgeToObservationGraph(m,n);
        }
    }
}

void NBM3P::addEdgeToObservationGraph(const Vertex a, const Vertex b)
{

    // See if there is an overlap in observation between the two vertices
    unsigned int weight = 0;

    if(getObservationOverlap(a, b, weight))
    {
        // Add edge if overlap true, with weight = number of overlaps
        const unsigned int id = maxEdgeID_++;

        const Graph::edge_property_type properties(weight, id);

        // create an edge with the edge weight property
        std::pair<Edge, bool> newEdge = boost::add_edge(a, b, properties, g_);

    }

}

void NBM3P::evaluateObservationListForVertex(const Vertex v)
{
    // get Zero Noise Observation
    arma::colvec obs = si_->getObservationModel()->getObservation(stateProperty_[v], false);

    unsigned int singleObsSize = CamAruco2DObservationModel::singleObservationDim;

    std::vector<unsigned int> obsList;

    for(int i = 0; i < obs.n_rows/singleObsSize; i++)
    {
        obsList.push_back(obs[singleObsSize*i]);
    }

    stateObservationProperty_[v] = obsList;
}

bool NBM3P::getObservationOverlap(Vertex a, Vertex b, unsigned int &weight)
{

    // get the list of observations for both vertices
    evaluateObservationListForVertex(a);

    evaluateObservationListForVertex(b);

    bool isOverlapping = false;

    weight = 0;

    // if both nodes dont see anything, that is also an overlap.
    if(stateObservationProperty_[a].size() ==0 && stateObservationProperty_[b].size()==0)
    {
        weight++;
        return true;
    }

    // Check if there is an overlap
    for(int i = 0; i < stateObservationProperty_[a].size(); i++)
    {
        for(int j= 0; j < stateObservationProperty_[b].size(); j++)
        {
            // Check for overlap if they are not looking at the same physical landmark
            if(si_->distance(stateProperty_[a],stateProperty_[b]) > 4.0) // 4.0 is 2x the camera range
            {
                if(stateObservationProperty_[a][i] == stateObservationProperty_[b][j])
                {
                    // for every overlap, increase weight
                    weight++;

                    isOverlapping = true;

                }
            }
        }
    }

    return isOverlapping;
}

std::vector<NBM3P::Vertex> NBM3P::getNeighbors(const ompl::base::State *state)
{
    std::vector<Vertex> nn;

    foreach(Vertex v, boost::vertices(g_))
    {
        if(si_->distance(state, stateProperty_[v]) <  ompl::magic::NEIGHBORHOOD_RANGE)
        {
            nn.push_back(v);
        }
    }

    return nn;
}

NBM3P::Vertex NBM3P::findTarget(const unsigned int beliefStateIndx)
{
    // mapping from index of belief state to its neighbors
    std::map<unsigned int, std::vector<Vertex> > setOfAllNeighbors;

    // find the neighbors of all the belief States
    for(int i=0; i < currentBeliefStates_.size(); i++)
    {
        std::vector<Vertex> nn;

        nn = this->getNeighbors(currentBeliefStates_[i]);

        setOfAllNeighbors[i] = nn; // insert in mapping
    }

    std::vector<Vertex> neighborsOfBelief = setOfAllNeighbors[beliefStateIndx];

     // find the node in the required belief's neighbors that is least similar to nodes in other neighboorhoods
    int targetNodeIndx = -1;
    int minWeight = 1e6;

    // iterate over the nodes in the required belief's neighborhood;
    for(int i = 0; i < neighborsOfBelief.size(); i++)
    {
        int w = 0;

        // iterate over all other neighborhoods
        for(int j = 0; j < setOfAllNeighbors.size(); j++)
        {

            if(j != beliefStateIndx)
            {
                w += calculateIntersectionWithNeighbor(neighborsOfBelief[i],  setOfAllNeighbors[j]);

            }
        }

        if(w < minWeight)
        {
            minWeight = w;
            targetNodeIndx = i;
        }

        //If 2 targets have same weight, then choose the closer one
        /*
        if(w==minWeight && targetNodeIndx >= 0)
        {
            if(si_->distance(currentBeliefStates_[beliefStateIndx],stateProperty_[i]) <= si_->distance(currentBeliefStates_[beliefStateIndx], stateProperty_[targetNodeIndx]))
            {
                targetNodeIndx = i;
            }
        }
        */

    }

    assert(targetNodeIndx >= 0);

    return neighborsOfBelief[targetNodeIndx];
}

int NBM3P::calculateIntersectionWithNeighbor(const Vertex v, std::vector<Vertex> neighbors)
{
    int w = 0;

    foreach(Edge e, boost::out_edges(v, g_))
    {
        for(int i = 0; i < neighbors.size(); i++)
        {
            if(neighbors[i] == boost::target(e, g_))
            {
                // then get the edge weight
                int edgeWeight =  boost::get(boost::edge_weight, g_, e);

                w += edgeWeight;
            }
        }
    }

    return w;
}


///////////////////////////////////////////////////////////////////////////////////////////
// distribution::survival::example::data::dataset.cpp                                    //
//                                                                                //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                            //
//  Software License, Version 1.0. (See accompanying file                                //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                     //
///////////////////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <ostream>
#include <fstream>
#include <stdexcept>
#include <string> //needed?
#include <algorithm>
#include <iterator>
#include <boost/typeof/typeof.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/range.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/fusion/container/generation/make_map.hpp>
#include <boost/fusion/include/make_map.hpp>
#include <boost/statistics/detail/math/function/versatile_equal.hpp>

#include <boost/statistics/detail/distribution/model/tag/response.hpp>
#include <boost/statistics/detail/distribution/model/tag/covariate.hpp>

#include <boost/statistics/detail/distribution/survival/record/key/include.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/include.hpp>
#include <boost/statistics/detail/distribution/survival/record/function/include.hpp>

#include <libs/statistics/detail/distribution/survival/example/data/dataset.h>

void example_data_dataset(std::ostream& out)
{
    out << "-> example_data_dataset : ";

    // Steps shown in this example:
    //
    // Records creation
    // Events creation
    // I/O
    // Statistics

    using namespace boost;
    namespace stat = boost::statistics::detail;
    namespace ds = stat::distribution::survival;
    namespace dm = stat::distribution::model;

    typedef unsigned val_; // do not modify, results depend on it.

    // Record
    typedef dm::tag::covariate          tag_x_;
    typedef dm::tag::response           tag_y_;
    typedef ds::record::tag::entry_time      tag_et_;
    typedef ds::record::tag::failure_time    tag_ft_;
    typedef fusion::result_of::make_map<
        tag_et_,tag_ft_,val_,val_>::type record_;
    typedef std::vector<record_> records_;

    // Reponse
    typedef ds::response::event<val_> y_;
    typedef std::vector<y_> ys_;
    typedef ds::response::event_factory<val_> factory_;

    // I/O
    typedef boost::archive::text_oarchive oa_;
    typedef boost::archive::text_iarchive ia_;

    // Constants
    const unsigned n_record = 4;
    const val_ entry_bound = n_record - 1;
    const val_ fail_time = 2;
    // [ Records ]
    records_ records;
    for(val_ i = 0; i<n_record; i++){
        records.push_back(
            boost::fusion::make_map<tag_et_,tag_ft_>(i,fail_time)
        );
    }

    out << " records : ";
    for(val_ i = 0; i<n_record; i++){
        out << ds::record::description( records[i] ) << ' ';
    }

    // [ Responses ]
    {
        ys_ ys;

        std::transform(
            boost::begin(records),
            std::lower_bound(
                 boost::begin(records),
                 boost::end(records),
                 boost::fusion::make_map<tag_et_>(entry_bound),
                 ds::record::less_than()
            ),
            std::back_inserter(ys),
            factory_(entry_bound)
        );

        // Analysis for n_record = 4 and entry_bound = 3;
        // et: entry time
        // ft: failure time
        // dt: time since entry
        //     et      ft      dt     ft<=dt  min(ft,dt)
        //      0       2       3       1       2
        //      1       2       2       1       2
        //      2       2       1       0       1
        //      3       2       0       NA      NA

        BOOST_ASSERT(boost::size(ys) == entry_bound);

        // [ Mean ]

        out << "mean events : " << std::endl;

        typedef ds::response::mean_event<val_> me_;
        // Analysis for n_record = 4 and entry_bound = 3;
        //      j       ft<=dt  min(ft,dt)
        //      1       1       2
        //      2       1       2
        //      1       0       1
        // -------------------------------
        //      1       1/2=0   3/2 = 1         // Remember, val_ = unsigned,
        //      2       1       2               // not a float

    }
    {
    	// Mean events
        unsigned n_i = n_record / 2;
        for(unsigned j = 0; j<2; j++){
            records_ rs;
            for(unsigned i = 0; i<n_i; i++){
                rs.push_back( records[j + i*2] );
            }
            BOOST_ASSERT( boost::size(rs) == 2 );

            ys_ es;
            std::transform(
                boost::begin( rs ),
                std::lower_bound(
                    boost::begin( rs ),
                    boost::end( rs ),
                    boost::fusion::make_map<tag_et_>(entry_bound),
                    ds::record::less_than()
                ),
                std::back_inserter(es),
                factory_(entry_bound)
            );
			typedef ds::response::mean_event<val_> me_;
            me_ me = std::for_each( boost::begin(es), boost::end(es), me_() );
            out << me << std::endl;
        }
    }

    out << "<-" << std::endl;
}


#include <opencv3/core.hpp>
#include "opencv3/highgui.hpp"
// #include <opencv2/opencv.hpp>
// #include "loadsave.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <vector>
#include <fstream>
#include <string>

// function for converting image names and times into 2 arrays.
// one of image matrices and one of image times.
void readImagesAndTimes(char** filenames, float* timesArray, int numImages,
                        vector<Mat> &imageList, vector<float> &exposureTimeList)
{

  times.assign(timesArray, timesArray + numImages);
  for(int i=0; i < numImages; i++)
  {
    Mat im = imread(filenames[i]);
    images.push_back(im);
  }

}

int main(int argc, char* argv) {
  /* These should be collected from the commandline */
  static const char* filenames[] = {"img_0.033.jpg", "img_0.25.jpg", "img_2.5.jpg", "img_15.jpg"};
  static const float timesArray[] = {1/30.0f, 0.25, 2.5, 15.0};
  int numImages = 4;

  Eigen::MatrixXf responseList;
  Eigen::VectorXf weightList;
  Eigen::MatrixXf hdrImage;

  vector<Mat> images;
  vector<float> times;
  readImagesAndTimes(filenames, timesArray, numImages, image, times);

  // Align input images
  Ptr<AlignMTB> alignMTB = createAlignMTB();
  alignMTB->process(images, images);

}


#include "pch.h"
#include <openssl/rsa.h>
#include <openssl/pem.h>
#include <vcruntime_string.h>
#include <string>
#include <uchar.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>

#define PASS "" //口令

#define PUBLICKEY "publicKey.pem"
#define PRIVATEKEY "privateKey.pem"

using namespace std;

BOOL APIENTRY DllMain( HMODULE hModule,
                       DWORD  ul_reason_for_call,
                       LPVOID lpReserved
                     )
{
    switch (ul_reason_for_call)
    {
    case DLL_PROCESS_ATTACH:
    case DLL_THREAD_ATTACH:
    case DLL_THREAD_DETACH:
    case DLL_PROCESS_DETACH:
        break;
    }
    return TRUE;
}

bool Base64Encode(const string& input, string* output) {
	typedef boost::archive::iterators::base64_from_binary<boost::archive::iterators::transform_width<string::const_iterator, 6, 8> > Base64EncodeIterator;
	stringstream result;
	copy(Base64EncodeIterator(input.begin()), Base64EncodeIterator(input.end()), ostream_iterator<char>(result));
	size_t equal_count = (3 - input.length() % 3) % 3;
	for (size_t i = 0; i < equal_count; i++) {
		result.put('=');
	}
	*output = result.str();
	return output->empty() == false;
}

bool Base64Decode(const string& input, string* output) {
	typedef boost::archive::iterators::transform_width<boost::archive::iterators::binary_from_base64<string::const_iterator>, 8, 6> Base64DecodeIterator;
	stringstream result;
	try {
		copy(Base64DecodeIterator(input.begin()), Base64DecodeIterator(input.end()), ostream_iterator<char>(result));
	}
	catch (...) {
		return false;
	}
	*output = result.str();
	return output->empty() == false;
}

// 私钥解密
std::string rsa_pri_decrypt(const std::string& cipherText, std::string& priKey)
{
	std::string strRet;
	std::string temp_strRet;
	RSA* rsa = RSA_new();
	BIO* keybio;
	keybio = BIO_new_mem_buf((unsigned char*)priKey.c_str(), -1);

	rsa = PEM_read_bio_RSAPrivateKey(keybio, &rsa, NULL, NULL);
	int len = RSA_size(rsa);
	char* decryptedText = (char*)malloc(len + 1);
	memset(decryptedText, 0, len + 1);
	int step = 256;    //解密长度限制
	int olen = cipherText.length();
	int ret = 0;
	for (int i = 0; i < olen; i = i + step)
	{
		memset(decryptedText, '\0', len + 1);
		string tmp_str = cipherText.substr(i, step);
		ret = RSA_private_decrypt(tmp_str.length(), (const unsigned char*)tmp_str.c_str(), (unsigned char*)decryptedText, rsa, RSA_PKCS1_PADDING);
		if (ret >= 0) {
			temp_strRet = std::string(decryptedText, ret);
			strRet.append(temp_strRet);
		}
	}

	// 释放内存
	free(decryptedText);
	BIO_free_all(keybio);
	RSA_free(rsa);
	return strRet;
}

// 公钥加密
std::string rsa_pub_encrypt(const std::string& clearText, std::string& pubKey)
{
	std::string strRet;
	std::string temp_strRet;

	bool with_new_line = false;
	BIO* keybio = BIO_new_mem_buf((unsigned char*)pubKey.c_str(), -1);
	RSA* rsa = RSA_new();
	rsa = PEM_read_bio_RSA_PUBKEY(keybio, &rsa, NULL, NULL);
	if (!rsa)
	{
		BIO_free_all(keybio);
		return NULL;
	}
	int len = RSA_size(rsa);
	char* encryptedText = (char*)malloc(len + 1);
	memset(encryptedText, 0, len + 1);

	// 加密函数
	int step = 245;    //加密长度限制
	int olen = clearText.length();
	int ret = 0;
	for (int i = 0; i < olen; i = i + step)
	{
		memset(encryptedText, '\0', len + 1);
		string tmp_str = clearText.substr(i, step);
		ret = RSA_public_encrypt(tmp_str.length(), (const unsigned char*)tmp_str.c_str(), (unsigned char*)encryptedText, rsa, RSA_PKCS1_PADDING);
		if (ret >= 0) {
			temp_strRet = std::string(encryptedText, ret);
			strRet.append(temp_strRet);
		}
	}

	// 释放内存
	free(encryptedText);
	BIO_free_all(keybio);
	RSA_free(rsa);

	return strRet;
}

std::string ReadFile(const char* pFileName)
{
	ifstream inFile(pFileName);
	string contents("");
	if (inFile.is_open())
	{
		std::stringstream buffer;
		buffer << inFile.rdbuf();
		contents.append(buffer.str());
	}

	inFile.close();

	return contents.c_str();
}

extern "C" int _declspec(dllexport) RsaEncrypt(char* str, char*& outstr)// char* outstr， char*& outstr
{
	bool with_new_line = false;
	std::string publicRsa;

	publicRsa = ReadFile(PUBLICKEY);
	//char* result;
	//std::string result;
	int cc = 0;
	std::ofstream fout;
	fout.open("input.txt");
	fout << str;
	fout.close();
	//std::string str_input = str;
	std::string ret = "";
	//ret = rsa_pub_encrypt("aasdddtyu中tyu", publicRsa);
	ret = rsa_pub_encrypt(str, publicRsa);
	//cout << ret << endl;

	string base64_str;
	Base64Encode(ret, &base64_str);

	//std::ofstream fout;
	fout.open("test.txt");
	fout << base64_str;
	fout.close();

	strcpy(outstr, base64_str.c_str());;
	cc = 1;
	return cc;
}

extern "C" int _declspec(dllexport) RsaDecrypt(char* str, char*& outstr)// C# char* outstr， c/c++、delphi char*& outstr
{
	bool with_new_line = false;
	std::string privateRsa;

	privateRsa = ReadFile(PRIVATEKEY);
	//char* result;
	//std::string result;
	int cc = 0;

	std::string str_e_base64 = str;
	string output_str;
	Base64Decode(str_e_base64, &output_str);
	std::string ret = rsa_pri_decrypt(output_str, privateRsa);

	strcpy(outstr, ret.c_str());
	//strcpy(outstr, '\0');

	/*std::ofstream fout;
	fout.open("de_test.txt");
	fout << outstr;
	fout.close();*/

	cc = 1;
	return cc;
}

extern "C" int _declspec(dllexport) Test(char* str) // c#
{
	str[0] = 'p';
	int cc = 1;
	return cc;
}


//  lcm.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  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_RATIO_MPL_LCM_HPP
#define BOOST_RATIO_MPL_LCM_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/ratio/detail/mpl/lcm.hpp>

namespace boost {
namespace mpl {

template<>
struct lcm_impl< rational_c_tag,rational_c_tag >
{
    template< typename R1, typename R2 > struct apply
        : ratio_lcm<R1, R2>
    {
    };
};
}
}

#endif  // BOOST_RATIO_MPL_LCM_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_SCALAR_IS_INVALID_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SCALAR_IS_INVALID_HPP_INCLUDED

#include <boost/simd/predicates/functions/is_invalid.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT( is_invalid_, tag::cpu_
                            , (A0)
                            , (scalar_< bool_<A0> >)
                            )
  {
    typedef bool result_type;
    inline result_type operator()(A0 const&) const
    {
      return false;
    }
  };
  BOOST_DISPATCH_IMPLEMENT( is_invalid_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    inline result_type operator()(A0 const&) const
    {
      return boost::simd::False<result_type>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT( is_invalid_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return is_nan(a0-a0); }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_PROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_PROD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
   This function object returns the product of the elements of the argument

    @see sum

    @par Header <boost/simd/function/prod.hpp>

    @par Example:

      @snippet prod.cpp prod

    @par Possible output:

      @snippet prod.txt prod

  **/
  scalar_of_t<Value> prod(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/prod.hpp>
#include <boost/simd/function/simd/prod.hpp>

#endif


#include <boost/accumulators/statistics/error_of.hpp>


/*
Copyright (c) by respective owners including Yahoo!, Microsoft, and
individual contributors. All rights reserved.  Released under a BSD (revised)
license as described in the file LICENSE.
*/

#include <boost/test/unit_test.hpp>

#include "ds_explore.h"

using namespace boost::unit_test;
using namespace std;
using namespace Microsoft::DecisionService;

BOOST_AUTO_TEST_SUITE( DecisionServiceExploreSoftmax )

BOOST_AUTO_TEST_CASE( Softmax )
{
    SoftmaxExplorer explorer(1, 0);

    vector<float> scores = { 1, 2, 3 };
    DecisionServicePredictorsSimple predictor(scores);
    PredictorContainer container(&predictor);
    ActionProbabilities actionProbs = explorer.explore(container);

    BOOST_CHECK_EQUAL(actionProbs.size(), 3 );

    // sorted by score
    BOOST_CHECK_EQUAL(actionProbs[0].action, 2);
    BOOST_CHECK_EQUAL(actionProbs[1].action, 1);
    BOOST_CHECK_EQUAL(actionProbs[2].action, 0);

    // deviation expressed in %
    BOOST_CHECK_CLOSE(actionProbs[0].probability, 0.665, 0.5);
    BOOST_CHECK_CLOSE(actionProbs[1].probability, 0.244, 0.5);
    BOOST_CHECK_CLOSE(actionProbs[2].probability, 0.090, 0.5);
}

BOOST_AUTO_TEST_CASE( SoftmaxMinEpsilon )
{
    SoftmaxExplorer explorer(1, 0.45f);

    vector<float> scores = { 1, 2, 3 };
    DecisionServicePredictorsSimple predictor(scores);
    PredictorContainer container(&predictor);
    ActionProbabilities actionProbs = explorer.explore(container);

    BOOST_CHECK_EQUAL(actionProbs.size(), 3 );

    // sorted by score
    BOOST_CHECK_EQUAL(actionProbs[0].action, 2);
    BOOST_CHECK_EQUAL(actionProbs[1].action, 1);
    BOOST_CHECK_EQUAL(actionProbs[2].action, 0);

    // deviation expressed in %
    BOOST_CHECK_CLOSE(actionProbs[0].probability, 0.621, 0.5);
    BOOST_CHECK_CLOSE(actionProbs[1].probability, 0.228, 0.5);
    BOOST_CHECK_CLOSE(actionProbs[2].probability, 0.45/3, 0.5);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2018-2020 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_SEGMENT_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_SEGMENT_BOX_HPP

#include <type_traits>

#include <boost/geometry/algorithms/detail/distance/segment_to_box.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/spherical/disjoint_box_box.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/spherical/point_in_point.hpp>
#include <boost/geometry/strategies/cartesian/point_in_box.hpp> // spherical
#include <boost/geometry/strategies/spherical/ssf.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

struct generic_segment_box
{
    template
    <
            typename LessEqual,
            typename ReturnType,
            typename SegmentPoint,
            typename BoxPoint,
            typename SegmentBoxStrategy,
            typename AzimuthStrategy,
            typename EnvelopeSegmentStrategy,
            typename NormalizePointStrategy,
            typename DisjointPointBoxStrategy,
            typename DisjointBoxBoxStrategy
    >
    static inline ReturnType segment_below_of_box(
            SegmentPoint const& p0,
            SegmentPoint const& p1,
            BoxPoint const&,
            BoxPoint const& top_right,
            BoxPoint const& bottom_left,
            BoxPoint const& bottom_right,
            SegmentBoxStrategy const& sb_strategy,
            AzimuthStrategy const& az_strategy,
            EnvelopeSegmentStrategy const& es_strategy,
            NormalizePointStrategy const& np_strategy,
            DisjointPointBoxStrategy const& dpb_strategy,
            DisjointBoxBoxStrategy const& dbb_strategy)
    {
        ReturnType result;
        typename LessEqual::other less_equal;
        typedef geometry::model::segment<SegmentPoint> segment_type;
        // if cs_tag is spherical_tag check segment's cs_tag with spherical_equatorial_tag as default
        typedef std::conditional_t
            <
                std::is_same<typename SegmentBoxStrategy::cs_tag, spherical_tag>::value,
                std::conditional_t
                    <
                        std::is_same
                            <
                                typename geometry::cs_tag<segment_type>::type,
                                spherical_polar_tag
                            >::value,
                        spherical_polar_tag, spherical_equatorial_tag
                    >,
                typename SegmentBoxStrategy::cs_tag
            > cs_tag;
        typedef geometry::detail::disjoint::
                disjoint_segment_box_sphere_or_spheroid<cs_tag>
                disjoint_sb;
        typedef typename disjoint_sb::disjoint_info disjoint_info_type;

        segment_type seg(p0, p1);

        geometry::model::box<BoxPoint> input_box;
        geometry::set_from_radian<geometry::min_corner, 0>
                (input_box, geometry::get_as_radian<0>(bottom_left));
        geometry::set_from_radian<geometry::min_corner, 1>
                (input_box, geometry::get_as_radian<1>(bottom_left));
        geometry::set_from_radian<geometry::max_corner, 0>
                (input_box, geometry::get_as_radian<0>(top_right));
        geometry::set_from_radian<geometry::max_corner, 1>
                (input_box, geometry::get_as_radian<1>(top_right));

        SegmentPoint p_max;

        disjoint_info_type disjoint_result = disjoint_sb::
                apply(seg, input_box, p_max,
                      az_strategy, np_strategy, dpb_strategy, dbb_strategy);

        if (disjoint_result == disjoint_info_type::intersect) //intersect
        {
            return 0;
        }
        // disjoint but vertex not computed
        if (disjoint_result == disjoint_info_type::disjoint_no_vertex)
        {
            typedef typename coordinate_type<SegmentPoint>::type CT;

            geometry::model::box<SegmentPoint> mbr;
            geometry::envelope(seg, mbr, es_strategy);

            CT lon1 = geometry::get_as_radian<0>(p0);
            CT lat1 = geometry::get_as_radian<1>(p0);
            CT lon2 = geometry::get_as_radian<0>(p1);
            CT lat2 = geometry::get_as_radian<1>(p1);

            if (lon1 > lon2)
            {
                std::swap(lon1, lon2);
                std::swap(lat1, lat2);
            }

            CT vertex_lat;
            CT lat_sum = lat1 + lat2;
            if (lat_sum > CT(0))
            {
                vertex_lat = geometry::get_as_radian<geometry::max_corner, 1>(mbr);
            } else {
                vertex_lat = geometry::get_as_radian<geometry::min_corner, 1>(mbr);
            }

            CT alp1;
            az_strategy.apply(lon1, lat1, lon2, lat2, alp1);
            CT vertex_lon = geometry::formula::vertex_longitude
                    <
                    CT,
                    cs_tag
                    >::apply(lon1, lat1, lon2, lat2,
                             vertex_lat, alp1, az_strategy);

            geometry::set_from_radian<0>(p_max, vertex_lon);
            geometry::set_from_radian<1>(p_max, vertex_lat);
        }
        //otherwise disjoint and vertex computed inside disjoint

        if (less_equal(geometry::get_as_radian<0>(bottom_left),
                       geometry::get_as_radian<0>(p_max)))
        {
            result = boost::numeric_cast<ReturnType>(typename
                        SegmentBoxStrategy::distance_ps_strategy::type().apply(bottom_left, p0, p1));
        }
        else
        {
            result = geometry::detail::distance::segment_to_box_2D
                        <
                            ReturnType,
                            SegmentPoint,
                            BoxPoint,
                            SegmentBoxStrategy
                        >::template call_above_of_box
                            <
                                typename LessEqual::other
                            >(p1, p0, p_max, bottom_right, sb_strategy);
        }
        return result;
    }

    template <typename SPoint, typename BPoint>
    static void mirror(SPoint& p0,
                       SPoint& p1,
                       BPoint& bottom_left,
                       BPoint& bottom_right,
                       BPoint& top_left,
                       BPoint& top_right)
    {
        //if segment's vertex is the southest point then mirror geometries
        if (geometry::get<1>(p0) + geometry::get<1>(p1) < 0)
        {
            BPoint bl = bottom_left;
            BPoint br = bottom_right;
            geometry::set<1>(p0, geometry::get<1>(p0) * -1);
            geometry::set<1>(p1, geometry::get<1>(p1) * -1);
            geometry::set<1>(bottom_left, geometry::get<1>(top_left) * -1);
            geometry::set<1>(top_left, geometry::get<1>(bl) * -1);
            geometry::set<1>(bottom_right, geometry::get<1>(top_right) * -1);
            geometry::set<1>(top_right, geometry::get<1>(br) * -1);
        }
    }
};

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

template
<
    typename CalculationType = void,
    typename Strategy = haversine<double, CalculationType>
>
struct spherical_segment_box
{
    template <typename PointOfSegment, typename PointOfBox>
    struct calculation_type
        : promote_floating_point
          <
              typename strategy::distance::services::return_type
                  <
                      Strategy,
                      PointOfSegment,
                      PointOfBox
                  >::type
          >
    {};

    typedef spherical_tag cs_tag;

    // strategy getters

    // point-point strategy getters
    struct distance_pp_strategy
    {
        typedef Strategy type;
    };

    inline typename distance_pp_strategy::type get_distance_pp_strategy() const
    {
        return typename distance_pp_strategy::type();
    }
    // point-segment strategy getters
    struct distance_ps_strategy
    {
        typedef cross_track<CalculationType, Strategy> type;
    };

    inline typename distance_ps_strategy::type get_distance_ps_strategy() const
    {
        return typename distance_ps_strategy::type();
    }

    struct distance_pb_strategy
    {
        typedef cross_track_point_box<CalculationType, Strategy> type;
    };

    inline typename distance_pb_strategy::type get_distance_pb_strategy() const
    {
        return typename distance_pb_strategy::type();
    }

    // TODO: why is the Radius not propagated above?

    typedef side::spherical_side_formula<CalculationType> side_strategy_type;

    static inline side_strategy_type get_side_strategy()
    {
        return side_strategy_type();
    }

    typedef within::spherical_point_point equals_point_point_strategy_type;

    static inline equals_point_point_strategy_type get_equals_point_point_strategy()
    {
        return equals_point_point_strategy_type();
    }

    // methods

    template <typename LessEqual, typename ReturnType,
              typename SegmentPoint, typename BoxPoint>
    inline ReturnType segment_below_of_box(SegmentPoint const& p0,
                                   SegmentPoint const& p1,
                                   BoxPoint const& top_left,
                                   BoxPoint const& top_right,
                                   BoxPoint const& bottom_left,
                                   BoxPoint const& bottom_right) const
    {
        typedef typename azimuth::spherical<CalculationType> azimuth_strategy_type;
        azimuth_strategy_type az_strategy;

        typedef typename envelope::spherical_segment<CalculationType>
                                             envelope_segment_strategy_type;
        envelope_segment_strategy_type es_strategy;

        return generic_segment_box::segment_below_of_box
               <
                    LessEqual,
                    ReturnType
               >(p0,p1,top_left,top_right,bottom_left,bottom_right,
                 spherical_segment_box<CalculationType>(),
                 az_strategy, es_strategy,
                 normalize::spherical_point(),
                 covered_by::spherical_point_box(),
                 disjoint::spherical_box_box());
    }

    template <typename SPoint, typename BPoint>
    static void mirror(SPoint& p0,
                       SPoint& p1,
                       BPoint& bottom_left,
                       BPoint& bottom_right,
                       BPoint& top_left,
                       BPoint& top_right)
    {

       generic_segment_box::mirror(p0, p1,
                                   bottom_left, bottom_right,
                                   top_left, top_right);
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<spherical_segment_box<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_segment_box type;
};

template <typename CalculationType, typename Strategy, typename PS, typename PB>
struct return_type<spherical_segment_box<CalculationType, Strategy>, PS, PB>
    : spherical_segment_box<CalculationType, Strategy>::template calculation_type<PS, PB>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<spherical_segment_box<CalculationType, Strategy> >
{
    // Define a cartesian_segment_box strategy with its underlying point-segment
    // strategy being comparable
    typedef spherical_segment_box
        <
            CalculationType,
            typename comparable_type<Strategy>::type
        > type;
};

template <typename CalculationType, typename Strategy>
struct get_comparable<spherical_segment_box<CalculationType, Strategy> >
{
    typedef typename comparable_type
        <
            spherical_segment_box<CalculationType, Strategy>
        >::type comparable_type;
public :
    static inline comparable_type apply(spherical_segment_box<CalculationType, Strategy> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Strategy, typename PS, typename PB>
struct result_from_distance<spherical_segment_box<CalculationType, Strategy>, PS, PB>
{
private :
    typedef typename return_type<
                                    spherical_segment_box
                                    <
                                        CalculationType,
                                        Strategy
                                    >,
                                    PS,
                                    PB
                                 >::type return_type;
public :
    template <typename T>
    static inline return_type apply(spherical_segment_box<CalculationType,
                                                          Strategy> const& ,
                                    T const& value)
    {
        Strategy s;
        return result_from_distance<Strategy, PS, PB>::apply(s, value);
    }
};

template <typename Segment, typename Box>
struct default_strategy
    <
        segment_tag, box_tag, Segment, Box,
        spherical_equatorial_tag, spherical_equatorial_tag
    >
{
    typedef spherical_segment_box<> type;
};

template <typename Box, typename Segment>
struct default_strategy
    <
        box_tag, segment_tag, Box, Segment,
        spherical_equatorial_tag, spherical_equatorial_tag
    >
{
    typedef typename default_strategy
        <
            segment_tag, box_tag, Segment, Box,
            spherical_equatorial_tag, spherical_equatorial_tag
        >::type type;
};

}
#endif

}} // namespace strategy::distance

}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_SEGMENT_BOX_HPP


#include <pcl/io/ply_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/common/centroid.h>

#include <Eigen/Dense>
#include <iostream>

#include <map>
#include <memory>

#include "utils/sorted_data_structures.hpp"

int main(int argc, char **argv)
{
  if (argc < 3) {
    std::cout << "usage: " << argv[0] << " data.ply output.ply [ k_normals [ k_region [ alph_max ] ] ]" << std::endl;
    return 1;
  }
  int k_n = 10;
  int k_r = 10;
  float alpha = 5 / 180.0f * (float) M_PI;
  try {
    if (argc >= 4) {
      k_n = std::stoi(argv[3]);
    }

    if (argc >= 5) {
      k_r = std::stoi(argv[4]);
    }

    if (argc >= 6) {
      alpha = std::stof(argv[5]) / 180.0f * (float) M_PI;
    }
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
  const std::string path = argv[1];
  const std::string outpath = argv[2];
  pcl::PLYReader reader;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  reader.read(path, *cloud);
  std::cout << "size: " << cloud->points.size() << std::endl;

  //normal estimation
  pcl::search::Search<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setSearchMethod (tree);
  normal_estimator.setInputCloud (cloud);
  normal_estimator.setKSearch (k_n);
  normal_estimator.compute (*normals);

  // region growing
  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
  // reg.setMinClusterSize (5);
  // reg.setMaxClusterSize (1000000);
  reg.setSearchMethod (tree);
  reg.setNumberOfNeighbours (k_r);
  reg.setInputCloud (cloud);
  reg.setInputNormals (normals);
  reg.setSmoothnessThreshold (alpha);
  // reg.setCurvatureThreshold (1.0);

  std::vector <pcl::PointIndices> clusters;
  reg.extract (clusters);

  std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;

  std::map<size_t, size_t> histogram;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
  colored->is_dense = true;
  for (size_t i=0; i<clusters.size(); i++) {
    const unsigned char r = rand() % 255;
    const unsigned char g = rand() % 255;
    const unsigned char b = rand() % 255;
    for (size_t p=0; p<clusters[i].indices.size(); p++) {
      const pcl::PointXYZ &point = (*cloud)[clusters[i].indices[p]];
      pcl::PointXYZRGB newpoint;
      newpoint.x = point.x;
      newpoint.y = point.y;
      newpoint.z = point.z;
      newpoint.r = r;
      newpoint.g = g;
      newpoint.b = b;
      colored->push_back(newpoint);
    }
    if (histogram.find(clusters[i].indices.size()) == histogram.end()) {
      histogram[clusters[i].indices.size()] = 1;
    } else {
      histogram[clusters[i].indices.size()] += 1;
    }
  }
  for (const auto &pair : histogram) {
    std::cout << "clusters with size " << pair.first << ": " << pair.second << std::endl;
  }
  pcl::PLYWriter writer;
  writer.write(outpath, *colored, true, false);

  //build adjacency graph
  std::cout << "building adjacency graph" << std::endl;

  //build inverse cluster map
  std::unique_ptr<size_t[]> cluster_map = std::make_unique<size_t[]>(cloud->size());
  for (size_t c=0; c<clusters.size(); c++) {
    for (size_t p=0; p<clusters[c].indices.size(); p++) {
      cluster_map[clusters[c].indices[p]] = c;
    }
  }

  //find nodes with neighbors in another cluster
  std::cout << "finding neighboring clusters" << std::endl;
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud(cloud);
  std::unique_ptr<size_t[]> cluster_adjacency_map = std::make_unique<size_t[]>(cloud->size());
  for (size_t p=0; p<cloud->size(); p++) {
    std::vector<int> ids;
    std::vector<float> dists;
    cluster_adjacency_map[p] = cluster_map[p];
    if (kdtree.nearestKSearch((*cloud)[p], k_r+1, ids, dists) > 0) { //TODO: Choose a better K
      for (int id : ids) {
	if (cluster_map[id] != cluster_map[p]) {
	  cluster_adjacency_map[p] = cluster_map[id];
	}
      }
    }
  }

  size_t count=0;
  for (size_t p=0; p<cloud->size(); p++) {
    if (cluster_adjacency_map[p] != cluster_map[p]) {
      count++;
    }
  }

  std::cout << "found " << count << " frontier vertices" << std::endl;

  //build graph
  std::cout << "building graph" << std::endl;
  std::vector<std::vector<size_t>> adjacency(clusters.size());
  for (size_t c=0; c<clusters.size(); c++) {
    for (size_t p=0; p<clusters[c].indices.size(); p++) {
      size_t c_adj = cluster_adjacency_map[clusters[c].indices[p]];
      if (c_adj != c) {
	insert_sorted_vector(adjacency[c], c_adj);
	insert_sorted_vector(adjacency[c_adj], c);
      }
    }
  }

  //get cluster summaries
  std::cout << "computing cluster statistics" << std::endl;
  pcl::PointCloud<pcl::PointXYZ>::Ptr centroids(new pcl::PointCloud<pcl::PointXYZ>);
  for (size_t c=0; c<clusters.size(); c++) {
    pcl::CentroidPoint<pcl::PointXYZ> centroid;
    for (size_t p=0; p<clusters[c].indices.size(); p++) {
      centroid.add((*cloud)[clusters[c].indices[p]]);
    }
    pcl::PointXYZ centroid_result;
    centroid.get(centroid_result);
    centroids->push_back(centroid_result);
  }

  //std::cout << "adding edges" << std::endl;
  //for (size_t c=0; c<clusters.size(); c++) {
  //
  //}

  return 0;
}