/**********************************************************
Copyright 2014 by Luca Zini and Francesca Odone,
Department of Informatics, Bioengineering, Robotics, and Systems
Engineering University of Genova.

This is a free software: you can redistribute it and/or modify it
under the terms of the CC-BY Public License, Version 4.0, 25
November 2013. This program is distributed in the hope that it will
be useful, but WITHOUT ANY WARRANTY, including but not limited to
the warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the CC-BY Public License copy provided with this software for more
details.

@author Luca Zini (luca.zini@gmail.com)

***********************************************************/
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <boost/thread/mutex.hpp>
#include "roc.h"
#include "extern/Libsvm/svm.h"
#include "cmd-utils.h"
#include "parfor.h"
#include "dataset_management.h"
#include "errors_evaluation.h"

void free_svm_problem(svm_problem& prob){
    delete[] prob.y;
    for(unsigned i = 0; i < prob.l; i++){
        delete[] prob.x[i];
    }
    delete[] prob.x;
}

int cacheSize = 1000;

struct svm_parameter buildPar(int kernel, double kernelpar, double eps, double c){
    struct svm_parameter param;
    param.cache_size = cacheSize;
    param.eps = eps;
    param.C = c;
    if(kernel == RBF || kernel == SIGMOID){
        param.gamma = kernelpar;
    }else if(kernel == POLY){
        param.gamma = 1;
        param.degree = kernelpar;
    }
    param.kernel_type = kernel;
    param.svm_type = C_SVC;
    param.shrinking = 1;
    param.probability = 0;
    param.nr_weight = 0;
    param.weight_label = 0;
    param.weight = 0;

    //DEFAULT
    param.coef0 = 0;
    param.nu = 0.5;
    param.p = 0.1;
    return param;
}

namespace po = boost::program_options;
using namespace std;

template <class ERR_EVAL>
class LibSVMPool : public ERR_EVAL{
public:
    void operator()(int cIndex){
        assert(cs.at(cIndex).size() == 2);

        struct svm_parameter par = buildPar(kernelType, cs.at(cIndex)[1], epsilon, cs.at(cIndex)[0]);
        double* ris = 0;
        if(validation){
            ris = new double[val.l];
        }else if(kcv > 1){
            ris = new double[prob.l];
        }

        std::pair<double, double> prec(0,0);
        if(validation){
            svm_model* mod = svm_train(&prob, &par);
            if(saveAll){
                std::stringstream modelPath;
                modelPath << baseModelPath <<  "_";
                for(unsigned p = 0; p < cs[cIndex].size(); p++){
                    modelPath <<  "_" << cs.at(cIndex)[p];
                }
                modelPath << ".model";
                svm_save_model(modelPath.str().c_str(),mod);
            }
            for(unsigned j = 0; j < val.l; j++){
                svm_predict_values(mod, val.x[j], &(ris[j]));
            }
            prec = ERR_EVAL::accuracy(ris, val.y, val.l);
            {
                boost::mutex::scoped_lock l(m_mutex);
                if(rocBasePath != "" && saveAll){
                    std::stringstream rocName;
                    rocName << rocBasePath;
                    for(unsigned p = 0; p < cs[cIndex].size(); p++){
                        rocName <<  "_" << cs.at(cIndex)[p];
                    }
                    rocName << ".roc";
                    std::ofstream outroc(rocName.str().c_str());
                    saveRoc(outroc, val.y, ris, val.l);
                    outroc.close();
                }else if(rocBasePath != ""&& prec.first > bestPrecision.first){
                    std::ofstream outroc(rocBasePath.c_str());
                    saveRoc(outroc, val.y, ris, val.l);
                    outroc.close();
                }
                if(prec > bestPrecision){
                    if(bestModel != 0){
                        svm_free_and_destroy_model(&bestModel);
                    }
                    bestPrecision = prec;
                    bestIndex = cIndex;
                    bestModel = mod;
                }else{
                    svm_free_and_destroy_model(&mod);
                }

                dstream << "LibSVM classifier C = " << par.C;
                if(cs[cIndex].size() > 1){
                    dstream <<" kernel parameters " ;
                    for(unsigned p = 1; p < cs[cIndex].size(); p++){
                        dstream <<" "  << cs[cIndex][p];
                    }
                }
                dstream << " Accuracy: " << prec.first << "\n";
            }
        }else if(kcv > 1){
            svm_cross_validation_fuzzy(&prob, &par, kcv, ris);
            prec = ERR_EVAL::accuracy(ris, prob.y, prob.l);
            {
                boost::mutex::scoped_lock l(m_mutex);
                if(rocBasePath != "" && saveAll){
                    std::stringstream rocName;
                    rocName << rocBasePath;
                    for(unsigned p = 0; p < cs[cIndex].size(); p++){
                        rocName <<  "_" << cs.at(cIndex)[p];
                    }
                    rocName << ".roc";
                    std::ofstream outroc(rocName.str().c_str());
                    saveRoc(outroc, prob.y, ris, prob.l);
                    outroc.close();
                }else  if(rocBasePath != ""&& prec > bestPrecision){
                    std::ofstream outroc(rocBasePath.c_str());
                    saveRoc(outroc, prob.y, ris, prob.l);
                    outroc.close();
                }
                if(prec > bestPrecision){
                    bestPrecision = prec;
                    bestIndex = cIndex;
                }
                dstream << "LibSVM classifier C = " << par.C;
                if(cs[cIndex].size() > 1){
                    dstream <<" kernel parameters " ;
                    for(unsigned p = 1; p < cs[cIndex].size(); p++){
                        dstream <<" "  << cs[cIndex][p];
                    }
                }
                dstream<< " Accuracy: " << prec.first << "\n";
            }
        }else if(saveAll || cs.size() == 1){
            svm_model* mod = svm_train(&prob, &par);
            {
                boost::mutex::scoped_lock l(m_mutex);

                std::stringstream modelPath;
                if(saveAll){
                    modelPath << baseModelPath <<  "_";
                    for(unsigned p = 0; p < cs[cIndex].size(); p++){
                        modelPath <<  "_" << cs.at(cIndex)[p];
                    }
                    modelPath << ".model";
                }else{
                    modelPath << baseModelPath;
                }
                svm_save_model(modelPath.str().c_str(), mod);

                dstream << "LibSVM classifier C = " << par.C;
                if(cs[cIndex].size() > 1){
                    dstream <<" kernel parameters " ;
                    for(unsigned p = 1; p < cs[cIndex].size(); p++){
                        dstream <<" "  << cs[cIndex][p] << "\n";
                    }
                }
                if(cs.size() > 1){
                    svm_free_and_destroy_model(&mod);
                }else{
                    bestModel = mod;
                }
            }
        }else{
            throw std::runtime_error("Undefined");
        }

        delete[] ris;
        computed[cIndex] = true;
    }

    double getPrecision() const {
        return bestPrecision.first;
    }

    unsigned getBestIndex() const {
        return bestIndex;
    }

    svm_model* getBestModel() const {
        return bestModel;
    }

    LibSVMPool( std::vector<std::vector<double> > cs, struct svm_problem prob, struct svm_problem val, bool validation, unsigned kcv, double epsilon, std::string rocBasePath, bool saveAll, int kernelType, std::string baseModelPath):
        kernelType(kernelType), cs(cs), computed(cs.size(), false), prob(prob), val(val),validation(validation), kcv(kcv), epsilon(epsilon), rocBasePath(rocBasePath), saveAll(saveAll), baseModelPath(baseModelPath), bestModel(0), computedModels(cs.size(), 0){
        bestPrecision = std::pair<double, double>(0, 0);
        bestIndex = -1;
        bestModel = 0;
    }

private:
    std::string rocBasePath;
    std::pair<double, double> bestPrecision;
    int bestIndex;
    int kernelType;
    std::vector<std::vector<double> > cs;
    std::vector<bool> computed;
    struct svm_problem prob;
    struct svm_problem val;
    bool validation;
    int kcv;
    double epsilon;
    boost::mutex m_mutex;
    bool saveAll;
    std::string baseModelPath;
    svm_model* bestModel;
    std::vector<svm_model*> computedModels;
};

