//  (C) Copyright John Maddock 2006-7.
//  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)

#include <boost/detail/workaround.hpp>
#if !BOOST_WORKAROUND(BOOST_BORLANDC, BOOST_TESTED_AT(0x582))

#include "test_rational.hpp"
#include <boost/math/concepts/real_concept.hpp>

template void do_test_spots<boost::math::concepts::real_concept, unsigned>(boost::math::concepts::real_concept, unsigned);

#endif


//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
///  A threadsafe interface to the STL random
///  number generators.  The generator to use can
///  be chosen in the ini/yaml file with option
///    rng: name
///  where name is one of the following:
///    default_random_engine
///      Default random engine
///    minstd_rand
///      Minimal Standard minstd_rand generator
///    minstd_rand0
///      Minimal Standard minstd_rand0 generator
///    mt19937
///      Mersenne Twister 19937 generator
///    mt19937_64
///      Mersene Twister 19937 generator (64 bit)
///    ranlux24_base
///      Ranlux 24 base generator
///    ranlux48_base
///      Ranlux 48 base generator
///    ranlux24
///      Ranlux 24 generator
///    ranlux48
///      Ranlux 48 generator
///    knuth_b
///      Knuth-B generator
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Pat Scott
///          (p.scott@imperial.ac.uk)
///  \date 2014 Dec
///
///  *********************************************

#include "gambit/Utils/threadsafe_rng.hpp"
#include "gambit/Utils/util_macros.hpp"
#include "gambit/Utils/standalone_error_handlers.hpp"
#include "gambit/Logs/logger.hpp"

#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/preprocessor/tuple/to_seq.hpp>

#define ALL_RNGS (default_random_engine, minstd_rand, minstd_rand0, mt19937, mt19937_64, ranlux24_base, ranlux48_base, ranlux24, ranlux48, knuth_b)
#define MAKE_SPECIALISED_RNG(r, data, elem)                              \
        else if (engine == STRINGIFY(elem))                              \
        {                                                                \
          static Utils::specialised_threadsafe_rng<elem> ultralocal_rng; \
          local_rng = &ultralocal_rng;                                   \
        }
#define ENABLE_ALL_RNGS BOOST_PP_SEQ_FOR_EACH(MAKE_SPECIALISED_RNG, , BOOST_PP_TUPLE_TO_SEQ(ALL_RNGS))

namespace Gambit
{

  /// Choose the engine to use for random number generation, based on the contents of the ini file.
  void Random::create_rng_engine(str engine)
  {
    using namespace std;
    if (engine == "default")
    {
      engine = "mt19937_64 (default)";
      static Utils::specialised_threadsafe_rng<mt19937_64> ultralocal_rng;
      local_rng = &ultralocal_rng;
    }
    ENABLE_ALL_RNGS
    else utils_error().raise(LOCAL_INFO, "Unknown random number generation engine: "+engine+".  Please check your yaml file.");
    logger() << LogTags::utils << "Random number engine " << engine << " selected." << EOM;
  }

  /// Draw a single uniform random deviate using the chosen RNG engine
  double Random::draw()
  {
    if (local_rng == NULL) create_rng_engine("default");
    return (*local_rng)();
  }

}


#include "EyeModelFitter.h"
#include "Fit/CircleOnSphereFit.h"
#include "CircleDeviationVariance3D.h"
#include "CircleEvaluation3D.h"
#include "CircleGoodness3D.h"

#include "utils.h"
#include "ImageProcessing/cvx.h"
#include "math/intersect.h"
#include "projection.h"
#include "fun.h"

#include "mathHelper.h"
#include "math/distance.h"
#include "common/constants.h"

#include <Eigen/StdVector>
#include <algorithm>
#include <queue>