int main(int argc, char** argv){
    int kernelType = RBF;
    int ERR_TYPE =1;
    bool singlePrecision = false;
    bool binary = false;
    int kcv = 1;
    std::string rocBasePath = "", debugFile, trainFile, testFile, cRange, gammaRange, modelFile = "", degreeRange;
    struct svm_problem val;
    double epsilon = 0.01;
    double requiredTP = 0.995;
    bool saveAll = false;
    std::vector<double> regs, gammas, degrees;
    std::vector<std::vector<double> > cs;
    struct svm_problem prob;
    po::options_description desc("Allowed options");
    desc.add_options()
            ("help", "produce help message")
            ("cachesize", po::value<int>(&cacheSize),"cache size in mb (-m libsvm option) default 1000")
            ("kernel", po::value<int>(&kernelType),"kernel type: \n 0 -- Linear \n 1 -- Polynomial \n 2 -- RBF \n 3 -- Sigmoid \n default RBF")
            ("save-all", "save all ROCs and models. If omitted saves only the roc selected with kcv or validation")
            ("binary", "use binary dataset instead of libsvm format")
            ("roc", po::value<std::string>(&rocBasePath), "roc base path and name")
            ("log", po::value<std::string>(&debugFile),"log file")
            ("train-set", po::value<std::string>(&trainFile),"train data file")
            ("validation-set", po::value<std::string>(&testFile), "validation data file used for model selection")
            ("c", po::value<std::string>(&cRange), "C parameter of the SVM. It can be a scalar or a range in the form first:N:end. In this second case a geometric serie of N elements will be used to sample in the range first-end")
            ("gamma", po::value<std::string>(&gammaRange), "rbf kernel parameter. It can be a scalar or a range in the form first:N:end. In this second case a geometric serie of N elements will be used to sample in the range first-end")
            ("degree", po::value<std::string>(&degreeRange), "degree of polynomial kernel parameter. It can be a scalar or a range in the form first:N:end. In this second case a linear span of N elements will be used to sample in the range first-end")
            ("epsilon", po::value<double>(&epsilon), " set tolerance of termination criterion\n -s 0 and 2\n |f'(w)|_2 <= eps*min(pos,neg)/l*|f'(w0)|_2,\n where f is the primal function and pos/neg are # of\n positive/negative data (default 0.01)\n -s 1, 3, 4 and 7\n Dual maximal violation <= eps; similar to libsvm (default 0.1)\n -s 5 and 6\n |f'(w)|_1 <= eps*min(pos,neg)/l*|f'(w0)|_1,\n	where f is the primal function (default 0.01)\n")
            ("err-type", po::value<int>(&ERR_TYPE), " Function used to evaluate the error: \n 1 -- error counting \n  2 -- equal error rate \n 3 -- fixed true positive rate ")
            ("required-tp", po::value<double>(&requiredTP), " required TP if is selected fixedTPerror ")
            ("model", po::value<std::string>(&modelFile), "output model file")
            ("folds", po::value<int>(&kcv), "kcv folds")
            ("single-precision", "the algorithms use single precision floating point instead of double")
            ("mt", "it use multithread algorithm")
            ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        cout << desc << "\n";
        return 1;
    }

    check(fileExists(trainFile), "Cannot open training set file", desc);
    if(vm.count("validation-set")){
        check(fileExists(testFile), "Cannot open validation set file", desc);
    }

    saveAll = vm.count("save-all");

    check(vm.count("c"), "no C specified", desc);
    if(vm.count("roc")){
        check(vm.count("validation-set") || vm.count("folds"), "KCV or validation set is needed to compute a ROC", desc);
    }
    singlePrecision = vm.count("single-precision");

    binary = vm.count("binary");
    if (vm.count("mt")) {
        ThreadManager::getInstance()->setThreadNumber();
    }else{
        ThreadManager::getInstance()->setThreadNumber(1);
    }
    if(vm.count("log")){
        dstream.setStream(new std::ofstream(debugFile.c_str()));
        for(unsigned i = 0; i < argc; i++){
            dstream << argv[i] << " ";
        }
        dstream << "\n";
    }
    dstream << "Kernel " << kernelType << "\n" << "C range: ";
    regs = getRange(cRange);
    if(kernelType == RBF){
        check(vm.count("gamma"), "no gamma specified", desc);
    }else if(kernelType == POLY){
        check(vm.count("degree"), "no degree specified", desc);
    }
    if(vm.count("gamma")){
        check(kernelType == RBF || kernelType == SIGMOID, "gamma can be used only with rbf and sigmoid kernels", desc);
        dstream << "gamma range: ";
        gammas = getRange(gammaRange);
        for(unsigned i = 0; i < regs.size(); i++){
            for(unsigned j = 0; j < gammas.size(); j++){
                std::vector<double> parameter(2);
                parameter[0] = regs[i];
                parameter[1] = gammas[j];
                cs.push_back(parameter);
            }
        }
    }else if (vm.count("degree")){
        check(kernelType == POLY, "gamma can be used only with polynomial kernel", desc);
        degrees = getLinearRange(degreeRange);
        for(unsigned i = 0; i < regs.size(); i++){
            for(unsigned j = 0; j < degrees.size(); j++){
                std::vector<double> parameter(2);
                parameter[0] = regs[i];
                parameter[1] = degrees[j];
                cs.push_back(parameter);
            }
        }
    }else if(kernelType == LINEAR){
        for(unsigned i = 0; i < regs.size(); i++){
            std::vector<double> parameter(1, regs[i]);
            cs.push_back(parameter);
        }
    }
    if(cs.size() > 1){
        check(vm.count("save-all") || vm.count("validation-set") || vm.count("folds"), "Multiple parameter require --save-all or --fold or --validation-set", desc);
    }

    if(binary){
        if(singlePrecision){
            readProblemBinaryLibSVM<float>(prob, trainFile);
        }else{
            readProblemBinaryLibSVM<double>(prob, trainFile);
        }
    }else{
        readProblemASCIILibSVM(prob, trainFile);
    }

    if(vm.count("validation-set")){
        if(binary){
            if(singlePrecision){
                readProblemBinaryLibSVM<float>(val, testFile);
            }else{
                readProblemBinaryLibSVM<double>(val, testFile);
            }
        }else{
            readProblemASCIILibSVM(val, testFile);
        }
    }

    double bestPrecision = -1;
    int bestIndex = -1;
    svm_model* bestModel = 0;

    if(ERR_TYPE == 1){
        LibSVMPool<StandardClassificationAccuracy<double> > MT(cs, prob, val,vm.count("validation-set"),  kcv, epsilon, rocBasePath, saveAll, kernelType, modelFile);
        par_for_each(0, cs.size(), MT);
        bestPrecision = MT.getPrecision();
        bestIndex = MT.getBestIndex();
        bestModel = MT.getBestModel();
    }else if(ERR_TYPE == 2){
        LibSVMPool<EqualErrorRateAccuracy<double> > MT(cs, prob, val,vm.count("validation-set"),  kcv, epsilon, rocBasePath, saveAll, kernelType, modelFile);
        par_for_each(0, cs.size(), MT);
        bestPrecision = MT.getPrecision();
        bestIndex = MT.getBestIndex();
        bestModel = MT.getBestModel();
    }else if(ERR_TYPE == 3){
        check(vm.count("required-tp") , "required true positive rate not set", desc);
        LibSVMPool<FixedTPAccuracy<double> > MT(cs, prob, val,vm.count("validation-set"),  kcv, epsilon, rocBasePath, saveAll, kernelType, modelFile);
        MT.setRequiredTP(requiredTP);
        par_for_each(0, cs.size(), MT);
        bestPrecision = MT.getPrecision();
        bestIndex = MT.getBestIndex();
        bestModel = MT.getBestModel();
    }else{
        throw std::runtime_error("Undefined");
    }

    if(vm.count("validation-set") || kcv > 1){
        cout << "Precision: " << bestPrecision << std::endl << "C: " << cs.at(bestIndex)[0];
        if( cs.at(bestIndex).size() > 1){
            cout << " kernel par: " << cs.at(bestIndex)[1];
        }
        cout << std::endl;
    }
    if(bestIndex >= 0 && modelFile != ""){
        svm_model* mod = bestModel;
        if(mod == 0){
            assert(kcv > 0);
            std::cout << "Recomputing model with the selected parameters..." << std::endl;
            struct svm_parameter par = buildPar(kernelType, cs.at(bestIndex)[1], epsilon, cs.at(bestIndex)[0]) ;
            const char* err = svm_check_parameter(&prob, &par);
            if(err != 0){
                check(false, err, desc);
            }
            mod = svm_train(&prob, &par);
        }
        std::cout << "Saving final model..." << std::endl;
        svm_save_model(modelFile.c_str(), mod);
        svm_free_and_destroy_model(&mod);
    }
    free_svm_problem(prob);
    if(vm.count("validation-set")){
        free_svm_problem(val);
    }
    std::cout << "Done" << std::endl;
    return 0;
}


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (C) 2021 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */
#include "opendrive/geometry/LaneSectionSampling.hpp"
#include <boost/math/tools/rational.hpp>
#include <spdlog/sinks/stdout_color_sinks.h>
#include <spdlog/spdlog.h>
#include "opendrive/geometry/LaneUtils.hpp"