namespace singleeyefitter {

EyeModelFitter::EyeModelFitter(double focalLength, Vector3 cameraCenter) :
    mFocalLength(std::move(focalLength)),
    mCameraCenter(std::move(cameraCenter)),
    mCurrentSphere(Sphere::Null), mCurrentInitialSphere(Sphere::Null),
    mNextModelID(1),
    mActiveModelPtr(new EyeModel(mNextModelID, -1, mFocalLength, mCameraCenter)),
    mLastTimeModelAdded( Clock::now() ),
    mApproximatedFramerate(30),
    mAverageFramerate(400), // windowsize is 400, let this be slow to changes to better compensate jumps
    mLastFrameTimestamp(0),
    mPupilState(7,3,0, CV_64F),
    mLogger( pupillabs::PyCppLogger("EyeModelFitter"))

{
    mNextModelID++;

    // our model for the kalman filter
    // x,y are phi and theta
    // size is the radius of the pupil

    // 1x + 0y + deltaTime*vx + 0vy + 0.5*deltaTime^2*ax + 0ay + 0size = x
    // 0x + 1y + 0vx + deltaTime*vy + 0ax + 0.5*deltaTime^2*ay + 0size = y
    // 0x + 0y + 1vx + 0vy + deltaTime*ax + 0ay + 0size= vx
    // 0x + 0y + 0vx + 1vy + 0ax + deltaTime*ay + 0size= vy
    // 0x + 0y + 0vx + 0vy + 1ax + 0ay + 0size= ax
    // 0x + 0y + 0vx + 0vy + 0ax + 1ay + 0size= ay
    // 0x + 0y + 0vx + 0vy + 0ax + 0ay + 1size  = size

    mPupilState.measurementMatrix = (cv::Mat_<double>(3, 7) <<  1, 0, 0, 0, 0, 0, 0,
                                                                0, 1, 0, 0, 0, 0, 0,
                                                                0, 0, 0, 0, 0, 0, 1);

    cv::setIdentity(mPupilState.processNoiseCov, cv::Scalar::all(1e-4));
    cv::setIdentity(mPupilState.errorCovPost, cv::Scalar::all(1));

    cv::setIdentity(mPupilState.measurementNoiseCov, cv::Scalar::all(1e-5));
    mPupilState.measurementNoiseCov.at<double>(2,2) =  0.1; // circle size has a different variance

    mPupilState.statePost.at<double>(6) = 2.0;  //initialise the size value with the average pupil radius

}

Detector3DResult EyeModelFitter::updateAndDetect(std::shared_ptr<Detector2DResult>& observation2D , const Detector3DProperties& props , bool debug)
{

    mDebug = debug;
    Detector3DResult result;
    result.confidence = observation2D->confidence; // if we don't fit we want to take the 2D confidence
    result.timestamp = observation2D->timestamp;

    float modelSensitivity = props.model_sensitivity;

    double deltaTime = observation2D->timestamp - mLastFrameTimestamp;
    if( mLastFrameTimestamp != 0.0 ){
        mApproximatedFramerate =  static_cast<int>(1.0 / (  deltaTime ));
        mAverageFramerate.addValue(mApproximatedFramerate);
    }
    mLastFrameTimestamp = observation2D->timestamp;

    int image_height = observation2D->image_height;
    int image_width = observation2D->image_width;
    int image_height_half = image_height / 2.0;
    int image_width_half = image_width / 2.0;

    Ellipse& ellipse = observation2D->ellipse;
    ellipse.center[0] -= image_width_half;
    ellipse.center[1] = image_height_half - ellipse.center[1];
    ellipse.angle = -ellipse.angle; //take y axis flip into account

    // Observation edge data are realtive to their ROI
    cv::Rect roi = observation2D->current_roi;

    //put the edges int or coordinate system
    // edges are needed for every optimisation step
    for (cv::Point& p : observation2D->final_edges) {
        p += roi.tl();
        p.x -= image_width_half;
        p.y = image_height_half - p.y;
    }

    auto observation3DPtr = std::make_shared<const Observation>(observation2D, mFocalLength);
    bool do3DSearch = false;
    // 2d observation good enough to show to models?
    if (observation2D->confidence >= 0.7) {

        // allow each model to decide by themself if the new observation supports the model or not
        auto circleAndFit = mActiveModelPtr->presentObservation(observation3DPtr, mAverageFramerate.getAverage()  );
        auto circle = circleAndFit.first;
        auto observationFit = circleAndFit.second;

        // overwrite confidence based on 3D observation
        double confidence2D = observation2D->confidence;
        result.confidence = confidence2D * (1.0 - observationFit.confidence) + observationFit.value * observationFit.confidence;
        // result.confidence = confidence2D;
        result.circle = circle;

        // only if the detected 2d pupil fits our model well we trust it to update the Kalman filter.
        if (circle != Circle::Null && observationFit.value > 0.99 ){
            predictPupilState( deltaTime );
            auto cc  = correctPupilState( circle );
            // if (mDebug) {
            //     result.predictedCircle = cc;
            // }
        }
        else {
            do3DSearch = true;
        }

        for (auto& modelPtr : mAlternativeModelsPtrs) {
             modelPtr->presentObservation(observation3DPtr, mAverageFramerate.getAverage() );
        }

    }
    else {
        do3DSearch = true;
    }

    if (do3DSearch  && mCurrentSphere != Sphere::Null) { // if it's too weak we try to find a better one in 3D

        // since the raw edges are used to find a better circle fit
        // they need to be converted in the out coordinate system
        for (cv::Point& p : observation2D->raw_edges) {
            p += roi.tl();
            p.x -= image_width_half;
            p.y = image_height_half - p.y;
        }

        // whenever we don't have a good 2D fit we use the model's state to predict the new pupil
        // and us this as as starting point for the search
        auto predictedCircle = predictPupilState( deltaTime );

        //fitCircle(observation2D->contours, props, result );
        //filterCircle(observation2D->raw_edges, props, result);
        // filterCircle2( predictedCircle , observation2D->raw_edges, props, result);
        filterCircle3( predictedCircle , observation2D->raw_edges, props, result);

        if (result.circle != Circle::Null){
           // mPupilState.measurementNoiseCov.at<double>(0,0) = 1e-4; // circle size has a different variance
           // mPupilState.measurementNoiseCov.at<double>(1,1) = 1e-4; // circle size has a different variance

            auto estimatedCircle = correctPupilState( result.circle );
           // mPupilState.measurementNoiseCov.at<double>(0,0) = 1e-5; // circle size has a different variance
           // mPupilState.measurementNoiseCov.at<double>(1,1) = 1e-5; // circle size has a different variance

            //result.circle = estimatedCircle;
            if (mDebug) {
               result.predictedCircle = estimatedCircle;
            }
        }

    }

    // error variance
    double positionError = getPupilPositionErrorVar();
    double sizeError = getPupilSizeErrorVar();

    //std::cout << "positionError: " << positionError << std::endl;
    //std::cout << "sizeError: " << sizeError << std::endl;

    // getSphere contains a mutex and blocks if optimisation is running
    // thus we just wanna call it once and save the results
    mCurrentSphere = mActiveModelPtr->getSphere();
    mCurrentInitialSphere = mActiveModelPtr->getInitialSphere();

    result.sphere = mCurrentSphere;
    // project the sphere back to 2D
    // needed to draw it in the eye window
    result.projectedSphere = project(mCurrentSphere, mFocalLength);
    // project the circle back to 2D
    // needed for some calculations in 2D later (calibration)
    if(result.circle != Circle::Null){
        result.ellipse  = Ellipse(project(result.circle,mFocalLength));
    }
    else{
        result.confidence = 0.0;
        result.ellipse = Ellipse::Null;
   }

    // contains the logic for building alternative models if the current one is bad
    checkModels(modelSensitivity,observation2D->timestamp );
    result.modelID = mActiveModelPtr->getModelID();
    result.modelBirthTimestamp = mActiveModelPtr->getBirthTimestamp();
    result.modelConfidence = mActiveModelPtr->getConfidence();

    if (mDebug) {
        result.models.reserve(mAlternativeModelsPtrs.size() + 1);

        ModelDebugProperties props;
        props.sphere = mActiveModelPtr->getSphere();
        props.initialSphere = mActiveModelPtr->getInitialSphere();
        props.binPositions = mActiveModelPtr->getBinPositions();
        props.maturity = mActiveModelPtr->getMaturity();
        props.solverFit = mActiveModelPtr->getSolverFit();
        props.confidence = mActiveModelPtr->getConfidence();
        props.performance = mActiveModelPtr->getPerformance();
        props.performanceGradient = mActiveModelPtr->getPerformanceGradient();
        props.modelID = mActiveModelPtr->getModelID();
        props.birthTimestamp = mActiveModelPtr->getBirthTimestamp();
        result.models.push_back(std::move(props));

        for (const auto& modelPtr : mAlternativeModelsPtrs) {

            ModelDebugProperties props;
            props.sphere = modelPtr->getSphere();
            props.initialSphere = modelPtr->getInitialSphere();
            props.binPositions = modelPtr->getBinPositions();
            props.maturity = modelPtr->getMaturity();
            props.solverFit = modelPtr->getSolverFit();
            props.confidence = modelPtr->getConfidence();
            props.performance = modelPtr->getPerformance();
            props.performanceGradient = modelPtr->getPerformanceGradient();
            props.modelID = modelPtr->getModelID();
            props.birthTimestamp = modelPtr->getBirthTimestamp();
            result.models.push_back(std::move(props));

        }
    }

    return result;

}

void EyeModelFitter::checkModels( float sensitivity,double frame_timestamp )
{

    using namespace std::chrono;

    static const int maxAltAmountModels  = 3;
    static const double minMaturity  = 0.15;
    const double minPerformance = sensitivity;
    static const seconds altModelExpirationTime(10);
    static const seconds minNewModelTime(3);
    static const double gradientChangeThreshold = -2.0e-05; // with this we are also sensitive to changes even if the performance is still above the threshold

    Clock::time_point  now( Clock::now() );

    /* whenever our current model's performance is below the threshold or the performance decreases rapidly (performance gradient)
       we try to create an alternative model
    */
    //std::cout << "current performance gradient: " << mActiveModelPtr->getPerformanceGradient() << std::endl;
    if(  mActiveModelPtr->getPerformance() < minPerformance || mActiveModelPtr->getPerformanceGradient() <= gradientChangeThreshold){

        mLastTimePerformancePenalty = now;
        auto lastTimeAdded =  duration_cast<seconds>(now - mLastTimeModelAdded);

        if( mAlternativeModelsPtrs.size() <= maxAltAmountModels &&
            mActiveModelPtr->getMaturity() > minMaturity &&
            lastTimeAdded  > minNewModelTime )
        {
            mAlternativeModelsPtrs.emplace_back(  new EyeModel(mNextModelID , frame_timestamp, mFocalLength, mCameraCenter ) );
            mNextModelID++;
            mLastTimeModelAdded = now;
        }

    }else if( mActiveModelPtr->getPerformance() > minPerformance /*&& mActiveModelPtr->getPerformanceGradient() >  0.0*/ ) {
        // kill other models whenever the performance is good enough AND the performance doesn't decrease
        mAlternativeModelsPtrs.clear();
        mLastTimeModelAdded = now - minNewModelTime - seconds(1); // so we can add a new model right away
    }

    if(mAlternativeModelsPtrs.size() == 0)
        return; // early exit

    // sort the model with increasing fit
    const auto sortFit = [](const EyeModelPtr&  a , const EyeModelPtr& b){
        return a->getSolverFit() < b->getSolverFit();
    };
    mAlternativeModelsPtrs.sort(sortFit);

    bool foundNew = false;
    // now get the first alternative model where the performance and maturity is higher then the current one
    for( auto& modelptr : mAlternativeModelsPtrs){

        if(modelptr->getMaturity() > minMaturity &&  mActiveModelPtr->getPerformance() < modelptr->getPerformance() ){
            mActiveModelPtr.reset( modelptr.release() );
            mAlternativeModelsPtrs.clear(); // we got a better one, let's remove others
            foundNew = true;
            break;
        }
    }

    auto lastPenalty =  duration_cast<seconds>(now - mLastTimePerformancePenalty);
    // if we didn't find a better one after repeatedly looking, remove all of them and start new
    if( !foundNew && lastPenalty > altModelExpirationTime ){

        mAlternativeModelsPtrs.clear();
        mActiveModelPtr.reset(  new EyeModel(mNextModelID , frame_timestamp, mFocalLength, mCameraCenter ));
        mNextModelID++;
    }

}

void EyeModelFitter::reset()
{
    mNextModelID = 1;
    mAlternativeModelsPtrs.clear();
    mActiveModelPtr = EyeModelPtr( new EyeModel(mNextModelID , -1, mFocalLength, mCameraCenter ));
    mLastTimeModelAdded =  Clock::now();
    mCurrentSphere = Sphere::Null;
    mCurrentInitialSphere = Sphere::Null;
    //mLogger.setLogLevel( pupillabs::PyCppLogger::LogLevel::DEBUG);
    //mLogger.info("Reset models");
    //mLogger.error("Reset models");

}

// void  EyeModelFitter::fitCircle(const Contours_2D& contours2D , const Detector3DProperties& props,  Detector3DResult& result) const
// {

//     if (contours2D.size() == 0)
//         return;

//     Contours3D contoursOnSphere  = unprojectContours( contours2D );

//     double minRadius = props.pupil_radius_min;
//     double maxRadius =  props.pupil_radius_max;

//     if (mPreviousPupil.radius != 0.0) {

//         minRadius = std::max(mPreviousPupil.radius * 0.85, minRadius );
//         maxRadius = std::min(mPreviousPupil.radius * 1.25, maxRadius );
//     }
//     const double maxDiameter = maxRadius * 2.0;

//     //final_candidate_contours.clear(); // otherwise we fill this infinitly

//     //first we want to filter out the bad stuff, too short ones
//     const auto contour_size_min_pred = [](const std::vector<Vector3>& contour) {
//         return contour.size() >= 3;
//     };
//     contoursOnSphere = singleeyefitter::fun::filter(contour_size_min_pred , contoursOnSphere);

//     if (contoursOnSphere.size() == 0)
//         return ;

//     // sort the contours so the contour with the most points is at the begining
//     std::sort(contoursOnSphere.begin(), contoursOnSphere.end(), [](const std::vector<Vector3>& a, const std::vector<Vector3>& b) { return a.size() < b.size();});

//     // saves the best solution and just the Vector3Ds not every single contour
//     Contours3D bestSolution;
//     Circle bestCircle;
//     double bestVariance = std::numeric_limits<double>::infinity();
//     double bestGoodness = 0.0;
//     double bestResidual = 0.0;

//     auto circleFitter = CircleOnSphereFitter<double>(mCurrentSphere);
//     auto circleEvaluation = CircleEvaluation3D<double>(mCameraCenter, mCurrentSphere, props.max_fit_residual, minRadius, maxRadius);
//     auto circleVariance = CircleDeviationVariance3D<double>();
//     auto circleGoodness = CircleGoodness3D<double>();

//     auto pruning_quick_combine = [&](const Contours3D & contours,  int max_evals = 1e20, int max_depth = 5) {
//         // describes different combinations of contours
//         typedef std::set<int> Path;
//         // combinations we wanna test
//         std::queue<Path> unvisited;

//         // contains all the indices for the contours, which altogther fit best
//         std::vector<Path> results;

//         // contains bad paths, we won't test again
//         // even a superset is not tested again, because if a subset is bad, we can't make it better if more contours are added
//         std::vector<Path> prune;
//         prune.reserve(std::pow(contours.size() , 3));   // we gonna prune a lot if we have alot contours
//         int eval_count = 0;
//         //std::cout << "size:" <<  contours.size()  << std::endl;
//         //std::cout << "possible combinations: " <<  std::pow(2,contours.size()) + 1<< std::endl;

//         // contains the first moment of each contour
//         // we precalculate this inorder to prune contours combinations if the distance of these are to long
//         std::vector<Vector3> moments;
//         moments.reserve(contours.size());

//         // enqueue all contours as starting point
//         // and calculate moment
//         for (int i = 0; i < contours.size(); i++) {
//             unvisited.emplace(std::initializer_list<int> {i});

//             Vector3 m = std::accumulate(contours[i].begin(), contours[i].end(), Vector3(0, 0, 0), std::plus<Vector3>());
//             m /= contours[i].size();
//             moments.push_back(m);
//         }

//         // inorder to minimize the search space we already prune combinations, which can't fit ,before the search starts
//         int prune_count = 0;

//         for (int i = 0; i < contours.size(); i++) {
//             auto& a = moments[i];

//             for (int j = i + 1; j < contours.size(); j++) {
//                 auto& b = moments[j];
//                 double distance_squared  = (a - b).squaredNorm();

//                 if (distance_squared >  std::pow(maxDiameter * 1.5, 2.0)) {
//                     prune.emplace_back(std::initializer_list<int> {i, j});
//                     prune_count++;
//                 }
//             }
//         }

//         // std::cout << "pruned " << prune_count << std::endl;

//         while (!unvisited.empty() && eval_count <= max_evals) {
//             eval_count++;
//             //take a path and combine it with others to see if the fit gets better
//             Path current_path = unvisited.front();
//             unvisited.pop();

//             if (current_path.size() <= max_depth) {
//                 bool includes_bad_paths = fun::isSubset(current_path, prune);

//                 if (!includes_bad_paths) {
//                     int size = 0;

//                     for (int j : current_path) { size += contours.at(j).size(); };

//                     Contour3D test_contour;

//                     Contours3D test_contours;

//                     test_contour.reserve(size);

//                     std::set<int> test_contour_indices;

//                     //concatenate contours to one contour
//                     for (int k : current_path) {
//                         const Contour3D& c = contours.at(k);
//                         test_contours.push_back(c);
//                         test_contour.insert(test_contour.end(), c.begin(), c.end());
//                         test_contour_indices.insert(k);
//                     }

//                     //we have not tested this and a subset of this was sucessfull before

//                     // need at least 3 points
//                     if (!circleFitter.fit(test_contour)) {
//                         std::cout << "Error! Too little points!" << std::endl; // filter too short ones before
//                     }

//                     // we got a circle fit
//                     Circle current_circle = circleFitter.getCircle();
//                     // see if it's even a candidate
//                     double variance =  circleVariance(current_circle , test_contours);

//                     if (variance <  props.max_circle_variance) {
//                         //yes this was good, keep as solution
//                         //results.push_back(test_contour_indices);

//                         //lets explore more by creating paths to each remaining node
//                         for (int l = (*current_path.rbegin()) + 1 ; l < contours.size(); l++) {
//                             // if a new contour is to far away from the current circle center, we can also ignore it
//                             // Vector3 contour_moment = moments.at(l);
//                             // double distance_squared = (current_circle.center - contour_moment).squaredNorm();
//                             // if( distance_squared <   std::pow(pupil_max_radius * 1.5, 2.0) ){
//                             //     unvisited.push(current_path);
//                             //     unvisited.back().insert(l); // add a new path
//                             // }
//                             unvisited.push(current_path);
//                             unvisited.back().insert(l); // add a new path
//                         }

//                         double residual = circleFitter.calculateResidual(test_contour);
//                         bool isCandidate = circleEvaluation(current_circle, residual);
//                         double goodness =  circleGoodness(current_circle , test_contours);

//                         // if (isCandidate)
//                         //     final_candidate_contours.push_back(test_contours);

//                         //check if this one is better then the best one and swap
//                         if (isCandidate &&  goodness > bestGoodness) {

//                             bestResidual = residual;
//                             bestVariance = variance;
//                             bestGoodness = goodness;
//                             bestCircle = current_circle;
//                             bestSolution = test_contours;
//                         }

//                     } else {
//                         prune.push_back(current_path);
//                     }
//                 }
//             }
//         }

//         //std::cout << "tried: "  << eval_count  << std::endl;
//         //return results;
//     };

//     pruning_quick_combine(contoursOnSphere, props.combine_evaluation_max, props.combine_depth_max);

//     //std::cout << "residual: " <<  bestResidual << std::endl;
//     //std::cout << "goodness: " <<  bestGoodness << std::endl;
//     //std::cout << "variance: " <<  bestVariance << std::endl;
//     result.circle = std::move(bestCircle);
//     result.fittedCircleContours = std::move(bestSolution); // save this for debuging
//     result.fitGoodness = bestGoodness;
//     result.contours = std::move( contoursOnSphere );
//     // project the circle back to 2D
//     // need for some calculations in 2D later (calibration)
//     result.ellipse = Ellipse(project(bestCircle, mFocalLength));

// }

// Contours3D EyeModelFitter::unprojectContours(const Contours_2D& contours) const
// {
//     Contours3D contoursOnSphere;
//     contoursOnSphere.resize(contours.size());
//     int i = 0;
//     //TODO handle contours with no intersection points, because they get closed
//     for (auto& contour : contours) {
//         for (auto& point : contour) {
//             Vector3 point3D(point.x, point.y , mFocalLength);
//             Vector3 direction = point3D - mCameraCenter;

//             try {
//                 // we use the eye properties of the current eye, when ever we call this
//                 const auto& unprojectedPoint = intersect(Line3(mCameraCenter,  direction.normalized()), mCurrentSphere);
//                 contoursOnSphere[i].push_back(std::move(unprojectedPoint.first));

//             } catch (no_intersection_exception&) {
//                 // if there is no intersection we don't do anything
//             }
//         }
//         i++;
//     }
//     return contoursOnSphere;

// }

Edges3D EyeModelFitter::unprojectEdges(const Edges2D& edges) const
{
    Edges3D edgesOnSphere;
    edgesOnSphere.reserve(edges.size());

    for (auto& edge : edges) {
        Vector3 point3D(edge.x, edge.y , mFocalLength);
        Vector3 direction = point3D - mCameraCenter;

        std::pair<Vector3, Vector3> unprojectedPoints;
        // we use the eye properties of the current eye, when ever we call this
        bool didIntersect = intersect(Line3(mCameraCenter,  direction.normalized()), mCurrentSphere, unprojectedPoints);

        if( didIntersect)
            edgesOnSphere.push_back(std::move(unprojectedPoints.first));

    }

    return edgesOnSphere;

}

// void  EyeModelFitter::filterCircle(const Edges2D& rawEdges , const Detector3DProperties& props,  Detector3DResult& result) const
// {

//     if (rawEdges.size() == 0 || mPreviousPupil == Circle::Null)
//         return;

//     Edges3D edgesOnSphere = unprojectEdges(rawEdges);

//     //Inorder to filter the edges depending on the distance of the previous pupil center
//     // imagine a sphere with center equal to the previous pupil center (pupilcenters are always on the sphere )
//     // and sphere radius equal the distance from sphere center to pupil border
//     double h =  mCurrentSphere.radius - std::sqrt(mCurrentSphere.radius * mCurrentSphere.radius - mPreviousPupil.radius * mPreviousPupil.radius);
//     double pupilSphereRadiusSquared =  2.0 * mCurrentSphere.radius  * h;
//     double pupilSphereRadius = std::sqrt(pupilSphereRadiusSquared);
//     Vector3 pupilSphereCenter = mPreviousPupil.center;

//     const double delta = std::pow(1.5, 2);
//     const double maxFilterDistanceSquared = pupilSphereRadiusSquared * delta;
//     auto regionFilter = [&](const Vector3 & point) {
//         double distanceSquared = (point - pupilSphereCenter).squaredNorm();
//         return  distanceSquared < maxFilterDistanceSquared;
//     };

//     auto filteredEdges = fun::filter(regionFilter, edgesOnSphere);

//     // now we got all edges in the surrounding of the previous pupil
//     // let find the circle where most edges support the circle including a certain region around the circle border

//     const double maxAngularVelocity = 0.1;  //TODO this should depend on the realy velocity calculated per frame

//     Vector3 c  = mPreviousPupil.center - mCurrentSphere.center;
//     // search space is in spehrical coordinates
//     Vector2 previousPupilCenter  = math::cart2sph(c);
//     const double maxTheta = previousPupilCenter.x() + maxAngularVelocity ;
//     const double maxPsi = previousPupilCenter.y() + maxAngularVelocity ;
//     const double minTheta = previousPupilCenter.x() - maxAngularVelocity;
//     const double minPsi = previousPupilCenter.y() - maxAngularVelocity;

//     const double stepSizeAngle = 0.01; // in radian

//     // defined in pixel space and recalculated for 3D space further down
//     const int bandWidthPixel =  4 ;

//     int maxEdgeCount = 0;
//     Vector3 bestCircleCenter(0, 0, 0);
//     double bestDistanceVariance = std::numeric_limits<double>::max();
//     Edges3D inliers;
//     Edges3D finalInliers;

//     for (double i = minTheta; i <= maxTheta; i += stepSizeAngle) {
//         for (double j = minPsi; j <=  maxPsi; j += stepSizeAngle) {

//             // from here in cartesian again
//             // if we use cartesian we can just compare the distances from the pupil sphere center
//             // all this happens in world coordinates
//             const Vector3 newPupilCenter  = mCurrentSphere.center + math::sph2cart(mCurrentSphere.radius, i, j);

//             const double bandWidth =  bandWidthPixel * newPupilCenter.z() / mFocalLength ;
//             const double bandWidthHalf = bandWidth / 2.0 ;
//             const double maxDistanceSquared  = std::pow(pupilSphereRadius + bandWidthHalf, 2) ;
//             const double minDistanceSquared  = std::pow(pupilSphereRadius - bandWidthHalf, 2) ;

//             if(mDebug)
//                 inliers.clear();

//             int  edgeCount = 0;
//             //count all edges which fall into this current circle
//             for (const auto& e : filteredEdges) {

//                 double distanceSquared = (e - newPupilCenter).squaredNorm();
//                 if (distanceSquared < maxDistanceSquared && distanceSquared > minDistanceSquared) {
//                     edgeCount++;
//                     if(mDebug)
//                         inliers.push_back(e);
//                 }
//             }

//             if (edgeCount > maxEdgeCount  ) {
//                 bestCircleCenter = newPupilCenter;
//                 maxEdgeCount = edgeCount;
//                 if(mDebug)
//                     finalInliers = std::move(inliers);
//             }

//         }
//     }

//     if (maxEdgeCount != 0) {
//         result.circle.center = bestCircleCenter;
//         result.circle.normal = (bestCircleCenter - mCurrentSphere.center).normalized() ;
//         result.circle.radius = mPreviousPupil.radius;

//         // project the circle back to 2D
//         // needed for some calculations in 2D later (calibration)
//         result.ellipse  = Ellipse(project(result.circle,mFocalLength));

//         double circumference = result.ellipse.circumference();
//         result.confidence =  std::min(maxEdgeCount / circumference, 1.0 ) ;
//     }

//     if( mDebug )
//       result.edges = std::move(finalInliers);  // visualize

// }

void  EyeModelFitter::filterCircle2( const Circle& predictedCircle, const Edges2D& rawEdges , const Detector3DProperties& props,  Detector3DResult& result) const
{

    if (rawEdges.size() == 0 )
        return;

    Edges3D edgesOnSphere = unprojectEdges(rawEdges);

    //Inorder to filter the edges depending on the distance of the predicted pupil center
    // imagine a sphere with center equal to the predicted pupil center (pupilcenters are always on the sphere )
    // and sphere radius equal the distance of the predicted pupil radius
    double h =  mCurrentSphere.radius - std::sqrt(mCurrentSphere.radius * mCurrentSphere.radius - predictedCircle.radius * predictedCircle.radius);
    double pupilSphereRadiusSquared =  2.0 * mCurrentSphere.radius  * h;
    double pupilSphereRadius = std::sqrt(pupilSphereRadiusSquared);
    Vector3 pupilSphereCenter = predictedCircle.center;

    const double delta = std::pow(1.5, 2);
    const double maxFilterDistanceSquared = pupilSphereRadiusSquared * delta;
    auto regionFilter = [&](const Vector3 & point) {
        double distanceSquared = (point - pupilSphereCenter).squaredNorm();
        return  distanceSquared < maxFilterDistanceSquared;
    };

    auto filteredEdges = fun::filter(regionFilter, edgesOnSphere);

    // now we got all edges in the surrounding of the predicted pupil
    // let's find the circle where most edges support the circle including a certain region around the circle border

    const double maxAngularVelocity = 0.06;  //TODO this should depend on the realy velocity calculated per frame

    Vector3 c  = predictedCircle.center - mCurrentSphere.center;
    // search space is in spehrical coordinates
    Vector2 predictedPupilCenter  = math::cart2sph(c);
    const double maxTheta = predictedPupilCenter.x() + maxAngularVelocity ;
    const double maxPsi = predictedPupilCenter.y() + maxAngularVelocity ;
    const double minTheta = predictedPupilCenter.x() - maxAngularVelocity;
    const double minPsi = predictedPupilCenter.y() - maxAngularVelocity;

    const double stepSizeAngle = 0.005; // in radian

    // defined in pixel space and recalculated for 3D space further down
    const int bandWidthPixel =  4 ;

    int maxEdgeCount = 0;
    Vector3 bestCircleCenter(0, 0, 0);
    double bestCircleRadius = 0.0;
    double bestDistanceVariance = std::numeric_limits<double>::max();
    Edges3D inliers;
    Edges3D finalInliers;

    for (double i = minTheta; i <= maxTheta; i += stepSizeAngle) {
        for (double j = minPsi; j <=  maxPsi; j += stepSizeAngle) {

            // from here in cartesian again
            // if we use cartesian we can just compare the distances from the pupil sphere center
            // all this happens in world coordinates
            const Vector3 newPupilCenter  = mCurrentSphere.center + math::sph2cart(mCurrentSphere.radius, i, j);

            const double bandWidth =  bandWidthPixel * newPupilCenter.z() / mFocalLength ;
            const double bandWidthHalf = bandWidth / 2.0 ;
            const double maxDistanceSquared  = std::pow(pupilSphereRadius + bandWidthHalf, 2) ;
            const double minDistanceSquared  = std::pow(pupilSphereRadius - bandWidthHalf, 2) ;

            if(mDebug)
                inliers.clear();

            int  edgeCount = 0;
            double accRadius = 0.0;
            //count all edges which fall into this current circle
            for (const auto& e : filteredEdges) {

                double distanceSquared = (e - newPupilCenter).squaredNorm();
                if (distanceSquared < maxDistanceSquared && distanceSquared > minDistanceSquared) {
                    edgeCount++;
                    accRadius += std::sqrt(distanceSquared);
                    if(mDebug)
                        inliers.push_back(e);
                }
            }

            if (edgeCount > maxEdgeCount  ) {
                bestCircleCenter = newPupilCenter;
                bestCircleRadius = accRadius / edgeCount;
                maxEdgeCount = edgeCount;
                if(mDebug)
                    finalInliers = std::move(inliers);
            }

        }
    }

    if (maxEdgeCount != 0) {
        result.circle.center = bestCircleCenter;
        result.circle.normal = (bestCircleCenter - mCurrentSphere.center).normalized() ;
        result.circle.radius = predictedCircle.radius;
        result.circle.radius = bestCircleRadius;

        // project the circle back to 2D
        // needed for some calculations in 2D later (calibration)
        result.ellipse  = Ellipse(project(result.circle,mFocalLength));

        double circumference = result.ellipse.circumference();
        result.confidence =  std::min(maxEdgeCount / circumference, 1.0 ) ;
    }

    if( mDebug )
      result.edges = std::move(finalInliers);  // visualize
}

void  EyeModelFitter::filterCircle3( const Circle& predictedCircle, const Edges2D& rawEdges , const Detector3DProperties& props,  Detector3DResult& result) const
{

    if (rawEdges.size() == 0 )
        return;

    Edges3D edgesOnSphere = unprojectEdges(rawEdges);
    Edges3D filteredEdges;
    Edges3D finalInliers;
    int maxEdgeCount;
    auto searchCenter  = [this, &finalInliers, &edgesOnSphere, &maxEdgeCount, &predictedCircle, &filteredEdges ]( Vector3 searchCenter,   double positionVariance = 0.06, double searchStep = 0.005, int bandWidthPixel = 4 ) -> Circle {

        //Inorder to filter the edges depending on the distance of the predicted pupil center
        // imagine a sphere with center equal to the predicted pupil center (pupilcenters are always on the sphere )
        // and sphere radius equal the distance of the predicted pupil radius
        double h =  mCurrentSphere.radius - std::sqrt(mCurrentSphere.radius * mCurrentSphere.radius - predictedCircle.radius * predictedCircle.radius);
        double pupilSphereRadiusSquared =  2.0 * mCurrentSphere.radius  * h;
        double pupilSphereRadius = std::sqrt(pupilSphereRadiusSquared);
        Vector3 pupilSphereCenter = predictedCircle.center;

        // delta depends on the postionvariance so the window has the right size
        // position variance is in radians
        const double delta = std::sin(positionVariance) * mCurrentSphere.radius;
        const double deltaSquared = std::pow(delta, 2);
        const double maxFilterDistanceSquared = pupilSphereRadiusSquared + deltaSquared;
        auto regionFilter = [&](const Vector3 & point) {
            double distanceSquared = (point - pupilSphereCenter).squaredNorm();
            return  distanceSquared < maxFilterDistanceSquared;
        };

        filteredEdges = fun::filter(regionFilter, edgesOnSphere);

        // now we got all edges in the surrounding of the predicted pupil
        // let's find the circle where most edges support the circle including a certain region around the circle border

        const double maxAngularVelocity = positionVariance;  //TODO this should depend on the realy velocity calculated per frame

        Vector3 c  = searchCenter - mCurrentSphere.center;
        // search space is in spehrical coordinates
        Vector2 predictedPupilCenter  = math::cart2sph(c);
        const double maxTheta = predictedPupilCenter.x() + maxAngularVelocity ;
        const double maxPsi = predictedPupilCenter.y() + maxAngularVelocity ;
        const double minTheta = predictedPupilCenter.x() - maxAngularVelocity;
        const double minPsi = predictedPupilCenter.y() - maxAngularVelocity;

        const double stepSizeAngle = searchStep; // in radian

        // defined in pixel space and recalculated for 3D space further down
        //const int bandWidthPixel =  4 ;

        maxEdgeCount = 0;
        Vector3 bestCircleCenter(0, 0, 0);
        double bestCircleRadius = 0.0;
        double bestDistanceVariance = std::numeric_limits<double>::max();
        Edges3D inliers;

        for (double i = minTheta; i <= maxTheta; i += stepSizeAngle) {
            for (double j = minPsi; j <=  maxPsi; j += stepSizeAngle) {

                // from here in cartesian again
                // if we use cartesian we can just compare the distances from the pupil sphere center
                // all this happens in world coordinates
                const Vector3 newPupilCenter  = mCurrentSphere.center + math::sph2cart(mCurrentSphere.radius, i, j);

                const double bandWidth =  bandWidthPixel * newPupilCenter.z() / mFocalLength ;
                const double bandWidthHalf = bandWidth / 2.0 ;
                const double maxDistanceSquared  = std::pow(pupilSphereRadius + bandWidthHalf, 2) ;
                const double minDistanceSquared  = std::pow(pupilSphereRadius - bandWidthHalf, 2) ;

                if(mDebug)
                    inliers.clear();

                int  edgeCount = 0;
                double accRadius = 0.0;
                //count all edges which fall into this current circle
                for (const auto& e : filteredEdges) {

                    double distanceSquared = (e - newPupilCenter).squaredNorm();
                    if (distanceSquared < maxDistanceSquared && distanceSquared > minDistanceSquared) {
                        edgeCount++;
                        accRadius += std::sqrt(distanceSquared);
                        if(mDebug)
                            inliers.push_back(e);
                    }
                }

                if (edgeCount > maxEdgeCount  ) {
                    bestCircleCenter = newPupilCenter;
                    bestCircleRadius = accRadius / edgeCount;
                    maxEdgeCount = edgeCount;
                    if(mDebug)
                        finalInliers = std::move(inliers);
                }

            }
        }
        if (maxEdgeCount != 0){
            Circle circle;
            circle.center = bestCircleCenter;
            circle.normal = (bestCircleCenter - mCurrentSphere.center).normalized() ;
            circle.radius = bestCircleRadius;
            return circle;
        }else{
            return Circle::Null;
        }

    };

    double positionVariance = 0.2;
    double searchStep = 0.05;
    int bandWidthPixel = 6;

    auto circle = searchCenter(predictedCircle.center, positionVariance, searchStep, bandWidthPixel);

    positionVariance = 0.05;
    searchStep = 0.01;
    bandWidthPixel = 2;

    circle  = searchCenter(circle.center, positionVariance, searchStep, bandWidthPixel);

    if (circle != Circle::Null ) {

        result.circle = circle;
        // project the circle back to 2D
        // needed for some calculations in 2D later (calibration)
        result.ellipse  = Ellipse(project(result.circle,mFocalLength));

        double circumference = result.ellipse.circumference();
        result.confidence =  std::min(maxEdgeCount / circumference, 1.0 ) ;
    }

    if( mDebug )
      result.edges = std::move(filteredEdges);  // visualize
}

Circle EyeModelFitter::predictPupilState( double deltaTime ){

    // correlates position and velocity
    // x,y are phi and theta
    // 1x + 0y + deltaTime*vx + 0vy + 0.5*deltaTime^2*ax + 0ay  = x
    // 0x + 1y + 0vx + deltaTime*vy + 0ax + 0.5*deltaTime^2*ay  = y
    // 0x + 0y + 1vx + 0vy + deltaTime*ax + 0ay = vx
    // 0x + 0y + 0vx + 1vy + 0ax + deltaTime*ay = vy
    // 0x + 0y + 0vx + 0vy + 1ax + 0ay = ax
    // 0x + 0y + 0vx + 0vy + 0ax + 1ay = ay
   mPupilState.transitionMatrix = (cv::Mat_<double>(7, 7) << 1, 0, deltaTime, 0, 0.5*deltaTime*deltaTime , 0, 0,
                                                          0, 1, 0, deltaTime, 0 , 0.5*deltaTime*deltaTime, 0,
                                                          0, 0, 1, 0, deltaTime, 0,0,
                                                          0, 0, 0, 1, 0, deltaTime,0,
                                                          0, 0, 0, 0, 1, 0,0,
                                                          0, 0, 0, 0, 0, 1,0,
                                                          0, 0, 0, 0, 0, 0,1);

    cv::Mat pupilStatePrediction = mPupilState.predict();
    double theta = pupilStatePrediction.at<double>(0);
    double psi = pupilStatePrediction.at<double>(1);
    double radius = pupilStatePrediction.at<double>(6);
    return circleOnSphere( mCurrentSphere, theta, psi, radius );

}

Circle EyeModelFitter::correctPupilState( const Circle& circle){

    Vector2 params = paramsOnSphere(mCurrentSphere, circle);
    cv::Mat meausurement = (cv::Mat_<double>(3,1) << params[0], params[1], circle.radius );
    auto estimated = mPupilState.correct( meausurement );

    double theta = estimated.at<double>(0);
    double psi = estimated.at<double>(1);
    double radius = estimated.at<double>(6);
    auto estimatedCircle = circleOnSphere( mCurrentSphere , theta, psi, radius );
    return estimatedCircle;
}

double EyeModelFitter::getPupilPositionErrorVar() const {

    // error variance
    double thetaError = mPupilState.errorCovPost.at<double>(0,0);
    double psiError = mPupilState.errorCovPost.at<double>(1,1);
   // std::cout << "te: " << thetaError << std::endl;
   // std::cout << "pE: " << psiError << std::endl;
    // for now let's just use the average from both values
    return (thetaError + psiError) * 0.5;

}

double EyeModelFitter::getPupilSizeErrorVar() const {

    // error variance
    double sizeError = mPupilState.errorCovPost.at<double>(6,6);
    return sizeError;

}

} // singleeyefitter