namespace opendrive {
namespace geometry {

double laneWidth(LaneWidthSet const &laneWidthSet, double s)
{
  return evalPoly3(laneWidthSet, s);
}

double laneSuperelevation(LateralProfileSuperelevationSet const &superelevationProfiles, double s)
{
  return evalPoly3(superelevationProfiles, s);
}

double laneShapeHeightEval(std::pair<std::map<double, std::set<LateralProfileShape>>::const_iterator,
                                     std::map<double, std::set<LateralProfileShape>>::const_iterator> bounds,
                           double s,
                           double t)
{
  auto const sr1 = bounds.first->first;
  auto const h_sr1 = evalPoly3(bounds.first->second, t);
  auto const sr2 = bounds.second->first;
  auto const h_sr2 = evalPoly3(bounds.second->second, t);
  auto h_s = (h_sr1 + h_sr2) * (s - sr1) / (sr2 - sr1);
  return h_s;
}

std::pair<std::map<double, std::set<LateralProfileShape>>::const_iterator,
          std::map<double, std::set<LateralProfileShape>>::const_iterator>
laneShapeHeightBounds(std::map<double, std::set<LateralProfileShape>> const &shapeProfile, double s)
{
  auto upperBound = shapeProfile.upper_bound(s);
  auto lowerBound = upperBound;
  if (lowerBound != shapeProfile.begin())
  {
    lowerBound--;
  }
  return {lowerBound, upperBound};
}

void normalizeEdge(Id const id, std::string const &what, Edge &edge)
{
  if (edge.size() <= 2u)
  {
    // if edge only consists of 2 points we could only remove the whole edge in case the points are too near to each
    // other
    return;
  }
  std::size_t pointsToDrop = 0u;
  Point previousEdgeDir(0., 0., 0.);
  for (std::size_t i = 1u; i < edge.size(); ++i)
  {
    // i-pointsToDrop > 0
    if (pointsToDrop > 0u)
    {
      edge[i - pointsToDrop] = edge[i];
    }
    // last point must not be dropped
    if (i < edge.size() - 1)
    {
      // eliminate z-component from normalization
      auto from = edge[i - pointsToDrop];
      from.z = 0;
      auto to = edge[i - pointsToDrop - 1];
      to.z = 0;
      if (from == to)
      {
        // drop identical points
        spdlog::trace("normalizeEdge {} dropping identical point from {} edge at index {}", id, what, i);
        pointsToDrop++;
      }
      else
      {
        //  moving the middle line in a narrow curve often leads to artifacts (circles) which are removed by the below
        auto nextEdgeDir = from - to;
        if (previousEdgeDir != Point(0., 0., 0.))
        {
          auto dotProduct = previousEdgeDir.dot(nextEdgeDir);
          if (dotProduct < 0.)
          {
            spdlog::trace("normalizeEdge {} extreme direction changing point from {} edge at index {}", id, what, i);
            pointsToDrop++;
          }
          else
          {
            previousEdgeDir = nextEdgeDir;
          }
        }
        else
        {
          previousEdgeDir = nextEdgeDir;
        }
      }
    }
  }
  if (pointsToDrop > 0u)
  {
    std::size_t const newEdgeSize = std::max(std::size_t(2u), edge.size() - pointsToDrop);
    spdlog::trace("normalizeEdge {} dropping points from {} edge {} remaining {}", id, what, pointsToDrop, newEdgeSize);
    edge.resize(newEdgeSize, Point(0., 0., 0.));
  }
}

void LaneSectionSampling::generateSamples()
{
  auto samplingPoints = centerLine.samplingPoints();

  for (uint64_t i = 0u; i < roadInfo.lanes.lane_sections.size(); i++)
  {
    uint64_t laneSectionIndex = i + 1u;
    addSamples(roadInfo.lanes.lane_sections[i], laneSectionIndex, samplingPoints);
  }
}

bool LaneSectionSampling::addSamples(LaneSection const &laneSection,
                                     uint64_t const &laneSectionIndex,
                                     std::list<double> const &laneSamplingPoints)
{
  bool ok = true;
  double s0 = laneSection.start_position;
  double s1 = laneSection.end_position;
  if (std::fabs(s1 - laneSamplingPoints.back()) > Point::cCoordinateTolerance)
  {
    // the tolerance at the end of the lane sections is necessary so that the lane offset applies only to the current
    // lane section
    s1 -= 1e-12;
  }

  if (s1 < s0)
  {
    spdlog::error("Invalid lane section length {} {}", s1, s0);
    ok = false;
  }

  auto inRange = [&s0, &s1](double const &s) { return (s < s1) && (s > s0); };

  std::list<double> localSamplingPoints;
  localSamplingPoints.push_back(s0);
  std::copy_if(laneSamplingPoints.begin(), laneSamplingPoints.end(), std::back_inserter(localSamplingPoints), inRange);
  localSamplingPoints.push_back(s1);

  for (auto localSamplingPoint : localSamplingPoints)
  {
    auto profile = evalProfile(laneSection, laneSectionIndex, localSamplingPoint);
    profiles.push_back(profile);
  }

  // now refine sampling points according to actual points influenced by laneWidth changes and lateral_profile
  auto iter = profiles.begin();
  while (iter != profiles.end())
  {
    auto nextIter = iter;
    nextIter++;
    if (nextIter == profiles.end())
    {
      break;
    }
    if (nextIter->sLane - iter->sLane < Geometry::cMaxSamplingError)
    {
      // sampling below the Geometry::cMaxSamplingError longitudinal distance makes no sense
      ++iter;
      continue;
    }
    auto const centerLinearInterpolated = interpolateProfile(*iter, *nextIter);
    auto centerActual = evalProfile(laneSection, laneSectionIndex, centerLinearInterpolated.sLane);
    centerActual.dropPointsWithAcceptedError(centerLinearInterpolated);
    if (!centerActual.laneBorderPoints.empty())
    {
      profiles.insert(nextIter, centerActual);
    }
    else
    {
      iter++;
    }
  }

  return ok;
}

LaneSectionSampling::LaneSectionProfile
LaneSectionSampling::evalProfile(LaneSection const &laneSection, uint64_t const &laneSectionIndex, double const &s)
{
  LaneSectionProfile profile;
  profile.sLane = s;
  auto const sLaneSection = s - laneSection.start_position;
  auto const centerPoint = centerLine.eval(s);

  LaneHeight laneHeightSearch;
  laneHeightSearch.start_offset = sLaneSection;

  auto const laneShapeBounds = laneShapeHeightBounds(roadInfo.road_profiles.lateral_profile_shape, s);
  bool const laneShapeBoundsAvailable
    = (laneShapeBounds.first != roadInfo.road_profiles.lateral_profile_shape.end()
       && laneShapeBounds.second != roadInfo.road_profiles.lateral_profile_shape.end());

  auto const angle = laneSuperelevation(roadInfo.road_profiles.lateral_profile_superelevation, s);
  auto const cos_angle = cos(angle);
  auto const sin_angle = sin(angle);

  double t = 0.;
  // store the shift of the outer superelevation operation to be applied to lanes kept at level
  Point superelevationShiftOuterPoint;
  for (auto &laneInfo : laneSection.left)
  {
    auto width = laneWidth(laneInfo.lane_width, sLaneSection);
    LaneBorderPoint point;
    auto const rightT = t;
    auto const leftT = t + width;

    // apply lane width
    point.rightEdgePoint = centerPoint.getLateralOffsetPoint(rightT);
    point.leftEdgePoint = centerPoint.getLateralOffsetPoint(leftT);

    // apply lane height
    auto laneHeightIter = laneInfo.lane_height.lower_bound(laneHeightSearch);
    if (laneHeightIter != laneInfo.lane_height.end())
    {
      point.rightEdgePoint.z += laneHeightIter->inner;
      point.leftEdgePoint.z += laneHeightIter->outer;
    }
    if (laneInfo.attributes.level)
    {
      // keep on level: no shape, superelevation level of last outer lane
      point.rightEdgePoint = point.rightEdgePoint + superelevationShiftOuterPoint;
      point.leftEdgePoint = point.leftEdgePoint + superelevationShiftOuterPoint;
    }
    else
    {
      if (laneShapeBoundsAvailable)
      {
        point.rightEdgePoint.z += laneShapeHeightEval(laneShapeBounds, s, rightT);
        point.leftEdgePoint.z += laneShapeHeightEval(laneShapeBounds, s, leftT);
      }

      point.rightEdgePoint
        = calculateSuperelevatedPoint(point.rightEdgePoint, rightT, centerPoint, cos_angle, sin_angle);
      auto const outerPoint
        = calculateSuperelevatedPoint(point.leftEdgePoint, leftT, centerPoint, cos_angle, sin_angle);
      superelevationShiftOuterPoint = outerPoint - point.leftEdgePoint;
      point.leftEdgePoint = outerPoint;
    }
    auto const id = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
    profile.laneBorderPoints[id] = point;
    t += width;
  }

  // right lanes
  t = 0.;
  superelevationShiftOuterPoint = Point();
  for (auto &laneInfo : laneSection.right)
  {
    auto width = laneWidth(laneInfo.lane_width, sLaneSection);
    LaneBorderPoint point;
    const auto leftT = t;
    const auto rightT = t - width;

    // apply lane width
    point.leftEdgePoint = centerPoint.getLateralOffsetPoint(leftT);
    point.rightEdgePoint = centerPoint.getLateralOffsetPoint(rightT);

    // apply lane height
    auto laneHeightIter = laneInfo.lane_height.lower_bound(laneHeightSearch);
    if (laneHeightIter != laneInfo.lane_height.end())
    {
      point.leftEdgePoint.z += laneHeightIter->inner;
      point.rightEdgePoint.z += laneHeightIter->outer;
    }

    if (laneInfo.attributes.level)
    {
      // keep on level: no shape, superelevation level of last outer lane
      point.leftEdgePoint = point.leftEdgePoint + superelevationShiftOuterPoint;
      point.rightEdgePoint = point.rightEdgePoint + superelevationShiftOuterPoint;
    }
    else
    {
      if (laneShapeBoundsAvailable)
      {
        point.leftEdgePoint.z += laneShapeHeightEval(laneShapeBounds, s, leftT);
        point.rightEdgePoint.z += laneShapeHeightEval(laneShapeBounds, s, rightT);
      }

      point.leftEdgePoint = calculateSuperelevatedPoint(point.leftEdgePoint, leftT, centerPoint, cos_angle, sin_angle);
      auto const outerPoint
        = calculateSuperelevatedPoint(point.rightEdgePoint, rightT, centerPoint, cos_angle, sin_angle);
      superelevationShiftOuterPoint = outerPoint - point.rightEdgePoint;
      point.rightEdgePoint = outerPoint;
    }

    auto const id = laneId(roadInfo.attributes.id, laneSectionIndex, laneInfo.attributes.id);
    profile.laneBorderPoints[id] = point;
    t -= width;
  }

  return profile;
}

Point LaneSectionSampling::calculateSuperelevatedPoint(
  Point const &point, double t, DirectedPoint const &centerPoint, double const cos_angle, double const sin_angle)
{
  Point resultPoint;
  resultPoint.x = centerPoint.location.x + cos_angle * (point.x - centerPoint.location.x);
  resultPoint.y = centerPoint.location.y + cos_angle * (point.y - centerPoint.location.y);
  resultPoint.z = point.z + sin_angle * t;
  return resultPoint;
}

LaneSectionSampling::LaneSectionProfile LaneSectionSampling::interpolateProfile(LaneSectionProfile const &start,
                                                                                LaneSectionProfile const &end)
{
  LaneSectionProfile profile;
  profile.sLane = 0.5 * (start.sLane + end.sLane);
  for (auto const &startLaneBorderPoint : start.laneBorderPoints)
  {
    auto const findEnd = end.laneBorderPoints.find(startLaneBorderPoint.first);
    if (findEnd != end.laneBorderPoints.end())
    {
      LaneBorderPoint point;
      point.rightEdgePoint = 0.5 * (startLaneBorderPoint.second.rightEdgePoint + findEnd->second.rightEdgePoint);
      point.leftEdgePoint = 0.5 * (startLaneBorderPoint.second.leftEdgePoint + findEnd->second.leftEdgePoint);
      profile.laneBorderPoints[startLaneBorderPoint.first] = point;
    }
  }

  return profile;
}

void LaneSectionSampling::LaneSectionProfile::dropPointsWithAcceptedError(
  LaneSectionSampling::LaneSectionProfile const &other)
{
  auto const maxErrorSquared = Geometry::cMaxSamplingError * Geometry::cMaxSamplingError;
  auto iter = laneBorderPoints.begin();
  while (iter != laneBorderPoints.end())
  {
    auto const findOther = other.laneBorderPoints.find(iter->first);
    if (findOther != other.laneBorderPoints.end())
    {
      auto errorSquaredLeft = (iter->second.leftEdgePoint - findOther->second.leftEdgePoint).normSquared();
      auto errorSquaredRight = (iter->second.rightEdgePoint - findOther->second.rightEdgePoint).normSquared();
      if ((errorSquaredLeft >= maxErrorSquared) || (errorSquaredRight >= maxErrorSquared))
      {
        // error too big, keep the point
        ++iter;
        continue;
      }
    }
    // either error acceptable or
    // the lane was already identified as acceptable before and is therefore not present anymore in the interpolated
    // other
    iter = laneBorderPoints.erase(iter);
  }
}

void LaneSectionSampling::writeLaneMap(LaneMap &laneMap)
{
  std::set<Id> lanesToProcess;
  for (auto const &profile : profiles)
  {
    for (auto const &laneBorderPoint : profile.laneBorderPoints)
    {
      laneMap[laneBorderPoint.first].leftEdge.push_back(laneBorderPoint.second.leftEdgePoint);
      laneMap[laneBorderPoint.first].rightEdge.push_back(laneBorderPoint.second.rightEdgePoint);
      lanesToProcess.insert(laneBorderPoint.first);
    }
  }

  for (auto &lane : laneMap)
  {
    if (lanesToProcess.count(lane.first) > 0u)
    {
      normalizeEdge(lane.first, "left", lane.second.leftEdge);
      normalizeEdge(lane.first, "right", lane.second.rightEdge);
    }
  }
}

} // namespace geometry
} // namespace opendrive


//==============================================================================
//         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 NT2_EXPONENTIAL_FUNCTIONS_SCALAR_IMPL_EXPO_EXPO_BASE_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_IMPL_EXPO_EXPO_BASE_HPP_INCLUDED

#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/exponential/functions/scalar/impl/selection_tags.hpp>
#include <nt2/exponential/functions/scalar/impl/expo/f_expo_reduction.hpp>
#include <nt2/exponential/functions/scalar/impl/expo/d_expo_reduction.hpp>
#include <nt2/exponential/functions/scalar/impl/expo/expo_scale.hpp>
#include <boost/simd/sdk/config.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif

namespace nt2 { namespace details
{
  template < class A0,
             class Tag,
             class Style ,
             class Speed_Tag = fast_tag,
             class base_A0 = typename meta::scalar_of<A0>::type>
  struct exponential{};

  template < class A0, class Tag,  class Speed_Tag>
  struct exponential< A0, Tag, tag::not_simd_type, Speed_Tag, double>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax) where a is 1, 2 or ten depending on Tag
    static inline A0 expa(const A0& a0)
    {
      if (reduc_t::isgemaxlog(a0)) return Inf<A0>();
      if (reduc_t::isleminlog(a0)) return Zero<A0>();
      #ifndef BOOST_SIMD_NO_INVALIDS
        if (is_nan(a0)) return a0;
      #endif
      A0 hi, lo, x;
      A0 k = reduc_t::reduce(a0, hi, lo, x);
      A0 c = reduc_t::approx(x);
      c = reduc_t::finalize(x, c, hi, lo);
      return  scale(c, k);
    }
  };

  template < class A0, class Tag,  class Speed_Tag>
  struct exponential< A0, Tag, tag::not_simd_type, Speed_Tag, float>
  {
    typedef exp_reduction<A0,Tag>                        reduc_t;
    // compute exp(ax) where a is 1, 2 or ten depending on Tag
    static inline A0 expa(const A0& a0)
    {
      if (reduc_t::isgemaxlog(a0)) return Inf<A0>();
      if (reduc_t::isleminlog(a0)) return Zero<A0>();
      #ifndef BOOST_SIMD_NO_INVALIDS
        if (is_nan(a0)) return a0;
      #endif
      A0 x;
      A0 k = reduc_t::reduce(a0, x);
      x = reduc_t::approx(x);
      return scale(x, k);
    }
  };
} }

#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#include "DeNoising.h"

#include <opencv2\core.hpp>

#include "Utils\Logging.h"
#include <arcana\threading\parallel.h>

#include <unordered_map>
#include <tuple>

// TODO:  feed back to Boost that 4457 is firing
#pragma warning (push)
#pragma warning (disable:4457)
#include <boost/geometry/strategies/strategies.hpp>
#include <boost\geometry\index\rtree.hpp>
#pragma warning (pop)

#include <tuple>

namespace geo = boost::geometry;

namespace mage
{
    namespace clouds
    {
        using rtree_point = geo::model::point<float, 3, geo::cs::cartesian>;
        using rtree_value = std::ptrdiff_t;

        struct indexable
        {
            using result_type = rtree_point;

            static_assert(sizeof(rtree_point) == sizeof(cv::Point3f), "types should be identical");

            const rtree_point& operator()(const rtree_value& idx) const
            {
                return reinterpret_cast<const rtree_point&>(points[idx]);
            }

            gsl::span<const cv::Point3f> points;
        };

        using rtree = geo::index::rtree<rtree_value, geo::index::rstar<12>, indexable>;

        struct Knn::Impl
        {
            rtree Tree;
        };

        Knn::Knn(gsl::span<const cv::Vec3f> points)
            : Knn({ reinterpret_cast<const cv::Point3f*>(points.data()), points.size() })
        {
            static_assert(sizeof(cv::Vec3f) == sizeof(cv::Point3f), "types should have identical layouts");
        }

        Knn::Knn(gsl::span<const cv::Point3f> points)
        {
            std::vector<size_t> indexes(points.size());
            std::iota(indexes.begin(), indexes.end(), 0);

            m_impl = std::make_unique<Impl>(Impl{ rtree{ indexes.begin(), indexes.end(), rtree::parameters_type{}, indexable{ points } } });
        }

        Knn& Knn::operator=(Knn&& other)
        {
            m_impl = std::move(other.m_impl);
            return *this;
        }

        Knn::~Knn()
        {}

        void Knn::Query(const cv::Vec3f& vi, const size_t N, std::vector<size_t>& indexes) const
        {
            indexes.resize(N);
            size_t count = m_impl->Tree.query(boost::geometry::index::nearest(rtree_point{ vi[0], vi[1], vi[2] }, gsl::narrow_cast<unsigned int>(N)), indexes.begin());
            indexes.resize(count);
        }

        cv::Vec3f Averages(gsl::span<const cv::Vec3f> elements)
        {
            cv::Vec3f average = std::accumulate(elements.begin(), elements.end(), cv::Vec3f{ 0, 0, 0 }) / gsl::narrow_cast<int>(elements.size());
            return average;
        }

        cv::Vec3f Variances(gsl::span<const cv::Vec3f> elements)
        {
            auto avg = Averages(elements);

            cv::Vec3f variances{ 0, 0, 0 };

            for (ptrdiff_t i = 0; i < elements.size(); ++i)
            {
                cv::Vec3f del = elements[i] - avg;

                for(int c = 0; c < 3; ++c)
                    variances[c] += del[c] * del[c];
            }

            return variances / gsl::narrow_cast<int>(elements.size());
        }

        cv::Vec3f Medians(gsl::span<const cv::Vec3f> elements)
        {
            cv::Vec3f medians;

            std::vector<float> componentValues(elements.size());

            for (int c = 0; c < 3; ++c)
            {
                std::transform(elements.begin(), elements.end(), componentValues.begin(), [c](const auto& pt) { return pt(c); });

                auto nth = componentValues.begin() + componentValues.size() / 2;

                std::nth_element(componentValues.begin(), nth, componentValues.end());

                medians[c] = *nth;
            }

            return medians;
        }

        void ComputeCharacteristics(
            gsl::span<const cv::Point3f> points,
            std::vector<cv::Vec3f>& normals,
            gsl::span<const size_t> mapIds,
            std::vector<float>& effectiveDissimilarities,
            std::vector<float>& distanceScores,
            std::vector<float>& homogeneityScores,
            const size_t maxN)
        {
            if (normals.size() != (size_t)points.size())
            {
                normals.clear();
                normals.resize(points.size());
            }

            effectiveDissimilarities.clear();
            effectiveDissimilarities.resize(points.size());

            distanceScores.clear();
            distanceScores.resize(points.size());

            homogeneityScores.clear();
            homogeneityScores.resize(points.size());

            const size_t N = std::min(maxN, (size_t)points.size());

            const Knn knn{ points };

            // compute all the normals
            mira::parallel(points.size(), 8, [&points, &normals, &knn, indexes = std::vector<size_t>{}, Ni = std::vector<cv::Vec3f>{}, perps = std::vector<cv::Vec3f>{}, N](size_t i) mutable
            {
                const cv::Vec3f vi = points[i];

                knn.Query(vi, N + 1, indexes);
                indexes.erase(std::find(indexes.begin(), indexes.end(), i));

                Ni.resize(indexes.size());
                std::transform(indexes.begin(), indexes.end(), Ni.begin(), [points](auto idx) { return points[idx]; });

                const cv::Vec3f mu = Medians(Ni);

                perps.resize(Ni.size());
                std::transform(Ni.begin(), Ni.end(), perps.begin(), [vi, mu](const cv::Vec3f& vj)
                {
                    float invWij = sqrt(cv::normL2Sqr(vi.val, vj.val, 3));
                    if (invWij < 0.0001f) invWij = 1; // TODO what's a good ratio for an identical point?

                    return (1 / invWij) * (cv::Vec3f{ vj } - mu);
                });

                const cv::Mat pcaInputMat(gsl::narrow_cast<int>(perps.size()), 3, CV_32FC1, perps.data());

                const cv::PCA pca{ pcaInputMat, cv::Vec3f::zeros().t(), cv::PCA::Flags::DATA_AS_ROW };

                const auto minel = std::min_element(pca.eigenvalues.begin<float>(), pca.eigenvalues.end<float>());
                const auto index = std::distance(pca.eigenvalues.begin<float>(), minel);

                cv::Vec3f normal{ &pca.eigenvectors.at<float>(gsl::narrow_cast<int>(index), 0) };

                if (normal.dot(normals[i]) < 0)
                {
                    normal *= -1;
                }

                normals[i] = normal;
            });

            mira::parallel(points.size(), 8, [&points, &normals, &knn, &mapIds, &homogeneityScores, &effectiveDissimilarities, &distanceScores, indexes = std::vector<size_t>{}, distances = std::vector<float>{}, N](size_t i) mutable
            {
                const cv::Vec3f vi = points[i];

                knn.Query(vi, N + 1, indexes);
                indexes.erase(std::find(indexes.begin(), indexes.end(), i));

                // compute homogeneity score
                if (!mapIds.empty())
                {
                    size_t same = std::count_if(indexes.begin(), indexes.end(), [&](const size_t j)
                    {
                        return mapIds[i] == mapIds[j];
                    });
                    homogeneityScores[i] = same / (float)N;
                }

                // compute the effective dissimilarites for the point
                {
                    float effectiveDissimilarity = 0.0f;

                    for (size_t k = 0; k < indexes.size(); ++k)
                    {
                        const cv::Point3f& vk = points[indexes[k]];
                        const cv::Vec3f& nk = normals[indexes[k]];

                        const cv::Vec3f vik = vi - cv::Vec3f{ vk };

                        const cv::Vec3f vikParallel = nk * vik.dot(nk);
                        const cv::Vec3f vikPerp = vik - vikParallel;

                        const float dissimilarity = cv::normL2Sqr<float, float>(vikParallel.val, 3) / (cv::normL2Sqr<float, float>(vikPerp.val, 3) + 0.0001f);
                        effectiveDissimilarity += dissimilarity;
                    }

                    effectiveDissimilarities[i] = effectiveDissimilarity / indexes.size();
                }

                // compute the distance based outlier detection
                {
                    distances.resize(indexes.size());
                    std::transform(indexes.begin(), indexes.end(), distances.begin(), [vi, points](const size_t& k)
                    {
                        return cv::normL2Sqr<float, float>(vi.val, &points[k].x, 3);
                    });

                    const auto nth = distances.begin() + distances.size() / 2;

                    std::nth_element(distances.begin(), nth, distances.end());

                    distanceScores[i] = sqrt(*nth);
                }
            });
        }