/**
 * MIT License
 *
 * Copyright (c) 2020 IWT Wirtschaft und Technik GmbH
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * @file alligne_model.cpp
 * @author Vishnuprasad Prachandabhanu (vishnu.pbhat93@gmail.com)
 */

#include <iostream>
#include <thread>
#include <pthread.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Matrix3x3.h>

#include <pcl_ros/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/io/pcd_io.h>
#include <boost/foreach.hpp>

#include <pcl/io/vtk_io.h>
#include <vtkPolyDataMapper.h>
#include <pcl/apps/render_views_tesselated_sphere.h>

double size_x, size_y, size_z, px, py, pz, roll, pitch, yaw;
static volatile bool save_cloud = false;

static void* user_thread(void*)
{
    while (!save_cloud && ros::ok())
    {
        if(std::cin.get() == 'q')
        {
            save_cloud = true;
        }

    }
}

double
computeCloudResolution(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud)
{
	double resolution = 0.0;
	int numberOfPoints = 0;
	int nres;
	std::vector<int> indices(2);
	std::vector<float> squaredDistances(2);
	pcl::search::KdTree<pcl::PointXYZ> tree;
	tree.setInputCloud(cloud);

	for (size_t i = 0; i < cloud->size(); ++i)
	{
		if (! pcl_isfinite((*cloud)[i].x))
			continue;

		// Considering the second neighbor since the first is the point itself.
		nres = tree.nearestKSearch(i, 2, indices, squaredDistances);
		if (nres == 2)
		{
			resolution += sqrt(squaredDistances[1]);
			++numberOfPoints;
		}
	}
	if (numberOfPoints != 0)
		resolution /= numberOfPoints;

	return resolution;
}