        void MollifyNormals(
            gsl::span<const cv::Point3f> points,
            gsl::span<cv::Vec3f> normals,
            const float normalScale,
            const float spatialScale,
            const size_t iterations,
            const size_t maxN)
        {
            std::vector<size_t> indexes(points.size());
            std::iota(indexes.begin(), indexes.end(), 0);

            size_t N = std::min(maxN, (size_t)points.size());

            Knn knn{ points };

            std::vector<cv::Vec3f> mollified{ normals.begin(), normals.end() };

            for (size_t iteration = 0; iteration < iterations; ++iteration)
            {
                for (ptrdiff_t i = 0; i < points.size(); ++i)
                {
                    knn.Query(points[i], N + 1, indexes);
                    indexes.erase(find(indexes.begin(), indexes.end(), static_cast<size_t>(i)));

                    cv::Vec3f vi = points[i];
                    cv::Vec3f ni = normals[i];

                    cv::Vec3f summed = ni;
                    for (size_t j : indexes)
                    {
                        cv::Vec3f vj = points[j];
                        cv::Vec3f nj = normals[j];

                        float norms = cv::normL2Sqr<float, float>(ni.val, nj.val, 3) / (normalScale * normalScale);
                        float spats = cv::normL2Sqr<float, float>(vi.val, vj.val, 3) / (spatialScale * spatialScale);

                        float Oij = std::expf(-(norms + spats));
                        summed += Oij * nj;
                    }

                    mollified[i] = cv::normalize(summed);
                }

                for (ptrdiff_t i = 0; i < points.size(); ++i)
                    normals[i] = mollified[i];
            }
        }

        void MollifyNormalsInternal(gsl::span<const cv::Vec3f> points, gsl::span<cv::Vec3f> inOutNormals, gsl::span<const std::vector<size_t>> neighbourhoods, const float normalScale, const float spatialScale)
        {
            const std::vector<cv::Vec3f> normals{ inOutNormals.begin(), inOutNormals.end() };

            for (ptrdiff_t i = 0; i < points.size(); ++i)
            {
                const cv::Vec3f& vi = points[i];
                const cv::Vec3f& ni = normals[i];

                cv::Vec3f summed = ni;
                for (size_t j : neighbourhoods[i])
                {
                    const cv::Vec3f& vj = points[j];
                    const cv::Vec3f& nj = normals[j];

                    float norms = cv::normL2Sqr<float, float>(ni.val, nj.val, 3) / normalScale;
                    float spats = cv::normL2Sqr<float, float>(vi.val, vj.val, 3) / spatialScale;

                    float Oij = std::expf(-(norms + spats));
                    summed += Oij * nj;
                }

                inOutNormals[i] = cv::normalize(summed);
            }
        }

        cv::Vec3f LambdaSumDerivative(const cv::Vec3f& original, const cv::Vec3f& modified, float lambda)
        {
            return 2 * lambda * (modified - original);
        };

        float Tau(const cv::Vec3f& vi, const cv::Vec3f& vj, float sigmaSqr)
        {
            float norm2 = cv::normL2Sqr<float, float>(vi.val, vj.val, 3);
            return std::exp(-norm2 / sigmaSqr);
        };

        cv::Vec3f TauDerivedOverVi(const cv::Vec3f& vi, const cv::Vec3f& vj, float sigmaSqr)
        {
            float norm2 = cv::normL2Sqr<float, float>(vi.val, vj.val, 3);
            return (2 / sigmaSqr) * (vj - vi) * std::exp(-norm2 / sigmaSqr);
        };

        cv::Vec3f TauDerivedOverVj(const cv::Vec3f& vi, const cv::Vec3f& vj, float sigmaSqr)
        {
            float norm2 = cv::normL2Sqr<float, float>(vi.val, vj.val, 3);

            //when differentiated by j the sign is flipped
            return -(2 / sigmaSqr) * (vj - vi) * std::exp(-norm2 / sigmaSqr);
        };

        struct
        {
            std::vector<float> SumTijs;
            std::vector<cv::Vec3f> SumTijDis;
            std::vector<cv::Vec3f> SumGammaNormalDerivedOverVi;
        } caches;

        float Gamma(const size_t& vid, const cv::Vec3f& vi, const cv::Vec3f& vj, float sigmaSqr)
        {
            float tij = Tau(vi, vj, sigmaSqr);

            float sumtij = caches.SumTijs[vid];

            return tij / sumtij;
        };

        cv::Vec3f GammaDerivedOverVi(const size_t& vid, const cv::Vec3f& vi, const cv::Vec3f& vj, float sigmaSqr)
        {
            cv::Vec3f tijdi = TauDerivedOverVi(vi, vj, sigmaSqr);
            float tij = Tau(vi, vj, sigmaSqr);

            float sumtij = caches.SumTijs[vid];

            cv::Vec3f sumtijdi = caches.SumTijDis[vid];

            return (tijdi * sumtij - tij * sumtijdi) / (sumtij * sumtij);
        };

        cv::Vec3f GammaDerivedOverVj(const size_t& vid, const cv::Vec3f& vi, const cv::Vec3f& vj, float sigmaSqr)
        {
            cv::Vec3f tijdj = TauDerivedOverVj(vi, vj, sigmaSqr);

            float tij = Tau(vi, vj, sigmaSqr);

            float sumtij = caches.SumTijs[vid];

            return (tijdj * sumtij - tij * tijdj) / (sumtij * sumtij);
        };

        cv::Vec3f GammaDerivedOverVjn(const size_t& vid, const cv::Vec3f& vi, const cv::Vec3f& vj, const cv::Vec3f& vjn, float sigmaSqr)
        {
            float tij = Tau(vi, vj, sigmaSqr);
            cv::Vec3f dtijn = TauDerivedOverVj(vi, vjn, sigmaSqr);

            float sumtij = caches.SumTijs[vid];

            return (-tij / (sumtij * sumtij)) * dtijn;
        };

        float GammaNormal(const size_t& id, const cv::Vec3f& vi, const cv::Vec3f& ni, const cv::Vec3f& vj, float sigmaSqr)
        {
            float gam = Gamma(id, vi, vj, sigmaSqr);
            float dot = ni.dot(vi - vj);
            return gam * dot * dot; // TODO Not certain if the dot should be squared or not
        };

        cv::Vec3f GammaNormalDerivedOverVi(const size_t& id, const cv::Vec3f& vi, const cv::Vec3f& ni, const cv::Vec3f& vj, float sigmaSqr)
        {
            float gam = Gamma(id, vi, vj, sigmaSqr);
            cv::Vec3f gamdix = GammaDerivedOverVi(id, vi, vj, sigmaSqr);

            float dot = ni.dot(vi - vj);
            return 2 * gam * ni * dot + gamdix * dot * dot;
        };

        cv::Vec3f GammaNormalDerivedOverVj(const size_t& id, const cv::Vec3f& vi, const cv::Vec3f& ni, const cv::Vec3f& vj, float sigmaSqr)
        {
            float gam = Gamma(id, vi, vj, sigmaSqr);
            cv::Vec3f gamdjx = GammaDerivedOverVj(id, vi, vj, sigmaSqr);

            float dot = ni.dot(vi - vj);
            return -2 * gam * ni * dot + gamdjx * dot * dot;
        };

        cv::Vec3f GammaNormalDerivedOverVjn(const size_t& id, const cv::Vec3f& vi, const cv::Vec3f& ni, const cv::Vec3f& vj, const cv::Vec3f& vjn, float sigmaSqr)
        {
            cv::Vec3f gamdjnx = GammaDerivedOverVjn(id, vi, vj, vjn, sigmaSqr);

            float dot = ni.dot(vi - vj);
            return gamdjnx * dot * dot;
        };

        cv::Vec3f GammaNormalsDerivedOverVi(
            gsl::span<const cv::Vec3f> points,
            gsl::span<const cv::Vec3f> normals,
            gsl::span<const std::vector<size_t>> neighbourhoods,
            const Knn& knn,
            const cv::Vec3f& realVi,
            size_t realViIndex,
            float sigmaSqr,
            const size_t N)
        {
            cv::Vec3f sum{ 0, 0, 0 };

            std::vector<size_t> searched;
            knn.Query(realVi, 2 * N, searched);

            for (size_t i : searched)
            {
                if (i == realViIndex)
                {
                    sum += caches.SumGammaNormalDerivedOverVi[i];
                }
                else
                {
                    auto& vi = points[i];
                    auto& ni = normals[i];
                    auto& neighborhood = neighbourhoods[i];

                    auto realVj = find_if(neighborhood.begin(), neighborhood.end(), [&](size_t ni) { return realViIndex == ni; });
                    if (realVj != neighborhood.end())
                    {
                        for (size_t j : neighborhood)
                        {
                            if (j == realViIndex)
                            {
                                sum += GammaNormalDerivedOverVj(i, vi, ni, points[j], sigmaSqr);
                            }
                            else
                            {
                                auto realIInNJ = find_if(neighbourhoods[j].begin(), neighbourhoods[j].end(), [&](size_t ni) { return realViIndex == ni; });
                                if (realIInNJ != neighbourhoods[j].end())
                                {
                                    sum += GammaNormalDerivedOverVjn(i, vi, ni, points[j], realVi, sigmaSqr);
                                }
                            }
                        }
                    }
                }
            }

            return sum;
        };

        std::vector<std::vector<size_t>> InternalPrimeCaches(gsl::span<cv::Point3f> points, gsl::span<cv::Vec3f> normals, const float sigmaSqr, const size_t maxN)
        {
            const size_t N = std::min(maxN, (size_t)points.size());

            Knn knn{ points };
            std::vector<std::vector<size_t>> neighbourhoods(points.size());

            mira::parallel(points.size(), 8, [&neighbourhoods, &knn, points, N](size_t i)
            {
                auto& neighbourhood = neighbourhoods[i];

                knn.Query(points[i], N + 1, neighbourhood);
                neighbourhood.erase(find(neighbourhood.begin(), neighbourhood.end(), i));
            });

            caches.SumTijs.clear();
            caches.SumTijs.resize(points.size());

            caches.SumTijDis.clear();
            caches.SumTijDis.resize(points.size());

            caches.SumGammaNormalDerivedOverVi.clear();
            caches.SumGammaNormalDerivedOverVi.resize(points.size());

            mira::parallel(points.size(), 8, [&neighbourhoods, points, sigmaSqr, normals](size_t i)
            {
                float sumtij = 0;
                cv::Vec3f sumtijdi{ 0, 0, 0 };
                for (size_t j : neighbourhoods[i])
                {
                    sumtij += Tau(points[i], points[j], sigmaSqr);
                    sumtijdi += TauDerivedOverVi(points[i], points[j], sigmaSqr);
                }
                caches.SumTijs[i] = abs(sumtij * sumtij) < std::numeric_limits<float>::epsilon() ? (std::signbit(sumtij) ? -1 : 1) : sumtij;
                caches.SumTijDis[i] = sumtijdi;

                cv::Vec3f sumgammanormalderivedovervi{ 0, 0, 0 };
                for (size_t j : neighbourhoods[i])
                {
                    cv::Vec3f val = GammaNormalDerivedOverVi(i, points[i], normals[i], points[j], sigmaSqr);
                    assert(val == val);
                    sumgammanormalderivedovervi += val;
                }
                caches.SumGammaNormalDerivedOverVi[i] = sumgammanormalderivedovervi;
            });

            return neighbourhoods;
        }

        void RepositionPointSets(
            gsl::span<cv::Point3f> toReposition,
            gsl::span<cv::Vec3f> normals,
            const float stepi,
            const size_t iterations,
            const float lambda,
            const float solverSigmaS,
            const float mollSigmaS,
            const float mollSigmaN,
            const size_t maxN)
        {
            std::vector<size_t> indexes;

            const size_t N = std::min(maxN, (size_t)toReposition.size());

            std::vector<cv::Vec3f> initialPoints{ toReposition.begin(), toReposition.end() };
            std::vector<cv::Vec3f> points = initialPoints;
            std::vector<cv::Vec3f> gradients(points.size());

            std::vector<std::vector<size_t>> neighbourhoods(points.size());

            for (size_t iteration = 0; iteration < iterations; ++iteration)
            {
                LogMessage(L"Starting Iteration");

                LogMessage(L"Computing Neighbourhoods");

                Knn knn{ points };

                mira::parallel(points.size(), 8, [&neighbourhoods, &knn, &points, N](size_t i)
                {
                    auto& neighbourhood = neighbourhoods[i];

                    knn.Query(points[i], N + 1, neighbourhood);
                    auto self = find(neighbourhood.begin(), neighbourhood.end(), i);
                    if (self != neighbourhood.end())
                    {
                        neighbourhood.erase(self);
                    }
                });

                LogMessage(L"Mollifying Normals");

                MollifyNormalsInternal(points, normals, neighbourhoods, mollSigmaN * mollSigmaN, mollSigmaS * mollSigmaN);

                LogMessage(L"Computing Neighborhood Sums");

                caches.SumTijs.clear();
                caches.SumTijs.resize(points.size());

                caches.SumTijDis.clear();
                caches.SumTijDis.resize(points.size());

                caches.SumGammaNormalDerivedOverVi.clear();
                caches.SumGammaNormalDerivedOverVi.resize(points.size());

                const float sigmaSqr = solverSigmaS * solverSigmaS;

                mira::parallel(points.size(), 8, [&neighbourhoods, &points, sigmaSqr, normals](size_t i)
                {
                    float sumtij = 0;
                    cv::Vec3f sumtijdi{ 0, 0, 0 };
                    for (size_t j : neighbourhoods[i])
                    {
                        sumtij += Tau(points[i], points[j], sigmaSqr);
                        sumtijdi += TauDerivedOverVi(points[i], points[j], sigmaSqr);
                    }
                    caches.SumTijs[i] = abs(sumtij * sumtij) < std::numeric_limits<float>::epsilon() ? (std::signbit(sumtij) ? -1 : 1) : sumtij;
                    caches.SumTijDis[i] = sumtijdi;

                    cv::Vec3f sumgammanormalderivedovervi{ 0, 0, 0 };
                    for (size_t j : neighbourhoods[i])
                    {
                        cv::Vec3f val = GammaNormalDerivedOverVi(i, points[i], normals[i], points[j], sigmaSqr);
                        assert(val == val);
                        sumgammanormalderivedovervi += val;
                    }
                    caches.SumGammaNormalDerivedOverVi[i] = sumgammanormalderivedovervi;
                });

                LogMessage(L"Computing Gradients");

                mira::parallel(points.size(), 8, [&gradients, &points, &neighbourhoods, &knn, &initialPoints, sigmaSqr, normals, N, lambda](size_t i)
                {
                    gradients[i] = GammaNormalsDerivedOverVi(points, normals, neighbourhoods, knn, points[i], i, sigmaSqr, N) + LambdaSumDerivative(initialPoints[i], points[i], lambda);
                    assert(gradients[i] == gradients[i]);
                });

                for (size_t i = 0; i < points.size(); ++i)
                {
                    points[i] = points[i] - stepi * gradients[i];
                }
            }

            for (size_t i = 0; i < points.size(); ++i)
                toReposition[i] = points[i];

            caches = {}; // release memory used by the compute caches
        }
    }
}


//
// Created by yaoyu on 3/29/20.
//

#ifndef MATRIXIO_HPP
#define MATRIXIO_HPP

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

#include <boost/algorithm/string.hpp>

#include <Eigen/Dense>

namespace stereo_utils
{

template < typename rT >
void read_matrix( const std::string& fn, int rows, int cols, const std::string& delimiter,
        Eigen::MatrixX<rT>& mat ) {
    assert(rows > 0);
    assert(cols > 0);

    mat.resize(rows, cols);

    std::string line;
    std::ifstream ifs(fn);
    std::size_t count = 0;

    if ( ifs.is_open() ) {
        while( std::getline(ifs, line) ) {
            boost::algorithm::trim(line);

            if ( line.empty() ) {
                continue;
            }

            if ( count == rows ) {
                std::stringstream ss;
                ss << "To many rows. Expecting " << rows << " rows. ";
                throw( std::runtime_error( ss.str() ) );
            }

            std::vector<std::string> splitStrings;

            boost::split( splitStrings, line, boost::is_any_of(delimiter) );

            if ( splitStrings.size() != cols ) {
                std::stringstream ss;
                ss << "Line " << count << " contains tokens other than " << cols << ". "
                   << "Line: " << line;
                throw( std::runtime_error( ss.str() ) );
            }

            for ( int i = 0; i < splitStrings.size(); ++i ) {
                std::stringstream ss( splitStrings[i] );
                rT number;
                ss >> number;

                mat(count, i) = number;
            }

            count++;
        }
    } else {
        std::stringstream ss;
        ss << "File " << fn << " not opened.";
        throw(std::runtime_error(ss.str()));
    }

    ifs.close();
}

template < typename Derived >
void write_matrix( const std::string &fn,
        const Eigen::MatrixBase<Derived> &mat,
        const std::string &delimiter="," ) {

    std::ofstream ofs(fn);

    if ( !ofs.good() ) {
        std::stringstream ss;
        ss << fn << " not good. ";
        throw std::runtime_error(ss.str());
    }

    const Eigen::IOFormat CSVFormat(
            Eigen::StreamPrecision, Eigen::DontAlignCols,
            delimiter, "\n");

    ofs << mat.format(CSVFormat);

    ofs.close();
}

} // namespace stereo_utils

#endif //MATRIXIO_HPP


/*!
 * \file BezierBox.hpp
 * \author Jun Yoshida
 * \copyright (c) 2020 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Januray 16, 2020: created
 */

#pragma once

#include <vector>
#include <future>

#include <Eigen/Dense>

#include "math/Bezier.hpp"
#include "math/Bezier2D.hpp"
#include "PathScheme.hpp"

class BezierScheme : public PathScheme<Eigen::Vector3d>
{
public:
    using vertex_type = typename PathScheme<Eigen::Vector3d>::vertex_type;
    using BBoxT = typename PathScheme<Eigen::Vector3d>::BBoxT;
    using BezierVarType = BezierVariant<Eigen::Vector3d,0,1,2,3>;
    using BezierSequence = std::vector<BezierVarType>;
    enum : size_t { max_degree = 3 };

private:

    std::vector<BezierSequence> m_bezseqs{};
    vertex_type m_lastpt{0.0, 0.0, 0.0};
    vertex_type m_origin{0.0, 0.0, 0.0};
    std::vector<vertex_type> m_orig_stack{};

    mutable std::thread m_computer{};
    mutable std::atomic_bool m_to_be_computed{false};

public:

    ~BezierScheme() {
        terminate();
    }
    std::vector<BezierSequence> const& getRaw() const noexcept {
        return m_bezseqs;
    }

    //! Move the raw buffer; after the function, the buffer becomes empty.
    std::vector<BezierSequence> moveRaw() noexcept {
        std::vector<BezierSequence> aux = std::move(m_bezseqs);
        m_bezseqs = std::vector<BezierSequence>{};
        return aux;
    }

    void clear() {
        terminate();
        m_bezseqs.clear();
        m_lastpt = vertex_type{};
        m_origin = vertex_type{};
        m_orig_stack.clear();
    }

    //! Terminate the computation thread.
    void terminate() const {
        m_to_be_computed.store(false);
        if(m_computer.joinable()) {
            m_computer.join();
        }
    }

    //! Divide The sequences of Bezier curves so that their projections have no intersection in the plane.
    //! \param prmat The projection matrix, which must be of rank 2. If two Bezier curves intersect with each other in their projections, only the lower curve is divided with respect to the vector obtained as the cross product of the first two rows.
    //! \param fun A functor object that is called at the end of the worker thread.
    //! \note The worker thread is not created if prmat is of rank less than 2.
    auto getProject(Eigen::Matrix<double,2,3> const& prmat,
                    std::function<void(void)> fun = []{}) const
        -> std::future<std::vector<BezierSequence>>;

    /*!
     * \name Inherited virtual members.
     * They are mostly trivial.
     */
    //@{
    virtual BBoxT getBBox() const noexcept override {
        return {};
    }

    virtual bool isvalid() const noexcept override {
        return true;
    }

    virtual void setPen(double, bord2::PathColor, bord2::StrokePattern) noexcept override {}
    virtual void setBrush(bord2::PathColor) noexcept override {}

    virtual void translate(vertex_type const& p) noexcept override {
        m_origin += p;
    }

    virtual void save() noexcept override {
        m_orig_stack.push_back(m_origin);
    }

    virtual void restore() noexcept override {
        if (m_orig_stack.empty()) {
            m_origin = Eigen::Vector3d(0.0, 0.0, 0.0);
        }
        else {
            m_origin = m_orig_stack.back();
            m_orig_stack.pop_back();
        }
    }