void updateParm(ros::NodeHandle nh)
{
    nh.getParam("size_x", size_x);
    nh.getParam("size_y", size_y);
    nh.getParam("size_z", size_z);
    nh.getParam("px", px);
    nh.getParam("py", py);
    nh.getParam("pz", pz);
    nh.getParam("roll", roll);
    nh.getParam("pitch", pitch);
    nh.getParam("yaw", yaw);
}

int main(int argc, char** argv){
    ros::init(argc, argv, "model_test_node");
    ros::NodeHandle nh("~");

    ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("model_point", 5);
    pcl::PointCloud<pcl::PointXYZ>::Ptr blob (new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr blob_trasform (new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr blob_orien (new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>());

    // Load Point for perticular location

    ros::Rate r(1);

    // pthread_t key_thread;
    // (void) pthread_create(&key_thread, 0, user_thread, 0);

    while (ros::ok() && !save_cloud)
    {
        updateParm(nh);
        pcl::io::loadPCDFile("/home/vishnu/ros_ws/test/box.pcd", *blob);
        pcl::transformPointCloud(*blob, *blob_trasform, Eigen::Vector3f(-size_x/2, -size_y/2, -size_z/2), Eigen::Quaternionf(1, 0, 0, 0));
        tf2::Quaternion q;
        q.setRPY(roll, pitch, yaw);
        pcl::transformPointCloud(*blob_trasform, *blob_orien, Eigen::Vector3f(0 , 0, 0), Eigen::Quaternionf(q.w(), q.x(), q.y(), q.z()));
        pcl::transformPointCloud(*blob_orien, *cloud, Eigen::Vector3f(px, py, pz), Eigen::Quaternionf(1, 0, 0, 0));

        cloud->header.frame_id = "model";
        pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
        pub.publish(cloud);

        // pcl::apps::RenderViewsTesselatedSphere rendered_view
        r.sleep();
        //TODO Save upon key stroke
        ROS_INFO("Saving Point Cloud");
        pcl::io::savePCDFile("/home/vishnu/ros_ws/test/box_modif2.pcd", *cloud);
    }
    // (void) pthread_join(key_thread, NULL);
    // ROS_INFO("Saving Point Cloud");
    // pcl::io::savePCDFile("/home/vishnu/ros_ws/test/box_modif.pcd", *cloud);
    return 0;
}


// Program that can extract features from an image and dump them to a file
//
// Author: Mark Desnoyer (markd@cmu.edu)
// Date: June 2010

#include <boost/filesystem.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/features2d/features2d.hpp"
#include <gflags/gflags.h>
#include <iostream>
#include <fstream>
#include <ros/ros.h>
#include <string>

#include "cv_blobs/BlobResult-Inline.h"
#include "cv_blobs/Blob.h"
#include "ImageDescriptor-Inl.h"
#include "ImageDescriptorGenerator-Inl.h"
#include "SIFTDescriptorGenerator.h"
#include "ImageDescriptorGeneratorFlags.h"

using namespace species_id;
using namespace std;
using namespace boost;
using namespace cv;
using namespace cv_blobs;
namespace fs = boost::filesystem;

// -------- Define the command line options  ------------

// The input and output locations
DEFINE_string(input, "", "Input image file to get the features of");
DEFINE_string(output, "","File to output the image descriptor to. "
              "If this is a "
              "blob image, multiple files will be created suffixed by "
              "indicies.");

int main(int argc, char *argv[]) {
  google::SetUsageMessage("Program that extracts features from an image "
                          "and prints them in ascii format to a file.");
  google::ParseCommandLineFlags(&argc, &argv, true);

  // Figure out the keypoint detector and descriptors to use
  VerifyDescriptorFlags();
  scoped_ptr<FeatureDetector> keyGenerator(ChooseFeatureDetector());
  scoped_ptr<ImageDescriptorGenerator<float> > descGenerator(
    ChooseImageDescriptor(keyGenerator.get()));
  ImageDescriptorINRIASerializer<float> serializer;

  // Now open up the input image
  ROS_INFO_STREAM("Extracting descriptors from " << FLAGS_input);
  vector<ImageRegion> masks;
  string imageFn;
  bool isBlobFile = FLAGS_input.rfind(".blob") != string::npos;
  if (isBlobFile) {
    // The file is a blob serialization
    BlobResultSerializer<int> blobReader;
    fstream blobStream(FLAGS_input.c_str(), ios::in);
    shared_ptr<BlobResult<int> > blobs =
      blobReader.Deserialize(blobStream, &imageFn,
                             fs::path(FLAGS_input).parent_path().string());

    for (int i = 0; i < blobs->nBlobs(); ++i) {
      masks.push_back(ImageRegion(*(blobs->GetBlob(i).ToImageRegion())));
    }
  } else {
    // The file is an actual image
    imageFn = FLAGS_input;
  }

  Mat image = imread(imageFn);
  if (image.empty()) {
    ROS_FATAL_STREAM("Could not read the image at file: " << imageFn);
  }

  // Next, create the descriptors of the image
  vector<shared_ptr<
    ImageDescriptorGenerator<float>::DescriptorCollection> >
    descriptors;
  descGenerator->ExtractUsingMasks(image, masks, &descriptors);

  // Finally, print out the descriptors
  for (unsigned int i = 0; i < descriptors.size(); ++i) {
    string outputFn(FLAGS_output);
    if (isBlobFile) {
      outputFn += '.' + lexical_cast<string>(i);
    } else {
      ROS_ASSERT(i == 0);
    }
    // Open the output file to write to
    fstream fileOut(outputFn.c_str(), ios::out | ios::trunc);
    ROS_INFO_STREAM("Writing descriptors to: " << outputFn);

    serializer.Write(fileOut, *(descriptors[i]));
  }

}


#include <cassert>
#include <boost/hana/assert.hpp>
#include <iostream>

#include <gem/gem.hpp>

using namespace gem;
using namespace boost::hana;

auto main(void) -> int
{

    /* CHECK DIMENSION */
    // {
    //     auto m = MatrixXd(1_u, 1_u);
    //     BOOST_HANA_CONSTANT_CHECK(m.order ^eq^ 2_u);
    // }

    // {
    //     auto m = MatrixXd(1, 2);

    //     BOOST_HANA_RUNTIME_CHECK(m.dim[GEM_START_IDX      ] ^eq^ 1u);
    //     BOOST_HANA_RUNTIME_CHECK(m.dim[GEM_START_IDX + 1_u] ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK(m.cols() ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK(m.cols() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK(m.rows() ^eq^ 1u);
    //     BOOST_HANA_RUNTIME_CHECK(m.rows() ^eq^ 1_u);
    //     BOOST_HANA_RUNTIME_CHECK(m.size() ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK(m.size() ^eq^ 2_u);
    // }

    // {
    //     auto m = MatrixXd(1_u, 2);

    //     BOOST_HANA_CONSTANT_CHECK(m.dim[GEM_START_IDX      ] ^eq^ 1_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.dim[GEM_START_IDX + 1_u] ^eq^ 2_u);
    //     BOOST_HANA_CONSTANT_CHECK(m.rows() ^eq^ 1_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.rows() ^eq^ 1u);
    //     BOOST_HANA_RUNTIME_CHECK (m.cols() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.cols() ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK (m.size() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.size() ^eq^ 2u);
    // }

    // {
    //     auto m = MatrixXd(1, 2_u);

    //     BOOST_HANA_RUNTIME_CHECK (m.dim[GEM_START_IDX      ] ^eq^ 1_u);
    //     BOOST_HANA_CONSTANT_CHECK(m.dim[GEM_START_IDX + 1_u] ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.rows() ^eq^ 1_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.rows() ^eq^ 1u);
    //     BOOST_HANA_CONSTANT_CHECK(m.cols() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.cols() ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK (m.size() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.size() ^eq^ 2u);
    // }

    // {
    //     auto m = MatrixXd(1_u, 2_u);

    //     BOOST_HANA_CONSTANT_CHECK(m.dim[GEM_START_IDX      ] ^eq^ 1_u);
    //     BOOST_HANA_CONSTANT_CHECK(m.dim[GEM_START_IDX + 1_u] ^eq^ 2_u);
    //     BOOST_HANA_CONSTANT_CHECK(m.rows() ^eq^ 1_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.rows() ^eq^ 1u);
    //     BOOST_HANA_CONSTANT_CHECK(m.cols() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.cols() ^eq^ 2u);
    //     BOOST_HANA_CONSTANT_CHECK(m.size() ^eq^ 2_u);
    //     BOOST_HANA_RUNTIME_CHECK (m.size() ^eq^ 2u);
    // }

    // return 0;
}


//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/CPPAlliance/url
//

#ifndef BOOST_URL_GRAMMAR_UNSIGNED_DECIMAL_HPP
#define BOOST_URL_GRAMMAR_UNSIGNED_DECIMAL_HPP

#include <boost/url/detail/config.hpp>
#include <boost/url/grammar/charset.hpp>
#include <boost/url/grammar/error.hpp>
#include <boost/url/grammar/parse_tag.hpp>
#include <boost/url/string_view.hpp>
#include <type_traits>

namespace boost {
namespace urls {
namespace grammar {

/** Rule for an unsigned decimal

    @par BNF
    @code
    unsigned      = "0" / ( ["1"..."9"] *DIGIT )
    @endcode
*/
template<class T>
struct unsigned_decimal
{
    BOOST_STATIC_ASSERT(
        std::is_unsigned<T>::value);

    T u;
    string_view s;

    friend
    void
    tag_invoke(
        parse_tag const&,
        char const*& it,
        char const* end,
        error_code& ec,
        unsigned_decimal& t) noexcept
    {
        parse(it, end, ec, t);
    }

private:
    static
    void
    parse(
        char const*& it,
        char const* end,
        error_code& ec,
        unsigned_decimal& t) noexcept
    {
        if(it == end)
        {
            ec = grammar::error::syntax;
            return;
        }
        if(*it == '0')
        {
            t.s = string_view(
                it, 1);
            ++it;
            t.u = 0;
            return;
        }
        T u = 0;
        auto const start = it;
        constexpr auto div = (T(-1) / 10);
        constexpr auto rem = (T(-1) % 10);
        while(it != end)
        {
            if(! digit_chars(*it))
                break;
            T const dig = *it - '0';
            if( u > div ||
                (u == div && dig > rem))
            {
                ec = grammar::error::overflow;
                return;
            }
            u = static_cast<T>(10 * u + dig);
            ++it;
        }
        t.u = u;
        t.s = string_view(start, it);
    }
};

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

} // grammar
} // urls
} // boost

#endif


#pragma once

#include "search/idf_map.hpp"

#include "base/assert.hpp"
#include "base/string_utils.hpp"

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <sstream>
#include <string>
#include <utility>

#include <boost/optional.hpp>

namespace search
{
class IdfMap;

struct TokenFrequencyPair
{
  TokenFrequencyPair() = default;

  template <typename Token>
  TokenFrequencyPair(Token && token, uint64_t frequency)
    : m_token(std::forward<Token>(token)), m_frequency(frequency)
  {
  }

  bool operator<(TokenFrequencyPair const & rhs) const;

  void Swap(TokenFrequencyPair & rhs);

  strings::UniString m_token;
  uint64_t m_frequency = 0;
};

std::string DebugPrint(TokenFrequencyPair const & tf);

// This class represents a document in a vector space of tokens.
class DocVec
{
public:
  class Builder
  {
  public:
    template <typename Token>
    void Add(Token && token)
    {
      m_tokens.emplace_back(std::forward<Token>(token));
    }

  private:
    friend class DocVec;

    std::vector<strings::UniString> m_tokens;
  };

  DocVec() = default;
  explicit DocVec(Builder const & builder);

  // Computes vector norm of the doc.
  double Norm(IdfMap & idfs) const;

  size_t GetNumTokens() const { return m_tfs.size(); }

  strings::UniString const & GetToken(size_t i) const;
  double GetIdf(IdfMap & idfs, size_t i) const;
  double GetWeight(IdfMap & idfs, size_t i) const;

  bool Empty() const { return m_tfs.empty(); }

private:
  friend std::string DebugPrint(DocVec const & dv)
  {
    return "DocVec " + ::DebugPrint(dv.m_tfs);
  }

  std::vector<TokenFrequencyPair> m_tfs;
};

// This class represents a search query in a vector space of tokens.
class QueryVec
{
public:
  class Builder
  {
  public:
    template <typename Token>
    void AddFull(Token && token)
    {
      m_tokens.emplace_back(std::forward<Token>(token));
    }

    template <typename Token>
    void SetPrefix(Token && token)
    {
      m_prefix = std::forward<Token>(token);
    }

  private:
    friend class QueryVec;

    std::vector<strings::UniString> m_tokens;
    boost::optional<strings::UniString> m_prefix;
  };

  explicit QueryVec(IdfMap & idfs) : m_idfs(&idfs) {}

  QueryVec(IdfMap & idfs, Builder const & builder);

  // Computes cosine similarity between |*this| and |rhs|.
  double Similarity(IdfMap & docIdfs, DocVec const & rhs);

  // Computes vector norm of the query.
  double Norm();

  bool Empty() const { return m_tfs.empty() && !m_prefix; }

private:
  double GetFullTokenWeight(size_t i);
  double GetPrefixTokenWeight();

  friend std::string DebugPrint(QueryVec const & qv)
  {
    std::ostringstream os;
    os << "QueryVec " + ::DebugPrint(qv.m_tfs);
    if (qv.m_prefix)
      os << " " << DebugPrint(*qv.m_prefix);
    return os.str();
  }

  IdfMap * m_idfs;
  std::vector<TokenFrequencyPair> m_tfs;
  boost::optional<strings::UniString> m_prefix;
};
}  // namespace search


#include <boost/numeric/ublas/blas.hpp>


#include "v4r/registration/StablePlanesRegistration.h"
#include <boost/graph/connected_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_matrix.hpp>

#include <pcl/registration/icp.h>
#include <pcl/surface/convex_hull.h>
#include <v4r/common/noise_models.h>
#include <v4r/registration/noise_model_based_cloud_integration.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/angles.h>

template<class PointT>
v4r::Registration::StablePlanesRegistration<PointT>::StablePlanesRegistration()
{
    name_ = "StablePlanesRegistration";
}

template<class PointT>
void
v4r::Registration::StablePlanesRegistration<PointT>::mergeTriangles(pcl::PolygonMesh::Ptr & mesh_out,
                                                                    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & model_cloud,
                                                                    std::vector<stablePlane> & stable_planes)
{

    pcl::PointCloud<pcl::PointXYZ> hull_cloud;
    pcl::fromPCLPointCloud2(mesh_out->cloud, hull_cloud);

    std::cout << "Number of polygons:" << mesh_out->polygons.size() << std::endl;

    std::vector<Eigen::Vector3f> normals;
    std::vector<float> areas;
    std::vector<Eigen::Vector3f> centers;

    normals.resize(mesh_out->polygons.size());
    areas.resize(mesh_out->polygons.size());
    centers.resize(mesh_out->polygons.size());

    Eigen::Vector3f centroid;
    int n_points = 0;
    for(size_t i=0; i < mesh_out->polygons.size(); i++, n_points++)
    {
        Eigen::Vector3f center;
        center.setZero();
        for(size_t k=0; k < mesh_out->polygons[i].vertices.size(); k++)
            center = center + hull_cloud.points[mesh_out->polygons[i].vertices[k]].getVector3fMap();

        center = center / 3.f;
        centroid += center;
    }

    centroid /= static_cast<float>(n_points);

    for(size_t i=0; i < mesh_out->polygons.size(); i++)
    {
        Eigen::Vector3f v1, v2;
        v1 = hull_cloud.points[mesh_out->polygons[i].vertices[1]].getVector3fMap() - hull_cloud.points[mesh_out->polygons[i].vertices[0]].getVector3fMap();
        v2 = hull_cloud.points[mesh_out->polygons[i].vertices[2]].getVector3fMap() - hull_cloud.points[mesh_out->polygons[i].vertices[0]].getVector3fMap();
        Eigen::Vector3f cross_v1_v2 = v2.cross(v1);
        float area = 0.5f * cross_v1_v2.norm();
        areas[i] = area;

        Eigen::Vector3f center;
        center.setZero();
        for(size_t k=0; k < mesh_out->polygons[i].vertices.size(); k++)
            center = center + hull_cloud.points[mesh_out->polygons[i].vertices[k]].getVector3fMap();

        center = center / 3.f;
        centers[i] = center;

        Eigen::Vector3f normal = cross_v1_v2;
        normal.normalize();

        //make sure that the normal obtained from the face is pointing in the right direction, otherwise flip it
        //all normals looking outwards from the convex hull centroid
        Eigen::Vector3f direction = center - centroid;
        direction.normalize();
        if(normal.dot(direction) < 0)
            normal = normal * -1.f;

        normals[i] = normal;
    }

    float good_dot = 0.98f;
    int good_normals_pairs=0;
    float min_area = 1e-4;
    std::vector<std::pair<int,int> > lines;

    for(size_t i=0; i < normals.size(); i++)
    {
        if(areas[i] < min_area)
            continue;

        if(checkForGround(centers[i], normals[i]))
            continue;

        for(size_t j=(i+1); j < normals.size(); j++)
        {
            if(areas[j] < min_area)
                continue;

            /*if(checkForGround(centers[j], normals[j]))
                continue;*/

            float dot = normals[i].dot(normals[j]);
            if(dot < good_dot)
                continue;

            if(!checkShareVertex(mesh_out->polygons[i].vertices,mesh_out->polygons[j].vertices))
                continue;

            good_normals_pairs++;
            lines.push_back(std::make_pair((int)i,(int)j));
        }
    }

    std::cout << "good_normals_pairs: " << good_normals_pairs << std::endl;

    //Build graph (good_normals_pairs edges, #vertices?)
    typedef boost::adjacency_matrix<boost::undirectedS, int> Graph;
    Graph G(mesh_out->polygons.size());
    for(size_t i=0; i < lines.size(); i++)
    {
        boost::add_edge(lines[i].first, lines[i].second, G);
    }

    std::vector<int> components (boost::num_vertices (G));
    int n_cc = static_cast<int> (boost::connected_components (G, &components[0]));
    std::cout << "Number of connected components..." << n_cc << std::endl;

    std::vector< std::vector<int> > unique_vertices_per_cc;
    std::vector<int> cc_sizes;
    std::vector<float> cc_areas;
    cc_sizes.resize (n_cc, 0);
    cc_areas.resize (n_cc, 0);
    unique_vertices_per_cc.resize (n_cc);

    typename boost::graph_traits<Graph>::vertex_iterator vertexIt, vertexEnd;
    boost::tie (vertexIt, vertexEnd) = vertices (G);
    for (; vertexIt != vertexEnd; ++vertexIt)
    {
        int c = components[*vertexIt];
        unique_vertices_per_cc[c].push_back(*vertexIt);
        cc_sizes[c]++;
        cc_areas[c]+=areas[*vertexIt];
    }

    for(size_t i=0; i < cc_sizes.size(); i++)
    {
        if(cc_areas[i] < min_area)
            continue;

        std::cout << "size:" << cc_sizes[i] << " area:" << cc_areas[i] << std::endl;
        stablePlane sp;
        sp.area_ = cc_areas[i];
        sp.polygon_indices_ = unique_vertices_per_cc[i];

        //TODO: Weighted normal based on area!
        sp.normal_ = Eigen::Vector3f::Zero();
        for(size_t k=0; k < unique_vertices_per_cc[i].size(); k++)
        {
            sp.normal_ += normals[unique_vertices_per_cc[i][k]];
        }

        sp.normal_ = sp.normal_ / (int)(unique_vertices_per_cc[i].size());
        sp.normal_.normalize();
        stable_planes.push_back(sp);
    }

    /*pcl::visualization::PCLVisualizer vis ("merging triangles");
    int v1, v2;
    vis.createViewPort (0, 0, 0.5, 1, v1);
    vis.createViewPort (0.5, 0, 1, 1, v2);
    vis.setBackgroundColor(0,0,0);
    vis.addCoordinateSystem(0.1f);
    vis.addPolygonMesh(*mesh_out, "hull", v1);

    for(size_t i=0; i < lines.size(); i++)
    {
        pcl::PointXYZ p1,p2;
        p1.getVector3fMap() = centers[lines[i].first];
        p2.getVector3fMap()  = centers[lines[i].second];

        std::stringstream name;
        name << "line_" << i;
        vis.addLine(p1,p2, name.str(), v2);
    }

    pcl::PointCloud<pcl::PointNormal>::Ptr normal_center_cloud(new pcl::PointCloud<pcl::PointNormal>);
    for(size_t i=0; i < centers.size(); i++)
    {

        if(areas[i] < min_area)
            continue;

        if(checkForGround(centers[i], normals[i]))
            continue;

        pcl::PointNormal p;
        p.getVector3fMap() = centers[i];
        p.getNormalVector3fMap() = normals[i];
        normal_center_cloud->push_back(p);
    }

    pcl::PointXYZ centroid_sphere;
    centroid_sphere.getVector3fMap() = centroid;
    vis.addSphere<pcl::PointXYZ>(centroid_sphere, 0.01, "sphere");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> handler (normal_center_cloud, 255, 0 ,0);
    vis.addPointCloud<pcl::PointNormal> (normal_center_cloud, handler, "center_cloud");
    vis.addPointCloudNormals<pcl::PointNormal,pcl::PointNormal> (normal_center_cloud, normal_center_cloud, 1, 0.01, "normal_center_cloud");
    vis.spin();*/

}

template<class PointT>
void
v4r::Registration::StablePlanesRegistration<PointT>::initialize(std::vector<std::pair<int, int> > & session_ranges)
{
    //for each partial model, get a clean model and compute stable planes
    stable_planes_.resize(session_ranges.size());
    partial_models_with_normals_.resize(session_ranges.size());

    for(size_t i=0; i < session_ranges.size(); i++)
    {

        int clouds_session = session_ranges[i].second - session_ranges[i].first + 1;
        std::vector<std::vector<std::vector<float> > > pt_properties (clouds_session);
        std::vector<typename pcl::PointCloud<PointT>::ConstPtr> clouds(clouds_session);
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > poses(clouds_session);
        std::vector<std::vector<int> > indices(clouds_session);
        std::vector< pcl::PointCloud<pcl::Normal>::ConstPtr > normals(clouds_session);

        int k=0;
        for(int t=session_ranges[i].first; t <= session_ranges[i].second; t++, k++)
        {
            std::cout << k << " " << t << " " << clouds.size() << std::endl;
            clouds[k] = this->getCloud(t);
            poses[k] = this->getPose(t);
            normals[k] = this->getNormal(t);
            indices[k] = this->getIndices(t);

            NguyenNoiseModel<PointT> nm;
            nm.setInputCloud(clouds[k]);
            nm.setInputNormals(normals[k]);
            nm.compute();
            pt_properties[k] = nm.getPointProperties();
        }

        typename pcl::PointCloud<PointT>::Ptr octree_cloud(new pcl::PointCloud<PointT>);
        pcl::PointCloud<pcl::Normal>::Ptr big_normals(new pcl::PointCloud<pcl::Normal>);
        NMBasedCloudIntegrationParameter nmparam;
        nmparam.octree_resolution_ = 0.005f;
        nmparam.min_points_per_voxel_ = 1;
        NMBasedCloudIntegration<pcl::PointXYZRGB> nmIntegration (nmparam);
        nmIntegration.setInputClouds(clouds);
        nmIntegration.setTransformations(poses);
        nmIntegration.setInputNormals(normals);
        nmIntegration.setIndices(indices);
        nmIntegration.setPointProperties(pt_properties);
        nmIntegration.compute(octree_cloud);
        nmIntegration.getOutputNormals(big_normals);

        partial_models_with_normals_[i].reset (new pcl::PointCloud<pcl::PointXYZRGBNormal>());
        pcl::concatenateFields(*big_normals, *octree_cloud, *partial_models_with_normals_[i]);

        pcl::ConvexHull<pcl::PointXYZRGBNormal> convex_hull;
        convex_hull.setInputCloud (partial_models_with_normals_[i]);
        convex_hull.setDimension (3);
        convex_hull.setComputeAreaVolume (false);

        pcl::PolygonMeshPtr mesh_out(new pcl::PolygonMesh);
        convex_hull.reconstruct (*mesh_out);

        mergeTriangles(mesh_out, partial_models_with_normals_[i], stable_planes_[i]);
        std::cout << "Stable planes size:" << stable_planes_[i].size() << std::endl;

        std::stable_sort(stable_planes_[i].begin(), stable_planes_[i].end(),
          boost::bind(&stablePlane::area_, _1) > boost::bind(&stablePlane::area_, _2)
        );
    }
}

template<class PointT>
void
v4r::Registration::StablePlanesRegistration<PointT>::compute(int s1, int s2)
{
    //test stable planes and do ICP, return as many poses as necessary
    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > initial_poses;
    float step = 30;
    typedef pcl::PointXYZRGBNormal ModelPointT;

    int MAX_PLANES_ = 4;

//#define VIS_PLANES

#ifdef VIS_PLANES
    pcl::visualization::PCLVisualizer vis_s2("s2 on stable planes...");
    int v1,v2;
    vis_s2.createViewPort(0,0,0.5,1,v1);
    vis_s2.createViewPort(0.5,0,1,1,v2);
#endif

    //transform partial_models_with_normals_[s1] to be on computed_planes_[s1]
    typename pcl::PointCloud<ModelPointT>::Ptr target_model_on_plane(new pcl::PointCloud<ModelPointT>(*partial_models_with_normals_[s1]));
    Eigen::Matrix4f target_transform = Eigen::Matrix4f::Identity();

#ifdef VIS_PLANES
    pcl::visualization::PointCloudColorHandlerRGBField<ModelPointT> handler(target_model_on_plane);
    vis_s2.addPointCloud(target_model_on_plane, handler, "target", v1);
    vis_s2.addCoordinateSystem(0.1);
#endif

    for(size_t i=0; i < std::min(MAX_PLANES_, (int)stable_planes_[s2].size()); i++)
    {
        std::cout << stable_planes_[s2][i].area_ << std::endl;
        Eigen::Vector3f normal = stable_planes_[s2][i].normal_;
        Eigen::Matrix4f transform;
        transform.setIdentity();

        std::cout << normal << " norm:" << normal.norm() << std::endl;

        //ATTENTION: Make sure that the determinant is non-negative (meaning that we have an invertible rotation matrix, otherwise weird flips...)
        transform.block<3,1>(0,2) = normal * -1.f;

        if(std::abs(Eigen::Vector3f::UnitZ().dot(transform.block<3,1>(0,2))) < 0.9f)
        {
            transform.block<3,1>(0,1) = Eigen::Vector3f::UnitZ().cross(transform.block<3,1>(0,2));
        }
        else
        {
            transform.block<3,1>(0,1) = Eigen::Vector3f::UnitY().cross(transform.block<3,1>(0,2));
        }

        transform.block<3,1>(0,0) = transform.block<3,1>(0,1).cross(transform.block<3,1>(0,2));
        transform.block<3,1>(0,1).normalize();
        transform.block<3,1>(0,0).normalize();

        assert(transform.determinant() > 0);

        transform = transform.inverse().eval();
        typename pcl::PointCloud<ModelPointT>::Ptr model_cloud_trans(new pcl::PointCloud<ModelPointT>);
        pcl::transformPointCloudWithNormals(*partial_models_with_normals_[s2], *model_cloud_trans, transform);

        Eigen::Vector4f min_pt, max_pt;
        pcl::getMinMax3D(*model_cloud_trans, min_pt, max_pt);

        Eigen::Matrix4f translation;
        translation.setIdentity();
        translation(2,3) = -min_pt[2]; //- 0.01f; //maybe, using min point is not good
        pcl::transformPointCloudWithNormals(*model_cloud_trans, *model_cloud_trans, translation);

        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*model_cloud_trans, centroid);

        Eigen::Matrix4f center_transform;
        center_transform.setIdentity();
        center_transform(0,3) = -centroid[0];
        center_transform(1,3) = -centroid[1];
        center_transform(2,3) = 0;
        pcl::transformPointCloudWithNormals(*model_cloud_trans, *model_cloud_trans, center_transform);

        transform = center_transform * translation * transform;

#ifdef VIS_PLANES
        {
            pcl::transformPointCloudWithNormals(*partial_models_with_normals_[s2], *model_cloud_trans, transform);

            pcl::visualization::PointCloudColorHandlerRGBField<ModelPointT> handler(model_cloud_trans);
            vis_s2.addPointCloud(model_cloud_trans, handler, "source_v2", v2);
            vis_s2.spin();
        }
#endif

        float rotation = 0.f;
        while(rotation < 360.f)
        {
            Eigen::AngleAxisf rotation_z = Eigen::AngleAxisf (static_cast<float> (pcl::deg2rad(rotation)), Eigen::Vector3f::UnitZ());
            Eigen::Matrix4f pose;
            pose.setIdentity();
            pose.block<3,3>(0,0) = rotation_z.toRotationMatrix();
            pose = pose * transform;
            initial_poses.push_back(pose);
            rotation += step;

#ifdef VIS_PLANES
            {
                pcl::transformPointCloudWithNormals(*partial_models_with_normals_[s2], *model_cloud_trans, pose);

                pcl::visualization::PointCloudColorHandlerRGBField<ModelPointT> handler(model_cloud_trans);
                vis_s2.addPointCloud(model_cloud_trans, handler, "source", v1);
                vis_s2.spin();
                vis_s2.removePointCloud("source");
            }
#endif

        }

#ifdef VIS_PLANES
        vis_s2.removePointCloud("source_v2");
#endif
    }

    /*{
        Eigen::Vector3f normal = computed_planes_[s1];
        Eigen::Matrix4f transform;
        transform.setIdentity();

        //ATTENTION: Make sure that the determinant is non-negative (meaning that we have an invertible rotation matrix, otherwise weird flips...)
        transform.block<3,1>(0,2) = normal * -1.f;
        transform.block<3,1>(0,1) = Eigen::Vector3f::UnitZ().cross(transform.block<3,1>(0,2));
        transform.block<3,1>(0,0) = transform.block<3,1>(0,1).cross(transform.block<3,1>(0,2));
        transform.block<3,1>(0,1).normalize();
        transform.block<3,1>(0,0).normalize();

        assert(transform.determinant() > 0);

        transform = transform.inverse().eval();
        typename pcl::PointCloud<ModelPointT>::Ptr model_cloud_trans(new pcl::PointCloud<ModelPointT>);
        pcl::transformPointCloudWithNormals(*partial_models_with_normals_[s1], *model_cloud_trans, transform);

        Eigen::Vector4f min_pt, max_pt;
        pcl::getMinMax3D(*model_cloud_trans, min_pt, max_pt);

        Eigen::Matrix4f translation;
        translation.setIdentity();
        translation(2,3) = -min_pt[2];
        pcl::transformPointCloudWithNormals(*model_cloud_trans, *model_cloud_trans, translation);

        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*model_cloud_trans, centroid);

        Eigen::Matrix4f center_transform;
        center_transform.setIdentity();
        center_transform(0,3) = -centroid[0];
        center_transform(1,3) = -centroid[1];
        center_transform(2,3) = 0;
        pcl::transformPointCloudWithNormals(*model_cloud_trans, *target_model_on_plane, center_transform);

        transform = center_transform * translation * transform;
        target_transform = transform;
    }*/

    poses_.clear();

    {
        /*pcl::visualization::PCLVisualizer vis("initial alignment on plane");
        pcl::visualization::PointCloudColorHandlerRGBField<ModelPointT> handler(target_model_on_plane);
        vis.addPointCloud(target_model_on_plane, handler, "target");
        vis.addCoordinateSystem(0.1);*/

        int steps_rotation = 360.f / step;
        std::cout << "steps rotation:" << steps_rotation << std::endl;

        std::cout << "Going to do ICP with " << initial_poses.size() << " poses:" << std::endl;
        poses_.resize(initial_poses.size());

#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
        for(size_t i=0; i < initial_poses.size(); i++)
        {
            pcl::IterativeClosestPoint<ModelPointT, ModelPointT> icp;
            icp.setInputSource(partial_models_with_normals_[s2]);
            icp.setInputTarget(target_model_on_plane);
            icp.setMaxCorrespondenceDistance(0.01f);
            icp.setUseReciprocalCorrespondences(false);
            icp.setMaximumIterations(50);

            pcl::PointCloud<ModelPointT> out_cloud;
            icp.align(out_cloud, initial_poses[i]);
            Eigen::Matrix4f output = icp.getFinalTransformation();

            //output transform from s2 to s1, however the coordinate system of both have been changed
            Eigen::Matrix4f total_trans_s2_to_s1 = target_transform.inverse() * output;
            poses_[i] = total_trans_s2_to_s1;

            std::cout << "Done " << i << std::endl;

            if(static_cast<int>(i) % steps_rotation != 0)
                continue;

            /*typename pcl::PointCloud<ModelPointT>::Ptr model_cloud_trans(new pcl::PointCloud<ModelPointT>);
            pcl::transformPointCloudWithNormals(*partial_models_with_normals_[s2], *model_cloud_trans, output);

            pcl::visualization::PointCloudColorHandlerRGBField<ModelPointT> handler(model_cloud_trans);
            vis.addPointCloud(model_cloud_trans, handler, "source");
            vis.spin();
            vis.removePointCloud("source");*/
        }

    }
}

template class V4R_EXPORTS v4r::Registration::StablePlanesRegistration<pcl::PointXYZRGB>;


/*
 * generator.tpp
 *
 *  Created on: Apr 19, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "generator_crtp.hpp"
#include "../error_handling/throw_assert.hpp"
#include "interpolation_method.hpp"

namespace tsdf {

namespace eig = Eigen;

template<typename Generator, typename ScalarContainer>
GeneratorCRTP<Generator, ScalarContainer>::GeneratorCRTP(const Parameters<ScalarContainer>& parameters) :
		parameters(parameters) {
}

template<typename Generator, typename ScalarContainer>
ScalarContainer GeneratorCRTP<Generator, ScalarContainer>::generate(
		const eig::Matrix<unsigned short, eig::Dynamic, eig::Dynamic, eig::ColMajor>& depth_image,
		const eig::Matrix<ContainerScalar, 4, 4, eig::ColMajor>& camera_pose,
		int image_y_coordinate) const {

	switch (parameters.interpolation_method) {
	case FilteringMethod::NONE:
		return static_cast<Generator const&>(*this).generate__none(depth_image, camera_pose, image_y_coordinate);
	case FilteringMethod::BILINEAR_IMAGE_SPACE:
		return static_cast<Generator const&>(*this).generate__bilinear_image_space(depth_image, camera_pose,
				image_y_coordinate);
	case FilteringMethod::BILINEAR_VOXEL_SPACE:
		return static_cast<Generator const&>(*this).generate__bilinear_voxel_space(depth_image, camera_pose,
				image_y_coordinate);
	case FilteringMethod::EWA_IMAGE_SPACE:
		return static_cast<Generator const&>(*this).generate__ewa_image_space(depth_image, camera_pose,
				image_y_coordinate);
		break;
	case FilteringMethod::EWA_VOXEL_SPACE:
		return static_cast<Generator const&>(*this).generate__ewa_voxel_space(depth_image, camera_pose,
				image_y_coordinate);
		break;
	case FilteringMethod::EWA_VOXEL_SPACE_INCLUSIVE:
		return static_cast<Generator const&>(*this).generate__ewa_voxel_space_inclusive(depth_image, camera_pose,
				image_y_coordinate);
		break;
	default:
		throw_assert(false,
				"Unknown InterpolationMethod enum value, " << static_cast<int>(parameters.interpolation_method))
		;
	}
}

}  // namespace tsdf


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "Utils/GeometryOptimization/AfirConvergenceCheck.h"
#include "Utils/GeometryOptimization/AfirOptimizerBase.h"
#include "Utils/Optimizer/GradientBased/GradientBasedCheck.h"
#include "Utils/Settings.h"
#include <Eigen/Core>

namespace Scine {
namespace Utils {

bool AfirConvergenceCheck::checkConvergence(const Eigen::VectorXd& parameter, double value, const Eigen::VectorXd& gradient) {
  bool gradientConvergence = GradientBasedCheck::checkConvergence(parameter, value, gradient);
  // If required evaluate fragment distance
  bool exceedsDistanceThreshold = false;
  if (afirUseMaxFragmentDistance) {
    Utils::PositionCollection positions;
    if (this->transformation) {
      positions = this->transformation->coordinatesToCartesian(parameter);
    }
    else {
      positions = Eigen::Map<const Utils::PositionCollection>(parameter.data(), parameter.size() / 3, 3);
    }
    exceedsDistanceThreshold = this->checkExceedsDistanceThreshold(positions);
  }
  // If either converged or distance threshold exceeded return true
  return gradientConvergence || exceedsDistanceThreshold;
}

bool AfirConvergenceCheck::checkExceedsDistanceThreshold(Utils::PositionCollection& positions) {
  return this->calculateMinFragmentDistance(positions) >= afirMaxFragmentDistance;
}

double AfirConvergenceCheck::calculateMinFragmentDistance(Utils::PositionCollection& positions) {
  double minDistance = std::numeric_limits<double>::max();
  double distance;
  if (!lhsList.empty() && !rhsList.empty()) {
    for (int i : lhsList) {
      for (int j : rhsList) {
        distance = (positions.row(i) - positions.row(j)).norm();
        // Set minimum distance to minimum of previous and current value
        minDistance = std::min(minDistance, distance);
      }
    }
  }
  // If at least one of the lists is empty this check does not make sense
  else {
    throw std::runtime_error(
        "AFIR LHS and/or RHS list empty. Cannot use a minimum fragment distance without defined fragments.");
  }
  return minDistance;
}

void AfirConvergenceCheck::addAfirSettingsDescriptors(UniversalSettings::DescriptorCollection& collection) const {
  // Add afir specific setting descriptors
  UniversalSettings::BoolDescriptor afir_use_max_fragment_distance(
      "Whether to stop the AFIR optimization when exceeding a maximum interfragment distance.");
  afir_use_max_fragment_distance.setDefaultValue(afirUseMaxFragmentDistance);
  collection.push_back(AfirConvergenceCheck::afirUseMaxFragmentDistanceKey, afir_use_max_fragment_distance);
  UniversalSettings::DoubleDescriptor afir_max_fragment_distance(
      "Interfragment distance upon exceeding which the AFIR optimization is stopped.");
  afir_max_fragment_distance.setDefaultValue(afirMaxFragmentDistance);
  collection.push_back(AfirConvergenceCheck::afirMaxFragmentDistanceKey, afir_max_fragment_distance);
}

void AfirConvergenceCheck::applyAfirSettings(const Settings& settings) {
  // Apply AFIR specific convergence settings
  afirUseMaxFragmentDistance = settings.getBool(AfirConvergenceCheck::afirUseMaxFragmentDistanceKey);
  afirMaxFragmentDistance = settings.getDouble(AfirConvergenceCheck::afirMaxFragmentDistanceKey);
}

} // namespace Utils
} // namespace Scine