    virtual void stroke() override {}
    virtual void strokePres() override {}
    virtual void fill() override {}
    virtual void fillPres() override {}

protected:
    virtual void putPathElement(typename PathScheme<vertex_type>::PathElement const& elem) override {
        switch(elem.type) {
        case bord2::PathElemType::BeginPoint:
            if(m_bezseqs.empty() || !m_bezseqs.back().empty())
                m_bezseqs.emplace_back();
            m_lastpt = m_origin + elem.v[0];
            break;

        case bord2::PathElemType::Line:
            if(!m_bezseqs.empty()) {
                m_bezseqs.back().emplace_back(
                    std::integral_constant<size_t,1>(),
                    m_lastpt,
                    m_origin + elem.v[0] );
            }
            m_lastpt = m_origin + elem.v[0];
            break;

        case bord2::PathElemType::QBezier:
            if(!m_bezseqs.empty()) {
                m_bezseqs.back().emplace_back(
                    std::integral_constant<size_t,2>(),
                    m_lastpt,
                    m_origin + elem.v[0],
                    m_origin + elem.v[1] );
            }
            m_lastpt = m_origin + elem.v[1];
            break;

        case bord2::PathElemType::Bezier:
            if(!m_bezseqs.empty()) {
                m_bezseqs.back().emplace_back(
                    std::integral_constant<size_t,3>(),
                    m_lastpt,
                    m_origin + elem.v[0],
                    m_origin + elem.v[1],
                    m_origin + elem.v[2] );
            }
            m_lastpt = m_origin + elem.v[2];
            break;

        case bord2::PathElemType::Closing:
            if(!m_bezseqs.empty() && !m_bezseqs.back().empty()) {
                m_bezseqs.back().emplace_back(
                    std::integral_constant<size_t,1>(),
                    m_bezseqs.back().back().target(),
                    m_bezseqs.back().front().source() );
                m_lastpt = m_bezseqs.back().front().source();
            }
            break;

        default:
            // Error
            break;
        }
    }
    //@}
};

template <class V>
void drawBezierSequence(
    std::vector<BezierVariant<V,0,1,2,3>> const& bezseq,
    PathScheme<V> &scheme)
{
    V lastpt{};

    for(auto& bezvar : bezseq) {
        switch(bezvar.getActive()) {
        case 0:
            scheme.moveTo(bezvar.source());
            break;

        case 1:
            if(lastpt != bezvar.source())
                scheme.moveTo(bezvar.source());
            scheme.lineTo(bezvar.target());
            break;

        case 2:
            if(lastpt != bezvar.source())
                scheme.moveTo(bezvar.source());
            scheme.qbezierTo(
                bezvar.get(
                    std::integral_constant<size_t,2>()
                    ).get(std::integral_constant<size_t,1>()),
                bezvar.get(
                    std::integral_constant<size_t,2>()
                    ).get(std::integral_constant<size_t,2>())
                );
            break;

        case 3:
            if(lastpt != bezvar.source())
                scheme.moveTo(bezvar.source());
            scheme.bezierTo(
                bezvar.get(
                    std::integral_constant<size_t,3>()
                    ).get(std::integral_constant<size_t,1>()),
                bezvar.get(
                    std::integral_constant<size_t,3>()
                    ).get(std::integral_constant<size_t,2>()),
                bezvar.get(
                    std::integral_constant<size_t,3>()
                    ).get(std::integral_constant<size_t,3>())
                );
            break;

        default:
            /** NEVER REACHED **/
            break;
        }
        lastpt = bezvar.target();
    }
}


#include <iostream>
#include <chrono> //debug
#include <thread> //debug
#include <Eigen/Dense>
#include <vector>
#include <numeric> //iota
#include <Eigen/Sparse>
#include <random>
#include <iostream> //openmp

//define fehlt noch

namespace longroad{
class Lane_i
{
private:
    /* data */
    int color;
    Eigen::VectorXi cars;
    Eigen::VectorXi hold;
    Eigen::VectorXi move;
    Eigen::VectorXi move_pre;
    Eigen::VectorXi move_last;

    Eigen::VectorXi ind_down;  //maybe put in some vector property class
    Eigen::VectorXi ind_up;
public:
    Lane_i(int size,int color);

    void move_cars(Eigen::VectorXi actions);
    void move_cars(Eigen::VectorXi actions,Eigen::VectorXi yellow);

    void circ_shift_down(Eigen::VectorXi &vector_to_shift);
    void circ_shift_up(Eigen::VectorXi &vector_to_shift);
    Eigen::VectorXi get_circ_shift_down(Eigen::VectorXi &vector_to_shift);
    Eigen::VectorXi get_circ_shift_up(Eigen::VectorXi &vector_to_shift);
    //~Lane_i();

    double calc_speedup_reward();

    Eigen::VectorXi get_hold();
    Eigen::MatrixXi get_states();
    void set_states(Eigen::VectorXi);
};

class World_i
{
private:
    /* data */
    Lane_i *Lane1;
    Lane_i *Lane2;
    Lane_i *Lane3;
    Lane_i *Lane4;

    u_int seed;
    std::mt19937 generator;

    double steptime;
    int steps;// max steps for measuring time!! 2,147,483,647

    Eigen::VectorXi signal_state;
    Eigen::VectorXi last_actions;

    Eigen::MatrixXi last_states;
    Eigen::VectorXd last_rewards;

    //env parameters
    const int size;
    const bool measure_time;
    const bool yellow;
    const double global_cost; //action ressource cost
    const double global_reward; //collaboration reward

    std::chrono::time_point<std::chrono::high_resolution_clock> t_start;
    std::chrono::time_point<std::chrono::high_resolution_clock> t_end;

    //reward matrix for local rewards

public:
    World_i(int size, bool measure_time,  bool yellow, double global_cost, double global_reward);
    void step(Eigen::VectorXi  actions);
    void setSeed(uint seed);
    void reset();

    //Getters for pyenv
    const Eigen::MatrixXi &lastStates() { return last_states; }
    const Eigen::VectorXd &lastRewards() { return last_rewards; }
    double avgTime() {return steptime;}

    void move_lanes(Eigen::VectorXi  actions);
    void move_lanes_yellow(Eigen::VectorXi  actions);

    //~World_i();
};

}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef SPHERICALSHARP_HPP
#define SPHERICALSHARP_HPP

#include <cmath>
#include <iosfwd>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

#include "DerivativeTypes.hpp"
#include "DerivativeUtils.hpp"
#include "bi_operators/IntegratorForward.hpp"
#include "GreensFunction.hpp"
#include "utils/legendre.h"
#include "utils/MathUtils.hpp"
#include "dielectric_profile/Sharp.hpp"

/*! \file SphericalSharp.hpp
 *  \class SphericalSharp
 *  \brief Green's function for a sharp interface with spherical symmetry
 *  \author Roberto Di Remigio
 *  \date 2015
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \tparam IntegratorPolicy policy for the calculation of the matrix represenation of S and D
 */

template <typename DerivativeTraits = AD_directional,
          typename IntegratorPolicy = CollocationIntegrator>
class SphericalSharp __final : public GreensFunction<DerivativeTraits, IntegratorPolicy, Sharp,
                                              SphericalSharp<DerivativeTraits, IntegratorPolicy> >
{
public:
    /*! Constructor for a one-layer interface
     * \param[in] e permittivity of the sphere
     * \param[in] esolv permittivity of the solvent
     * \param[in] r radius of the dielectric sphere
     * \param[in] o center of the sphere
     */
    SphericalSharp(double e, double esolv, double r, const Eigen::Vector3d & o)
        : GreensFunction<DerivativeTraits, IntegratorPolicy, Sharp, SphericalSharp<DerivativeTraits, IntegratorPolicy> >(), origin_(o)
    {
        this->profile_ = Sharp(e, esolv, r);
    }
    /*! Constructor for a one-layer interface
     * \param[in] e permittivity of the sphere
     * \param[in] esolv permittivity of the solvent
     * \param[in] r radius of the dielectric sphere
     * \param[in] o center of the sphere
     */
    SphericalSharp(double e, double esolv, double r, const Eigen::Vector3d & o, double f)
        : GreensFunction<DerivativeTraits, IntegratorPolicy, Sharp, SphericalSharp<DerivativeTraits, IntegratorPolicy> >(f), origin_(o)
    {
        this->profile_ = Sharp(e, esolv, r);
    }
    virtual ~SphericalSharp() {}

    /*! Calculates the matrix representation of the S operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd singleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.singleLayer(*this, e);
    }
    /*! Calculates the matrix representation of the D operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd doubleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.doubleLayer(*this, e);
    }

    /*! \brief Returns non-singular part of the Green's function (image potential)
     *  \param[in] source location of the source charge
     *  \param[in] probe location of the probe charge
     */
    double imagePotential(const Eigen::Vector3d & source, const Eigen::Vector3d & probe) const {
        DerivativeTraits sp[3], pp[3];
        sp[0] = source(0); sp[1] = source(1); sp[2] = source(2);
        pp[0] = probe(0); pp[1] = probe(1); pp[2] = probe(2);
        return this->imagePotential_impl(sp, pp)[0];
    }
    /*! Returns value of the directional derivative of the
     *  non-singular part (image potential) of the Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point, thus assuming that the direction is relative to that point.
     *
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    double imagePotentialDerivative(const Eigen::Vector3d & direction, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const {
        DerivativeTraits t1[3], t2[3];
        t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
        t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
        t2[0][1] = direction(0); t2[1][1] = direction(1); t2[2][1] = direction(2);
        return this->imagePotential_impl(t1, t2)[1];
    }

    friend std::ostream & operator<<(std::ostream & os, SphericalSharp & gf) {
        return gf.printObject(os);
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* See http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html */
private:
    /*! Center of the dielectric sphere */
    Eigen::Vector3d origin_;

    /*! Evaluates the Green's function given a pair of points
     *  \param[in] sp the source point
     *  \param[in] pp the probe point
     */
    virtual DerivativeTraits operator()(DerivativeTraits * sp, DerivativeTraits * pp) const
    {
        DerivativeTraits distance = sqrt((sp[0] - pp[0]) * (sp[0] - pp[0]) +
                          (sp[1] - pp[1]) * (sp[1] - pp[1]) +
                          (sp[2] - pp[2]) * (sp[2] - pp[2]));
        return (1/(this->profile_.epsilonSolvent * distance) - this->imagePotential_impl(sp, pp));
    }
    /*! Returns value of the kernel of the \f$\mathcal{D}\f$ integral operator for the pair of points p1, p2:
     *  \f$ [\boldsymbol{\varepsilon}\nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)]\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  To obtain the kernel of the \f$\mathcal{D}^\dagger\f$ operator call this methods with \f$\mathbf{p}_1\f$
     *  and \f$\mathbf{p}_2\f$ exchanged and with \f$\mathbf{n}_{\mathbf{p}_2} = \mathbf{n}_{\mathbf{p}_1}\f$
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double kernelD_impl(const Eigen::Vector3d & direction,
                              const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const
    {
        return (this->profile_.epsilonSolvent * this->derivativeProbe(direction, p1, p2));
    }
    virtual KernelS exportKernelS_impl() const __override {
      return pcm::bind(&SphericalSharp<DerivativeTraits, IntegratorPolicy>::kernelS, *this, pcm::_1, pcm::_2);
    }
    virtual KernelD exportKernelD_impl() const __override {
      return pcm::bind(&SphericalSharp<DerivativeTraits, IntegratorPolicy>::kernelD, *this, pcm::_1, pcm::_2, pcm::_3);
    }
    DerivativeTraits imagePotential_impl(DerivativeTraits * sp, DerivativeTraits * pp) const
    {
        // Data from permittivity profile
        double radius = this->profile_.radius;
        double epsSolv = this->profile_.epsilonSolvent;
        double eps = this->profile_.epsilon;
        DerivativeTraits sp_origin[3], pp_origin[3];
        for (int i = 0; i < 3; ++i) {
            sp_origin[i] = sp[i] - origin_(i);
            pp_origin[i] = pp[i] - origin_(i);
        }
        DerivativeTraits sp_origin_norm = norm(sp_origin);
        DerivativeTraits pp_origin_norm = norm(pp_origin);
        // Angle between source and probe point
        DerivativeTraits cos_gamma = dot_product(sp_origin, pp_origin) / (sp_origin_norm * pp_origin_norm);

        DerivativeTraits r_img[3];
        for (int i = 0; i < 3; ++i) {
            r_img[i] = origin_(i) + pow(radius / pp_origin_norm, 2) * pp_origin[i];
        }
        // Distance between sp and r_img
        DerivativeTraits sp_image = distance(sp, r_img);
        DerivativeTraits q_img = radius / pp_origin_norm;
        double factor = (eps - epsSolv) / (eps + epsSolv);
        //DerivativeTraits G_img = DerivativeTraits(0.0);
        DerivativeTraits G_img = factor * (q_img / sp_image - q_img / sp_origin_norm);
        DerivativeTraits f_0 = radius / (sp_origin_norm * pp_origin_norm);
        DerivativeTraits f_L = f_0;
        for (int L = 1; L <= 200; ++L) {
            f_L = f_L * radius * f_0;
            double C_0_L = (eps - epsSolv) * L / ((eps + epsSolv) * L + epsSolv);
            DerivativeTraits pl_x = Legendre::Pn<DerivativeTraits>(L, cos_gamma);
            G_img += f_L * (C_0_L - factor) * pl_x;
        }

        return G_img / epsSolv;
    }
    virtual std::ostream & printObject(std::ostream & os)
    {
        Eigen::IOFormat CleanFmt(Eigen::StreamPrecision, 0, ", ", "\n", "(", ")");
        os << "Green's function type: spherical sharp" << std::endl;
        os << this->profile_ << std::endl;
        os << "Sphere center        = " << this->origin_.transpose().format(CleanFmt);
        return os;
    }
};

#endif // SPHERICALSHARP_HPP


#ifndef plane_calibration_SRC_DEVIATION_PLANES_HPP_
#define plane_calibration_SRC_DEVIATION_PLANES_HPP_

#include <utility>
#include <memory>
#include <Eigen/Dense>

#include "calibration_parameters.hpp"
#include "plane_to_depth_image.hpp"
#include "visualizer_interface.hpp"

namespace plane_calibration
{

class DeviationPlanes
{
public:
  DeviationPlanes(const PlaneToDepthImage& plane_to_depth, const VisualizerInterfacePtr& depth_visualizer);

  void init(const CalibrationParameters::Parameters& parameters);
  void update(const CalibrationParameters::Parameters& parameters, const bool& use_max_deviation = false);

  std::pair<double, double> estimateAngles(const Eigen::MatrixXf& plane, const bool& debug = false);
  std::pair<double, double> getDistanceDiffs(const Eigen::MatrixXf& plane, const bool& debug = false);
  static double getDistance(const Eigen::MatrixXf& from, const Eigen::MatrixXf& to, const bool& remove_nans);

  double getDeviation();
  std::pair<double, double> getMultipliers();

  Eigen::MatrixXf xPositive();
  Eigen::MatrixXf xNegative();

  Eigen::MatrixXf yPositive();
  Eigen::MatrixXf yNegative();

  Eigen::Affine3d xPositiveTransform();
  Eigen::Affine3d xNegativeTransform();

  Eigen::Affine3d yPositiveTransform();
  Eigen::Affine3d yNegativeTransform();

protected:
  static std::vector<double> getDistances(const std::vector<Eigen::MatrixXf>& from, const Eigen::MatrixXf& to);

  int indexXPositive();
  int indexXNegative();
  int indexYPositive();
  int indexYNegative();

  VisualizerInterfacePtr depth_visualizer_;
  PlaneToDepthImage plane_to_depth_;
  double deviation_;

  std::vector<Eigen::MatrixXf> planes_;
  std::vector<Eigen::Affine3d> transform_;

  std::pair<double, double> magic_multipliers_;
};
typedef std::shared_ptr<DeviationPlanes> DeviationPlanesPtr;

} /* end namespace */

#endif


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// 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.

#ifndef fiber_surface_grad_3d_functor_hpp
#define fiber_surface_grad_3d_functor_hpp

#include "../common/common.hpp"

#include "basis_data.hpp"
#include "geometrical_data.hpp"
#include "collection_of_3d_arrays.hpp"
#include "basis_transformation_functor_wrappers.hpp"

#include <boost/array.hpp>

namespace Fiber
{

template <typename CoordinateType_>
class SurfaceGrad3dElementaryFunctor
{
public:
    typedef CoordinateType_ CoordinateType;

    int argumentDimension() const { return 1; }
    int resultDimension() const { return 3; }

    void addDependencies(size_t& basisDeps, size_t& geomDeps) const {
        basisDeps |= DERIVATIVES;
        geomDeps |= JACOBIAN_INVERSES_TRANSPOSED;
    }

    template <typename ValueType>
    void evaluate(
            const ConstBasisDataSlice<ValueType>& basisData,
            const ConstGeometricalDataSlice<CoordinateType>& geomData,
            _1dSliceOf3dArray<ValueType>& result) const {
        assert(basisData.componentCount() == 1);
        assert(geomData.dimWorld() == 3);

        // result := gradient of the basis function extended outside
        // the surface so that its normal derivative on the surf. is zero
        result(0) = basisData.derivatives(0, 0) * geomData.jacobianInverseTransposed(0, 0) +
                basisData.derivatives(0, 1) * geomData.jacobianInverseTransposed(0, 1);
        result(1) = basisData.derivatives(0, 0) * geomData.jacobianInverseTransposed(1, 0) +
                basisData.derivatives(0, 1) * geomData.jacobianInverseTransposed(1, 1);
        result(2) = basisData.derivatives(0, 0) * geomData.jacobianInverseTransposed(2, 0) +
                basisData.derivatives(0, 1) * geomData.jacobianInverseTransposed(2, 1);
    }
};

// Note: in C++11 we'll be able to make a "template typedef", or more precisely
// a using declaration, instead of this spurious inheritance
/** \ingroup functors
 *  \brief Functor calculating the surface gradient of a scalar field in 3D. */
template <typename CoordinateType_>
class SurfaceGrad3dFunctor :
        public ElementaryBasisTransformationFunctorWrapper<
        SurfaceGrad3dElementaryFunctor<CoordinateType_> >
{
public:
    typedef CoordinateType_ CoordinateType;
};

} // namespace Fiber

#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 1, 4> test_long_1_4(const Eigen::Matrix<boost::int64_t, 1, 4> & M)
{
	return M;
}
void export_long_1_4()
{
	boost::python::def("test_long_1_4",test_long_1_4);
}


//          Copyright Oliver Kowalke 2009.
// 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 <cstdlib>
#include <iostream>

#include <boost/range.hpp>
#include <boost/coroutine/all.hpp>

#ifdef BOOST_COROUTINES_UNIDIRECT
void fibonacci( boost::coroutines::push_coroutine< int > & c)
{
    int first = 1, second = 1;
    while ( true)
    {
        int third = first + second;
        first = second;
        second = third;
        c( third);
    }
}

int main()
{
    boost::coroutines::pull_coroutine< int > c( fibonacci);
    boost::range_iterator<
       boost::coroutines::pull_coroutine< int >
    >::type   it( boost::begin( c) );
    for ( int i = 0; i < 10; ++i)
    {
        std::cout << * it <<  " ";
        ++it;
    }

    std::cout << "\nDone" << std::endl;

    return EXIT_SUCCESS;
}
#else
void fibonacci( boost::coroutines::coroutine< void( int) > & c)
{
    int first = 1, second = 1;
    while ( true)
    {
        int third = first + second;
        first = second;
        second = third;
        c( third);
    }
}

int main()
{
    boost::coroutines::coroutine< int() > c( fibonacci);
    boost::range_iterator<
       boost::coroutines::coroutine< int() >
    >::type   it( boost::begin( c) );
    for ( int i = 0; i < 10; ++i)
    {
        std::cout << * it <<  " ";
        ++it;
    }

    std::cout << "\nDone" << std::endl;

    return EXIT_SUCCESS;
}
#endif


#ifndef RANDOMMOTIONFORCE_HPP_
#define RANDOMMOTIONFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractForce.hpp"
#include "AbstractOffLatticeCellPopulation.hpp"
#include "RandomNumberGenerator.hpp"

/**
 * A force class to model random cell movement.
 */
template<unsigned DIM>
class RandomMotionForce : public AbstractForce<DIM>
{
private :

    /**
     * Random Movement Parameter.
     */
    double mMovementParameter;

    /**
     * Archiving.
     */
    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<DIM> >(*this);
        archive & mMovementParameter;
    }

public :

    /**
     * Constructor.
     */
    RandomMotionForce();

    /**
     * Destructor.
     */
    ~RandomMotionForce();

    /**
     * Set the diffusion constant for the cells.
     *
     * @param movementParameter the movement parameter to use
     */
    void SetMovementParameter(double movementParameter);

    /**
     * Get the random motion coefficient.
     *
     * @return mMovementParameter
     */
    double GetMovementParameter();

    /**
     * Overridden AddForceContribution() method.
     *
     * @param rCellPopulation reference to the tissue
     *
     */
    void AddForceContribution(AbstractCellPopulation<DIM>& rCellPopulation);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(RandomMotionForce)

#endif /*RANDOMMOTIONFORCE_HPP_*/