/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include "tudat/math/statistics/randomVariableGenerator.h"
#include "tudat/math/statistics/boostProbabilityDistributions.h"

namespace tudat
{

namespace statistics
{

std::function< Eigen::VectorXd( const double ) > getIndependentGaussianNoiseFunction(
        const double standardDeviation,
        const double mean,
        const double seed,
        const int outputSize )
{
    std::function< double( ) > inputFreeNoiseFunction = statistics::createBoostContinuousRandomVariableGeneratorFunction(
                statistics::normal_boost_distribution, { mean, standardDeviation }, seed );
    if( outputSize == 1 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ) ).finished( ); };
    }
    else if( outputSize == 2 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ) ).finished( ); };
    }
    else if( outputSize == 3 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ), inputFreeNoiseFunction( ) ).finished( ); };
    }
    else if( outputSize == 6 )
    {
        return [=](const double){ return ( Eigen::VectorXd( outputSize )<<
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ), inputFreeNoiseFunction( ),
                                           inputFreeNoiseFunction( ), inputFreeNoiseFunction( ), inputFreeNoiseFunction( ) ).finished( ); };
    }
    else
    {
        throw std::runtime_error( "Cannot simulate observation noise of size " + std::to_string( outputSize ) );
    }
}

//! Function to create a random number generating function from a continuous univariate distribution implemented in boost
std::function< double( ) > createBoostContinuousRandomVariableGeneratorFunction(
        const ContinuousBoostStatisticalDistributions boostDistribution,
        const std::vector< double >& parameters,
        const double seed )
{
    return std::bind( &RandomVariableGenerator< double >::getRandomVariableValue,
                        createBoostContinuousRandomVariableGenerator( boostDistribution, parameters, seed ) );
}

//! Function to create a random number generator from a continuous univariate distribution implemented in boost
std::shared_ptr< RandomVariableGenerator< double > > createBoostContinuousRandomVariableGenerator(
        const ContinuousBoostStatisticalDistributions boostDistribution,
        const std::vector< double >& parameters,
        const double seed )
{
    return std::make_shared< ContinuousRandomVariableGenerator >(
                createBoostRandomVariable( boostDistribution, parameters ), seed );
}

}

}


#include <cmath>
#include <vector>
#include <iostream>
#include <cassert>

#include <Eigen/Dense>

#include "startree.h"

namespace startree {
// Make a have the minimum x, y and z coordinates of a and b
static void minVec(Vector3d& a, const Vector3d& b) {
    a[X] = fmin(a[X], b[X]);
    a[Y] = fmin(a[Y], b[Y]);
    a[Z] = fmin(a[Z], b[Z]);
}

// Make a have the maximum x, y and z coordinates of a and b
static void maxVec(Vector3d& a, const Vector3d& b) {
    a[X] = fmax(a[X], b[X]);
    a[Y] = fmax(a[Y], b[Y]);
    a[Z] = fmax(a[Z], b[Z]);
}

/* Return new set of sub-bounds given the parent bounds, the split point
 * and the direction (which octant) */
static void
splitBounds(const Vector3d oldBounds[2], Vector3d newBounds[2],
            const Vector3d splitPoint, Vector3d& childSplit,
            TreeDirection dir) {
    switch(dir) {
    case NED:
        newBounds[0] = splitPoint;
        newBounds[1] = oldBounds[1];
        break;
    case NWD:
        newBounds[0] = splitPoint;
        newBounds[0][Y] = oldBounds[0][Y];
        newBounds[1] = oldBounds[1];
        newBounds[1][Y] = splitPoint[Y];
        break;
    case SED:
        newBounds[0] = splitPoint;
        newBounds[0][X] = oldBounds[0][X];
        newBounds[1] = oldBounds[1];
        newBounds[1][X] = splitPoint[X];
        break;
    case SWD:
        newBounds[0] = oldBounds[0];
        newBounds[0][Z] = splitPoint[Z];
        newBounds[1] = splitPoint;
        newBounds[1][Z] = oldBounds[1][Z];
        break;
    case NEU:
        newBounds[0] = splitPoint;
        newBounds[0][Z] = oldBounds[0][Z];
        newBounds[1] = oldBounds[1];
        newBounds[1][Z] = splitPoint[Z];
        break;
    case NWU:
        newBounds[0] = oldBounds[0];
        newBounds[0][X] = splitPoint[X];
        newBounds[1] = splitPoint;
        newBounds[1][X] = oldBounds[1][X];
        break;
    case SEU:
        newBounds[0] = oldBounds[0];
        newBounds[0][Y] = splitPoint[Y];
        newBounds[1] = splitPoint;
        newBounds[1][Y] = oldBounds[1][Y];
        break;
    case SWU:
        newBounds[0] = oldBounds[0];
        newBounds[1] = splitPoint;
        break;
    }
    childSplit = 0.5 * (newBounds[0] + newBounds[1]);
}

// Static variable to generate node ID's (not threadsafe)
static uint64_t nextNodeId = 1;
StarTree::StarTree(uint64_t maxLeafSize, Vector3d splitPoint,
                   Vector3d minBounds, Vector3d maxBounds) :
    nodeId_(0),
    isLeaf_(true),
    splitPoint_(splitPoint),
    branches_{nullptr, nullptr, nullptr, nullptr,
              nullptr, nullptr, nullptr, nullptr},
    isRoot_(true),
    parent_(nullptr),
    maxLeafSize_(maxLeafSize),
    numStars_(0),
    maxLuminosity_(0),
    sumLuminosity_(0),
    sumColor_(0, 0, 0),
    averageColor_(0, 0, 0),
    treeBounds_{minBounds, maxBounds},
    starBounds_{Vector3d::Zero(), Vector3d::Zero()},
    boundsCenter_(0, 0, 0),
    boundsRadius_(0) {}

StarTree::StarTree(uint64_t maxLeafSize, StarTree* parent) :
    nodeId_(nextNodeId++),
    isLeaf_(true),
    branches_{nullptr, nullptr, nullptr, nullptr,
              nullptr, nullptr, nullptr, nullptr},
    isRoot_(false),
    parent_(parent),
    maxLeafSize_(maxLeafSize),
    numStars_(0),
    maxLuminosity_(0),
    sumLuminosity_(0),
    sumColor_(0, 0, 0),
    averageColor_(0, 0, 0),
    treeBounds_{Vector3d::Zero(), Vector3d::Zero()},
    starBounds_{Vector3d::Zero(), Vector3d::Zero()},
    boundsCenter_(0, 0, 0),
    boundsRadius_(0) {}

// This employs a trick similar to the chmod permission bits
TreeDirection
StarTree::getDirection(const Vector3d& from, const Vector3d& to) {
    bool cx = from[X] >= to[X];
    bool cy = from[Y] >= to[Y];
    bool cz = from[Z] >= to[Z];
    int direction =
        (cx ? 1 : 0) +
        (cy ? 2 : 0) +
        (cz ? 4 : 0);
    return static_cast<TreeDirection>(direction);
}

void
StarTree::addStarMetadata(const Star* star) {
    //std::cerr << "Start" << std::endl;
    numStars_ += 1;

    maxLuminosity_ = fmax(maxLuminosity_, star->lum());
    //std::cerr << "Set max luminocity." << std::endl;
    sumLuminosity_ += star->lum();
    //std::cerr << "Updated sum of luminocities." << std::endl;

    sumColor_ += star->color();
    //std::cerr << "Updated sum of colours." << std::endl;
    averageColor_ = sumColor_ / numStars_;
    //std::cerr << "Updated average colour." << std::endl;

    minVec(starBounds_[0], star->position());
    //std::cerr << "Updated star min bounds." << std::endl;
    maxVec(starBounds_[1], star->position());
    //std::cerr << "Updated star max bounds." << std::endl;

    boundsCenter_ = 0.5 * (starBounds_[0] + starBounds_[1]);
    //std::cerr << "Updated star bounds center." << std::endl;
    boundsRadius_ = (boundsCenter_ - starBounds_[1]).norm();
    //std::cerr << "Updated star bounds radius." << std::endl;
}

void
StarTree::addStar(const Star* star,
                  map<uint64_t,const StarTree*>& treeMap) {
    TreeDirection dir;
    // Always add the metadata
    addStarMetadata(star);

    // Do things differently depending on what type of node we are
    if (isLeaf_) {
        // If we'd go over our maximum leaf size, we need to split up
        if (numStars_ > maxLeafSize_) {
            isLeaf_ = false;
            // Create the empty child nodes
            for (int i = 0; i < 8; i++) {
                assert(branches_[i] == nullptr);
                branches_[i] = new StarTree(maxLeafSize_, this);
                splitBounds(treeBounds_, branches_[i]->treeBounds_,
                            splitPoint_, branches_[i]->splitPoint_,
                            static_cast<TreeDirection>(i));
                uint64_t nodeId = branches_[i]->nodeId();
                assert(treeMap.find(nodeId) == treeMap.end());
                treeMap[nodeId] = branches_[i];
            }

            // Add all the old stars
            for (vector<const Star*>::iterator it = stars_.begin();
                 it != stars_.end(); ++it) {
                dir = getDirection(splitPoint_, (*it)->position());
                branches_[dir]->addStar(*it, treeMap);
            }
            // And don't forget the new one
            dir = getDirection(splitPoint_, star->position());
            branches_[dir]->addStar(star, treeMap);
        } else {
            // Otherwise just add the star to the list
            stars_.push_back(star);
        }
    } else {
        // We're a branch, figure out the child that gets the star
        dir = getDirection(splitPoint_, star->position());
        branches_[dir]->addStar(star, treeMap);
    }
}

static double getDistance(const Vector3d& pointA,
                          const Vector3d& pointB) {
    return (pointA - pointB).norm();
}

// Check if an octant approximately intersects the given sphere
static bool octantApproxIntersect(const Vector3d& point, double radius,
                                  const StarTree* t) {
    const double centerDistance = getDistance(point, t->splitPoint());
    const double subRadius = getDistance(t->splitPoint(),
                                         t->maxTreeBounds());
    return (radius + subRadius) >= centerDistance;
}

// Get all stars within a given radius from a point
void starsInRadius(const Vector3d& point, double radius,
                   vector<const StarTree*>& searchList,
                   vector<const Star*>& starsFound) {
    // Loop until the search list is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        // Does this octant possibly intersect our sphere?
        if (octantApproxIntersect(point, radius, t)) {
            if (t->isLeaf()) {
                // If it's a leaf check all the stars
                for (unsigned int i = 0; i < t->numStars(); i++) {
                    if (getDistance(point, (t->stars()[i])->position()) <=
                        radius) {
                        starsFound.push_back(t->stars()[i]);
                    }
                }
            } else {
                // If it's a branch, add all of the subnodes to be
                // searched
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

static bool canSeeStar(const Vector3d& point, double minLum,
                       const Star* star) {
    const Vector3d& starPosition = star->position();
    const double starDistance = getDistance(point, starPosition);
    const double starLuminosity = star->lum();

    return (starLuminosity /
            (4 * M_PI * starDistance * starDistance)) >= minLum;
}

static bool hasVisibleStars(const Vector3d& point, double minLum,
                            const StarTree* t) {
    const double centerDistance = getDistance(point, t->splitPoint());
    const double subRadius = getDistance(t->splitPoint(),
                                         t->maxTreeBounds());
    const double minDistance = fmax((centerDistance - subRadius), 0);

    return (t->maxLuminosity() /
            (4 * M_PI * minDistance * minDistance)) >= minLum;
}

void visibleStars(const Vector3d& point, double minLuminosity,
                  vector<const StarTree*>& searchList,
                  vector<const Star*>& starsFound) {
    // Loop until the search list is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStars(point, minLuminosity, t)) {
            if (t->isLeaf()) {
                for (unsigned int i = 0; i < t->numStars(); i++) {
                    if (canSeeStar(point, minLuminosity, t->stars()[i])) {
                        starsFound.push_back(t->stars()[i]);
                    }
                }
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

static double sigmoid(double x) {
    return 1.0 / (1.0 + exp(x));
}

static double magicFormula(double distance, double blurRadius) {
    double a = log(distance) - log(blurRadius);
    double b = sigmoid((-2.03935397) * a);
    double c = sigmoid(-3.065390091 * a + 1.851112427);

    return ( (b + 1.872968808 * c * (1.0 - c)) /
             (4 * M_PI * distance * distance) );
}

static bool canSeeStarMagic(const Vector3d& point, double minLum,
                            double blurRadius, const Star* star) {
    const Vector3d& starPosition = star->position();
    const double starDistance = getDistance(point, starPosition);
    const double starLuminosity = star->lum();
    const double magic =
        magicFormula(starDistance, blurRadius) * starLuminosity;

    return magic >= minLum;
}

static bool hasVisibleStarsMagic(const Vector3d& point, double minLum,
                                 double blurRadius, const StarTree* t) {
    const double centerDistance = getDistance(point, t->splitPoint());
    const double subRadius = getDistance(t->splitPoint(),
                                         t->maxTreeBounds());
    const double minDistance = fmax((centerDistance - subRadius),
                                    (4.84814e-7));
    const double magic =
        magicFormula(minDistance, blurRadius) * t->maxLuminosity();

    return magic >= minLum;
}

void visibleStarsMagic(const Vector3d& point, double minLuminosity,
                       double blurRadius,
                       vector<const StarTree*>& searchList,
                       vector<const Star*>& starsFound) {
    // Loop until the searchlist is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStarsMagic(point, minLuminosity, blurRadius, t)) {
            if (t->isLeaf()) {
                for (unsigned int i = 0; i < t->numStars(); i++) {
                    if (canSeeStarMagic(point, minLuminosity, blurRadius,
                                        t->stars()[i])) {
                        starsFound.push_back(t->stars()[i]);
                    }
                }
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

void visibleOctants(const Vector3d& point, double minLuminosity,
                    vector<const StarTree*>& searchList,
                    vector<uint64_t>& octantsFound) {
    // Loop until the searchlist is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStars(point, minLuminosity, t)) {
            if (t->isLeaf()) {
                octantsFound.push_back(t->nodeId());
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}

void visibleOctantsMagic(const Vector3d& point, double minLuminosity,
                         double blurRadius,
                         vector<const StarTree*>& searchList,
                         vector<uint64_t>& octantsFound) {
    // Loop until the searchlist is empty
    while (searchList.size() > 0) {
        // Look at the first thing on the list
        const StarTree* t = searchList[0];
        searchList.erase(searchList.begin());

        if (hasVisibleStarsMagic(point, minLuminosity, blurRadius, t)) {
            if (t->isLeaf()) {
                octantsFound.push_back(t->nodeId());
            } else {
                for (int i = 0; i < 8; i++) {
                    const StarTree* tmp =
                        t->branch(static_cast<TreeDirection>(i));
                    searchList.push_back(tmp);
                }
            }
        }
    }
}
} // namespace StarTree


// -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/assign/std/vector.hpp>
#include "lin_time.h" // GetElapsedTime
#include "cmd/unit_generic.h"
#include "cmd/unit_util.h"
#include "gfxlib.h"
#include "gfx/quaternion.h"
#include "viewarea.h"
#include "plane_display.h"

namespace
{

float Degree2Radian(float angle)
{
  const float ratio = M_PI / 180.0;
  return angle * ratio;
}

float GetDangerRate(Radar::Sensor::ThreatLevel::Value threat)
{
    using namespace Radar;

    switch (threat)
    {
    case Sensor::ThreatLevel::High:
        return 20.0; // Fast pulsation

    case Sensor::ThreatLevel::Medium:
        return 7.5; // Slow pulsation

    default:
        return 0.0; // No pulsation
    }
}

} // anonymous namespace

namespace Radar
{

PlaneDisplay::PlaneDisplay()
    : finalCameraAngle(Degree2Radian(30), Degree2Radian(0), Degree2Radian(0)),
      currentCameraAngle(finalCameraAngle),
      radarTime(0.0),
      lastAnimationTime(0.0)
{
    using namespace boost::assign; // vector::operator+=

    CalculateRotation();

    // Calculate ground plane
    const float edges = 32;
    const float full = 2 * M_PI;
    const float step = full / edges;
    for (float angle = 0.0; angle < full; angle += step)
    {
        groundPlane.push_back(Vector(cosf(angle), 0.0f, sinf(angle)));
    }

    // Sequences start in 1 and ends in 0
    nothingSequence += 0.0;
    bounceSequence += 1.0, 0.9999, 0.9991, 0.9964, 0.9900, 0.9775, 0.9559, 0.9216, 0.8704, 0.7975, 0.6975, 0.5644, 0.3916, 0.1719, 0.0, 0.1287, 0.2164, 0.2535, 0.2297, 0.1343, 0.0, 0.0660, 0.0405, 0.0, 0.0341, 0.0;
    cosineSequence += 1.0, 0.999391, 0.997564, 0.994522, 0.990268, 0.984808, 0.978148, 0.970296, 0.961262, 0.951057, 0.939693, 0.927184, 0.913545, 0.898794, 0.882948, 0.866025, 0.848048, 0.829038, 0.809017, 0.788011, 0.766044, 0.743145, 0.71934, 0.694658, 0.669131, 0.642788, 0.615662, 0.587785, 0.559193, 0.529919, 0.5, 0.469472, 0.438371, 0.406737, 0.374607, 0.34202, 0.309017, 0.275637, 0.241922, 0.207911, 0.173648, 0.139173, 0.104528, 0.069756, 0.034899, 0.0;
}

void PlaneDisplay::CalculateRotation()
{
    const float cosx = cosf(currentCameraAngle.x);
    const float cosy = cosf(currentCameraAngle.y);
    const float cosz = cosf(currentCameraAngle.z);
    const float sinx = sinf(currentCameraAngle.x);
    const float siny = sinf(currentCameraAngle.y);
    const float sinz = sinf(currentCameraAngle.z);

    xrotation = Vector(cosy * cosz,
                       sinx * siny * cosz - cosx * sinz,
                       cosx * siny * cosz + sinx * sinz);
    yrotation = Vector(cosy * sinz,
                       cosx * cosz + sinx * siny * sinz,
                       cosx * siny * sinz - sinx * cosz);
    zrotation = Vector(-siny,
                       sinx * cosy,
                       cosx * cosy);
}

void PlaneDisplay::PrepareAnimation(const Vector& fromAngle,
                                    const Vector& toAngle,
                                    const AngleSequence& xsequence,
                                    const AngleSequence& ysequence,
                                    const AngleSequence& zsequence)
{
    AnimationItem firstItem;
    firstItem.duration = 0.0;
    firstItem.position = fromAngle;
    animation.push(firstItem);

    float duration = 2.0;

    // Use the longest-running sequence and zero-pad shorter sequuences
    AngleSequence::size_type longestSequenceSize = xsequence.size();
    longestSequenceSize = std::max(longestSequenceSize, ysequence.size());
    longestSequenceSize = std::max(longestSequenceSize, zsequence.size());
    for (AngleSequence::size_type i = 0; i < longestSequenceSize; ++i)
    {
        float xentry = (i < xsequence.size()) ? xsequence[i] : 0.0;
        float yentry = (i < ysequence.size()) ? ysequence[i] : 0.0;
        float zentry = (i < zsequence.size()) ? zsequence[i] : 0.0;
        float xangle = toAngle.x + xentry * (fromAngle.x - toAngle.x);
        float yangle = toAngle.y + yentry * (fromAngle.y - toAngle.y);
        float zangle = toAngle.z + zentry * (fromAngle.z - toAngle.z);
        AnimationItem item;
        item.duration = duration;
        item.position = Vector(xangle, yangle, zangle);
        animation.push(item);
        duration = 0.05;
    }

    AnimationItem finalItem;
    finalItem.duration = duration;
    finalItem.position = toAngle;
    animation.push(finalItem);
}

void PlaneDisplay::OnDockEnd()
{
    // Bounce from upright position
    Vector undockCameraAngle(Degree2Radian(90), finalCameraAngle.y, finalCameraAngle.z);
    PrepareAnimation(undockCameraAngle, finalCameraAngle, bounceSequence, nothingSequence, nothingSequence);
}

void PlaneDisplay::OnJumpEnd()
{
    // Full rotation around y-axis
    Vector jumpCameraAngle(finalCameraAngle.x, Degree2Radian(360), finalCameraAngle.z);
    PrepareAnimation(jumpCameraAngle, finalCameraAngle, nothingSequence, cosineSequence, nothingSequence);
}

void PlaneDisplay::Draw(const Sensor& sensor,
                        VSSprite *nearSprite,
                        VSSprite *distantSprite)
{
    assert(nearSprite || distantSprite); // There should be at least one radar display

    radarTime += GetElapsedTime();

    leftRadar.SetSprite(nearSprite);
    rightRadar.SetSprite(distantSprite);

    if (nearSprite)
        nearSprite->Draw();
    if (distantSprite)
        distantSprite->Draw();

    Sensor::TrackCollection tracks = sensor.FindTracksInRange();

    Animate();

    GFXEnable(DEPTHTEST);
    GFXEnable(DEPTHWRITE);
    GFXEnable(SMOOTH);

    DrawNear(sensor, tracks);
    DrawDistant(sensor, tracks);

    GFXPointSize(1);
    GFXDisable(DEPTHTEST);
    GFXDisable(DEPTHWRITE);
    GFXDisable(SMOOTH);
}

void PlaneDisplay::Animate()
{
    if (!animation.empty())
    {
        if (radarTime > lastAnimationTime + animation.front().duration)
        {
            currentCameraAngle = animation.front().position;
            CalculateRotation();
            animation.pop();
            lastAnimationTime = radarTime;
        }
    }
}

Vector PlaneDisplay::Projection(const ViewArea& radarView, const Vector& position)
{
    // 1. Rotate
    float rx = position.Dot(xrotation);
    float ry = position.Dot(yrotation);
    float rz = position.Dot(zrotation);

    // 2. Project perspective
    // Using the symmetric viewing volume where right = -left and top = -bottom
    // gives us this perspective projection matrix
    //   n/r           0             0             0
    //   0             n/t           0             0
    //   0             0             -(f+n)/(f-n)  -2fn/(f-n)
    //   0             0             -1            0
    // location = M_perspective * rotatedPosition
    const float nearDistance = -0.5; // -0.25 => zoom out, -0.75 => zoom in
    const float farDistance = 0.5;
    const float top = 0.5;
    const float right = 0.5;
    float x = rx * (nearDistance / right);
    float y = ry * (nearDistance / top);
    float z = (rz * (- (farDistance + nearDistance) / (farDistance - nearDistance)) - 2.0 * farDistance * nearDistance / (farDistance - nearDistance));

    // 3. Scale onto radarView
    return radarView.Scale(Vector(x, y, z));
}

void PlaneDisplay::DrawGround(const Sensor& sensor, const ViewArea& radarView)
{
    GFXColor groundColor = radarView.GetColor();
    const float outer = 3.0 / 3.0;
    const float middle = 2.0 / 3.0;
    const float inner = 1.0 / 3.0;

    groundColor.a = 0.1;
    GFXColorf(groundColor);
    GFXLineWidth(0.5);
    GFXBegin(GFXPOLY);
    for (std::vector<Vector>::const_iterator it = groundPlane.begin(); it != groundPlane.end(); ++it)
    {
        GFXVertexf(Projection(radarView, outer * (*it)));
    }
    GFXEnd();

    groundColor.a = 0.4;
    GFXColorf(groundColor);
    GFXBegin(GFXLINESTRIP);
    for (std::vector<Vector>::const_iterator it = groundPlane.begin(); it != groundPlane.end(); ++it)
    {
        GFXVertexf(Projection(radarView, middle * (*it)));
    }
    GFXVertexf(Projection(radarView, middle * groundPlane.front()));
    GFXEnd();

    GFXBegin(GFXLINESTRIP);
    for (std::vector<Vector>::const_iterator it = groundPlane.begin(); it != groundPlane.end(); ++it)
    {
        GFXVertexf(Projection(radarView, inner * (*it)));
    }
    GFXVertexf(Projection(radarView, inner * groundPlane.front()));
    GFXEnd();

    groundColor.a = 0.4;
    const float xcone = cosf(sensor.GetLockCone());
    const float zcone = sinf(sensor.GetLockCone());
    const float innerCone = inner;
    const float outerCone = outer;
    Vector leftCone(xcone, 0.0f, zcone);
    Vector rightCone(-xcone, 0.0f, zcone);
    GFXColorf(groundColor);
    GFXBegin(GFXLINE);
    GFXVertexf(Projection(radarView, outerCone * leftCone));
    GFXVertexf(Projection(radarView, innerCone * leftCone));
    GFXVertexf(Projection(radarView, outerCone * rightCone));
    GFXVertexf(Projection(radarView, innerCone * rightCone));
    GFXEnd();
    GFXLineWidth(1);
}

void PlaneDisplay::DrawNear(const Sensor& sensor,
                            const Sensor::TrackCollection& tracks)
{
    // Draw all near tracks (distance scaled)

    if (!leftRadar.IsActive())
        return;

    float maxRange = sensor.GetCloseRange();

    DrawGround(sensor, leftRadar);

    for (Sensor::TrackCollection::const_iterator it = tracks.begin(); it != tracks.end(); ++it)
    {
        if (it->GetDistance() > maxRange)
            continue;

        DrawTrack(sensor, leftRadar, *it, maxRange);
    }
}

void PlaneDisplay::DrawDistant(const Sensor& sensor,
                               const Sensor::TrackCollection& tracks)
{
    // Draw all near tracks (distance scaled)

    if (!rightRadar.IsActive())
        return;

    float minRange = sensor.GetCloseRange();
    float maxRange = sensor.GetMaxRange();

    DrawGround(sensor, rightRadar);

    for (Sensor::TrackCollection::const_iterator it = tracks.begin(); it != tracks.end(); ++it)
    {
        if ((it->GetDistance() < minRange) || (it->GetDistance() > maxRange))
            continue;

        DrawTrack(sensor, rightRadar, *it, maxRange);
    }
}

void PlaneDisplay::DrawTrack(const Sensor& sensor,
                             const ViewArea& radarView,
                             const Track& track,
                             float maxRange)
{
    const Track::Type::Value unitType = track.GetType();
    GFXColor color = sensor.GetColor(track);

    Vector position = track.GetPosition();
    Vector scaledPosition = Vector(position.x, -position.y, position.z) / maxRange;
    if (scaledPosition.Magnitude() > 1.0)
        return;

    // FIXME: Integrate radar into damage/repair system
    // FIXME: Jitter does not work when entering a nebula
    // FIXME: Jitter does not work close by
    if (sensor.InsideNebula())
    {
        Jitter(0.0, 0.01, scaledPosition);
    }
    else
    {
        const bool isNebula = (track.GetType() == Track::Type::Nebula);
        const bool isEcmActive = track.HasActiveECM();
        if (isNebula || isEcmActive)
        {
            const float errorOffset = (scaledPosition.x > 0.0 ? 0.01 : -0.01);
            const float errorRange = 0.03;
            Jitter(errorOffset, errorRange, scaledPosition);
        }
    }

    Vector head = Projection(radarView, scaledPosition);

    Vector scaledGround(scaledPosition.x, 0, scaledPosition.z);
    Vector ground = Projection(radarView, scaledGround);

    const bool isBelowGround = (scaledPosition.y > 0); // Y has been inverted

    // Tracks below ground are muted
    if (isBelowGround)
        color.a /= 3;
    // and so is cargo
    if (track.GetType() == Track::Type::Cargo)
        color.a /= 4;

    if (sensor.UseThreatAssessment())
    {
        float dangerRate = GetDangerRate(sensor.IdentifyThreat(track));
        if (dangerRate > 0.0)
        {
            // Blinking track
            color.a *= cosf(dangerRate * radarTime);
        }
    }

    // Fade out dying ships
    if (track.IsExploding())
    {
        color.a *= (1.0 - track.ExplodingProgress());
    }

    float trackSize = std::max(1.0f, std::log10(track.GetSize()));
    if (track.GetType() != Track::Type::Cargo)
        trackSize += 1.0;

    DrawTarget(unitType, head, ground, trackSize, color);

    if (sensor.IsTracking(track))
    {
        Vector center = Projection(radarView, Vector(0, 0, 0));
        DrawTargetMarker(head, ground, center, trackSize, color, sensor.UseObjectRecognition());
    }
}

void PlaneDisplay::DrawTarget(Track::Type::Value unitType,
                              const Vector& head,
                              const Vector& ground,
                              float trackSize,
                              const GFXColor& color)
{
    // Draw leg
    GFXColor legColor = color;
    legColor.a /= 2;
    GFXLineWidth(0.2);
    GFXColorf(legColor);
    GFXBegin(GFXLINE);
    GFXVertexf(head);
    GFXVertexf(ground);
    GFXEnd();

    // Draw head
    GFXColorf(color);
    GFXPointSize(trackSize);
    GFXBegin(GFXPOINT);
    GFXVertexf(head);
    GFXEnd();
}

void PlaneDisplay::DrawTargetMarker(const Vector& head,
                                    const Vector& ground,
                                    const Vector& center,
                                    float trackSize,
                                    const GFXColor& color,
                                    bool drawArea)
{
    if (drawArea)
    {
        GFXColor areaColor = color;
        areaColor.a /= 4;
        GFXColorf(areaColor);
        GFXBegin(GFXPOLY);
        GFXVertexf(head);
        GFXVertexf(ground);
        GFXVertexf(center);
        GFXEnd();
    }

    // Diamond
    float size = 6.0 * std::max(trackSize, 1.0f);
    float xsize = size / g_game.x_resolution;
    float ysize = size / g_game.y_resolution;

    GFXColorf(color);
    GFXLineWidth(1);
    GFXBegin(GFXLINESTRIP);
    GFXVertex3f(head.x - xsize, head.y, 0.0f);
    GFXVertex3f(head.x, head.y - ysize, 0.0f);
    GFXVertex3f(head.x + xsize, head.y, 0.0f);
    GFXVertex3f(head.x, head.y + ysize, 0.0f);
    GFXVertex3f(head.x - xsize, head.y, 0.0f);
    GFXEnd();
}

} // namespace Radar


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

#include <opengv/relative_pose/methods.hpp>
#include <opengv/Indices.hpp>

#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/arun.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/relative_pose/modules/main.hpp>
#include <opengv/triangulation/methods.hpp>

#include <cmath>
#include <iostream>

opengv::translation_t
opengv::relative_pose::twopt(
    const RelativeAdapterBase & adapter,
    bool unrotate,
    const std::vector<int> & indices )
{
  assert(indices.size()>1);
  return twopt( adapter, unrotate, indices[0], indices[1] );
};

opengv::translation_t
opengv::relative_pose::twopt(
    const RelativeAdapterBase & adapter,
    bool unrotate,
    size_t index0,
    size_t index1 )
{
  bearingVector_t f1 = adapter.getBearingVector1(index0);
  bearingVector_t f1prime = adapter.getBearingVector2(index0);
  bearingVector_t f2 = adapter.getBearingVector1(index1);
  bearingVector_t f2prime = adapter.getBearingVector2(index1);

  if(unrotate)
  {
    rotation_t R12 = adapter.getR12();
    f1prime = R12 * f1prime;
    f2prime = R12 * f2prime;
  }

  Eigen::Vector3d normal1 = f1.cross(f1prime);
  Eigen::Vector3d normal2 = f2.cross(f2prime);

  translation_t translation = normal1.cross(normal2);
  translation = translation/translation.norm();

  Eigen::Vector3d opticalFlow = f1 - f1prime;
  if( opticalFlow.dot(translation) < 0 )
    translation = -translation;

  return translation;
};

opengv::rotation_t
opengv::relative_pose::twopt_rotationOnly(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size() > 1);
  return twopt_rotationOnly( adapter, indices[0], indices[1] );
};

opengv::rotation_t
opengv::relative_pose::twopt_rotationOnly(
    const RelativeAdapterBase & adapter,
    size_t index0,
    size_t index1)
{
  Eigen::Vector3d pointsCenter1 =
      adapter.getBearingVector1(index0) + adapter.getBearingVector1(index1);
  Eigen::Vector3d pointsCenter2 =
      adapter.getBearingVector2(index0) + adapter.getBearingVector2(index1);
  pointsCenter1 = pointsCenter1/3.0;
  pointsCenter2 = pointsCenter2/3.0;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  Eigen::Vector3d f = adapter.getBearingVector1(index0) - pointsCenter1;
  Eigen::Vector3d fprime = adapter.getBearingVector2(index0) - pointsCenter2;
  Hcross += fprime * f.transpose();
  f = adapter.getBearingVector1(index1) - pointsCenter1;
  fprime = adapter.getBearingVector2(index1) - pointsCenter2;
  Hcross += fprime * f.transpose();

  return math::arun(Hcross);
};

namespace opengv
{
namespace relative_pose
{

rotation_t rotationOnly(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 2);

  Eigen::Vector3d pointsCenter1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d pointsCenter2 = Eigen::Vector3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    pointsCenter1 += adapter.getBearingVector1(indices[i]);
    pointsCenter2 += adapter.getBearingVector2(indices[i]);
  }

  pointsCenter1 = pointsCenter1 / numberCorrespondences;
  pointsCenter2 = pointsCenter2 / numberCorrespondences;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    Eigen::Vector3d f = adapter.getBearingVector1(indices[i]) - pointsCenter1;
    Eigen::Vector3d fprime =
        adapter.getBearingVector2(indices[i]) - pointsCenter2;
    Hcross += fprime * f.transpose();
  }

  return math::arun(Hcross);
};

}
}

opengv::rotation_t
opengv::relative_pose::rotationOnly( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return rotationOnly(adapter,idx);
};

opengv::rotation_t
opengv::relative_pose::rotationOnly(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return rotationOnly(adapter,idx);
};

namespace opengv
{
namespace relative_pose
{

complexEssentials_t fivept_stewenius(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::MatrixXd Q(numberCorrespondences,9);
  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f = adapter.getBearingVector1(indices[i]);
    //bearingVector_t fprime = adapter.getBearingVector2(indices[i]);
    //Stewenius' algorithm is computing the inverse transformation, so we simply
    //invert the input here
    bearingVector_t f = adapter.getBearingVector2(indices[i]);
    bearingVector_t fprime = adapter.getBearingVector1(indices[i]);
    Eigen::Matrix<double,1,9> row;
    row <<  f[0]*fprime[0], f[1]*fprime[0], f[2]*fprime[0],
        f[0]*fprime[1], f[1]*fprime[1], f[2]*fprime[1],
        f[0]*fprime[2], f[1]*fprime[2], f[2]*fprime[2];
    Q.row(i) = row;
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(Q, Eigen::ComputeFullV );
  Eigen::Matrix<double,9,4> EE = SVD.matrixV().block(0,5,9,4);
  complexEssentials_t complexEssentials;
  modules::fivept_stewenius_main(EE,complexEssentials);
  return complexEssentials;
};

}
}

opengv::complexEssentials_t
opengv::relative_pose::fivept_stewenius( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return fivept_stewenius(adapter,idx);
};

opengv::complexEssentials_t
opengv::relative_pose::fivept_stewenius(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return fivept_stewenius(adapter,idx);
};

namespace opengv
{
namespace relative_pose
{

essentials_t fivept_nister(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::MatrixXd Q(numberCorrespondences,9);
  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f = adapter.getBearingVector1(indices[i]);
    //bearingVector_t fprime = adapter.getBearingVector2(indices[i]);
    //Nister's algorithm is computing the inverse transformation, so we simply
    //invert the input here
    bearingVector_t f = adapter.getBearingVector2(indices[i]);
    bearingVector_t fprime = adapter.getBearingVector1(indices[i]);
    Eigen::Matrix<double,1,9> row;
    row <<  f[0]*fprime[0], f[1]*fprime[0], f[2]*fprime[0],
        f[0]*fprime[1], f[1]*fprime[1], f[2]*fprime[1],
        f[0]*fprime[2], f[1]*fprime[2], f[2]*fprime[2];
    Q.row(i) = row;
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(Q, Eigen::ComputeFullV );
  Eigen::Matrix<double,9,4> EE = SVD.matrixV().block(0,5,9,4);
  essentials_t essentials;
  modules::fivept_nister_main(EE,essentials);

  return essentials;
};

}
}

opengv::essentials_t
opengv::relative_pose::fivept_nister( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return fivept_nister(adapter,idx);
};

opengv::essentials_t
opengv::relative_pose::fivept_nister(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return fivept_nister(adapter,idx);
};

opengv::rotations_t
opengv::relative_pose::fivept_kneip(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences == 5);

  Eigen::Matrix<double,3,5> f1;
  Eigen::Matrix<double,3,5> f2;

  for(size_t i = 0; i < numberCorrespondences; i++)
  {
    f1.col(i) = adapter.getBearingVector1(indices[i]);
    f2.col(i) = adapter.getBearingVector2(indices[i]);
  }

  rotations_t rotations;
  modules::fivept_kneip_main( f1, f2, rotations );
  return rotations;
}

namespace opengv
{
namespace relative_pose
{

essentials_t sevenpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 6);

  Eigen::MatrixXd A(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    //bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    //The seven-point is computing the inverse transformation, which is why we
    //invert the input
    bearingVector_t f1 = adapter.getBearingVector2(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector1(indices[i]);

    A.block<1,3>(i,0) = f2[0] * f1.transpose();
    A.block<1,3>(i,3) = f2[1] * f1.transpose();
    A.block<1,3>(i,6) = f2[2] * f1.transpose();
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      A,
      Eigen::ComputeFullU | Eigen::ComputeFullV );

  Eigen::Matrix<double,9,1> f1 = SVD.matrixV().col(8);
  Eigen::Matrix<double,9,1> f2 = SVD.matrixV().col(7);

  Eigen::MatrixXd F1_temp(3,3);
  F1_temp.col(0) = f1.block<3,1>(0,0);
  F1_temp.col(1) = f1.block<3,1>(3,0);
  F1_temp.col(2) = f1.block<3,1>(6,0);
  essential_t F1 = F1_temp.transpose();

  Eigen::MatrixXd F2_temp(3,3);
  F2_temp.col(0) = f2.block<3,1>(0,0);
  F2_temp.col(1) = f2.block<3,1>(3,0);
  F2_temp.col(2) = f2.block<3,1>(6,0);
  essential_t F2 = F2_temp.transpose();

  double eps = 0.00000001;
  essentials_t essentials;

  if( fabs(F1.determinant()) < eps || numberCorrespondences > 7 )
  {
    essentials.push_back(F1);
  }
  else
  {
    essential_t M = F2.inverse() * F1;
    Eigen::EigenSolver< essential_t > Eig(M,true);
    Eigen::Matrix< std::complex<double>,3,1 > D = Eig.eigenvalues();

    double val1 = fabs(D(0,0).imag());
    double val2 = fabs(D(1,0).imag());
    double val3 = fabs(D(2,0).imag());

    if( val1 < eps && val2 < eps && val3 < eps )
    {
      essentials.push_back( F1 - D(0,0).real() * F2 );
      essentials.push_back( F1 - D(1,0).real() * F2 );
      essentials.push_back( F1 - D(2,0).real() * F2 );
    }
    else
    {
      double min = val1;
      int minIndex = 0;
      if( val2 < min )
      {
        min = val2;
        minIndex = 1;
      }
      if( val3 < min )
        minIndex = 2;

      essentials.push_back( F1 - D(minIndex,0).real() * F2 );
    }
  }

  return essentials;
}

}
}

opengv::essentials_t
opengv::relative_pose::sevenpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return sevenpt(adapter,idx);
}

opengv::essentials_t
opengv::relative_pose::sevenpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return sevenpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

essential_t eightpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 7);

  Eigen::MatrixXd A(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    //bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    //bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    //The eight-point essentially computes the inverse transformation, which is
    //why we invert the input here
    bearingVector_t f1 = adapter.getBearingVector2(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector1(indices[i]);

    A.block<1,3>(i,0) = f2[0] * f1.transpose();
    A.block<1,3>(i,3) = f2[1] * f1.transpose();
    A.block<1,3>(i,6) = f2[2] * f1.transpose();
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      A,
      Eigen::ComputeFullU | Eigen::ComputeFullV );
  Eigen::Matrix<double,9,1> f = SVD.matrixV().col(8);

  Eigen::MatrixXd F_temp(3,3);
  F_temp.col(0) = f.block<3,1>(0,0);
  F_temp.col(1) = f.block<3,1>(3,0);
  F_temp.col(2) = f.block<3,1>(6,0);
  essential_t F = F_temp.transpose();

  Eigen::JacobiSVD< Eigen::MatrixXd > SVD2(
      F,
      Eigen::ComputeFullU | Eigen::ComputeFullV );
  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  S(0,0) = SVD2.singularValues()[0];
  S(1,1) = SVD2.singularValues()[1];

  Eigen::Matrix3d U = SVD2.matrixU();
  Eigen::Matrix3d Vtr = SVD2.matrixV().transpose();

  essential_t essential = U * S * Vtr;
  return essential;
}

}
}

opengv::essential_t
opengv::relative_pose::eightpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return eightpt(adapter,idx);
}

opengv::essential_t
opengv::relative_pose::eightpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return eightpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

rotation_t eigensolver(
    const RelativeAdapterBase & adapter,
    const Indices & indices,
    eigensolverOutput_t & output,
    bool useWeights )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 4);

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  //compute the norm of all the scores
  double norm = 0.0;
  for(size_t i=0; i < numberCorrespondences; i++)
    norm += pow(adapter.getWeight(indices[i]),2);
  norm = sqrt(norm);

  //Fill summation terms
  for(size_t i=0; i < numberCorrespondences; i++)
  {
    bearingVector_t f1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 = adapter.getBearingVector2(indices[i]);
    Eigen::Matrix3d F = f2*f2.transpose();

    double weight = 1.0;
    if( useWeights )
      weight = adapter.getWeight(indices[i])/norm;

    xxF = xxF + weight*f1[0]*f1[0]*F;
    yyF = yyF + weight*f1[1]*f1[1]*F;
    zzF = zzF + weight*f1[2]*f1[2]*F;
    xyF = xyF + weight*f1[0]*f1[1]*F;
    yzF = yzF + weight*f1[1]*f1[2]*F;
    zxF = zxF + weight*f1[2]*f1[0]*F;
  }

  //Do minimization
  modules::eigensolver_main(xxF,yyF,zzF,xyF,yzF,zxF,output);

  //Correct the translation
  bearingVector_t f1 = adapter.getBearingVector1(indices[0]);
  bearingVector_t f2 = adapter.getBearingVector2(indices[0]);
  f2 = output.rotation * f2;
  Eigen::Vector3d opticalFlow = f1 - f2;
  if( opticalFlow.dot(output.translation) < 0.0 )
    output.translation = -output.translation;

  return output.rotation;
}

}
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    eigensolverOutput_t & output,
    bool useWeights )
{
  Indices idx(adapter.getNumberCorrespondences());
  return eigensolver(adapter,idx,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    eigensolverOutput_t & output,
    bool useWeights )
{
  Indices idx(indices);
  return eigensolver(adapter,idx,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    bool useWeights )
{
  eigensolverOutput_t output;
  output.rotation = adapter.getR12();
  return eigensolver(adapter,output,useWeights);
}

opengv::rotation_t
opengv::relative_pose::eigensolver(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    bool useWeights )
{
  eigensolverOutput_t output;
  output.rotation = adapter.getR12();
  return eigensolver(adapter,indices,output,useWeights);
}

namespace opengv
{
namespace relative_pose
{

rotations_t sixpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences == 6);

  Eigen::Matrix<double,6,6> L1;
  Eigen::Matrix<double,6,6> L2;

  for(size_t i = 0; i < numberCorrespondences; i++)
  {
    bearingVector_t f1 =
        adapter.getCamRotation1(indices[i]) * adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 =
        adapter.getCamRotation2(indices[i]) * adapter.getBearingVector2(indices[i]);

    L1.block<3,1>(0,i) = f1;
    L2.block<3,1>(0,i) = f2;

    L1.block<3,1>(3,i) = f1.cross(adapter.getCamOffset1(indices[i]));
    L2.block<3,1>(3,i) = f2.cross(adapter.getCamOffset2(indices[i]));
  }

  rotations_t solutions;
  modules::sixpt_main( L1, L2, solutions );
  return solutions;
}

}
}

opengv::rotations_t
opengv::relative_pose::sixpt(
    const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return sixpt(adapter,idx);
}

opengv::rotations_t
opengv::relative_pose::sixpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return sixpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

transformation_t ge(
    const RelativeAdapterBase & adapter,
    const Indices & indices,
    geOutput_t & output,
    bool useWeights )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 5);

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  Eigen::Matrix<double,3,9> x1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> y1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> z1P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> x2P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> y2P = Eigen::Matrix<double,3,9>::Zero();
  Eigen::Matrix<double,3,9> z2P = Eigen::Matrix<double,3,9>::Zero();

  Eigen::Matrix<double,9,9> m11P = Eigen::Matrix<double,9,9>::Zero();
  Eigen::Matrix<double,9,9> m12P = Eigen::Matrix<double,9,9>::Zero();
  Eigen::Matrix<double,9,9> m22P = Eigen::Matrix<double,9,9>::Zero();

  //compute the norm of all the scores
  double norm = 0.0;
  for(size_t i=0; i < numberCorrespondences; i++)
    norm += pow(adapter.getWeight(indices[i]),2);
  norm = sqrt(norm);

  //Fill summation terms
  for(size_t i=0; i < numberCorrespondences; i++)
  {
    //get the weight of this feature
    double weight = 1.0;
    if( useWeights )
      weight = adapter.getWeight(indices[i])/norm;

    //unrotate the bearing vectors
    bearingVector_t f1 = adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]);
    bearingVector_t f2 = adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]);

    //compute the standard summation terms
    Eigen::Matrix3d F = f2*f2.transpose();

    xxF = xxF + weight*f1[0]*f1[0]*F;
    yyF = yyF + weight*f1[1]*f1[1]*F;
    zzF = zzF + weight*f1[2]*f1[2]*F;
    xyF = xyF + weight*f1[0]*f1[1]*F;
    yzF = yzF + weight*f1[1]*f1[2]*F;
    zxF = zxF + weight*f1[2]*f1[0]*F;

    //now compute the "cross"-summation terms
    Eigen::Vector3d t1 = adapter.getCamOffset1(indices[i]);
    Eigen::Vector3d t2 = adapter.getCamOffset2(indices[i]);

    Eigen::Matrix<double,1,9> f2_19;
    double temp = f1[1]*t1[2]-f1[2]*t1[1];
    f2_19(0,0) = f2[0] * temp;
    f2_19(0,1) = f2[1] * temp;
    f2_19(0,2) = f2[2] * temp;
    temp = f1[2]*t1[0]-f1[0]*t1[2];
    f2_19(0,3) = f2[0] * temp;
    f2_19(0,4) = f2[1] * temp;
    f2_19(0,5) = f2[2] * temp;
    temp = f1[0]*t1[1]-f1[1]*t1[0];
    f2_19(0,6) = f2[0] * temp;
    f2_19(0,7) = f2[1] * temp;
    f2_19(0,8) = f2[2] * temp;

    Eigen::Matrix<double,1,9> f1_19;
    temp = f2[1]*t2[2]-f2[2]*t2[1];
    f1_19(0,0) = f1[0] * temp;
    f1_19(0,1) = f1[1] * temp;
    f1_19(0,2) = f1[2] * temp;
    temp = f2[2]*t2[0]-f2[0]*t2[2];
    f1_19(0,3) = f1[0] * temp;
    f1_19(0,4) = f1[1] * temp;
    f1_19(0,5) = f1[2] * temp;
    temp = f2[0]*t2[1]-f2[1]*t2[0];
    f1_19(0,6) = f1[0] * temp;
    f1_19(0,7) = f1[1] * temp;
    f1_19(0,8) = f1[2] * temp;

    if( useWeights )
    {
      x1P = x1P + ( (weight * f1[0]) * f2 ) * f1_19;
      y1P = y1P + ( (weight * f1[1]) * f2 ) * f1_19;
      z1P = z1P + ( (weight * f1[2]) * f2 ) * f1_19;

      x2P = x2P + ( (weight * f1[0]) * f2 ) * f2_19;
      y2P = y2P + ( (weight * f1[1]) * f2 ) * f2_19;
      z2P = z2P + ( (weight * f1[2]) * f2 ) * f2_19;

      m11P = m11P - ( weight * f1_19.transpose() ) * f1_19;
      m22P = m22P - ( weight * f2_19.transpose() ) * f2_19;
      m12P = m12P - ( weight * f2_19.transpose() ) * f1_19;
    }
    else
    {
      x1P = x1P + ( f1[0] * f2 ) * f1_19;
      y1P = y1P + ( f1[1] * f2 ) * f1_19;
      z1P = z1P + ( f1[2] * f2 ) * f1_19;

      x2P = x2P + ( f1[0]) * f2 * f2_19;
      y2P = y2P + ( f1[1]) * f2 * f2_19;
      z2P = z2P + ( f1[2]) * f2 * f2_19;

      m11P = m11P - f1_19.transpose() * f1_19;
      m22P = m22P - f2_19.transpose() * f2_19;
      m12P = m12P - f2_19.transpose() * f1_19;
    }
  }

  Eigen::Vector3d pointsCenter1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d pointsCenter2 = Eigen::Vector3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    pointsCenter1 += adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]);
    pointsCenter2 += adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]);
  }

  pointsCenter1 = pointsCenter1 / numberCorrespondences;
  pointsCenter2 = pointsCenter2 / numberCorrespondences;

  Eigen::MatrixXd Hcross(3,3);
  Hcross = Eigen::Matrix3d::Zero();

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    Eigen::Vector3d f =      adapter.getCamRotation1(indices[i]) *
        adapter.getBearingVector1(indices[i]) - pointsCenter1;
    Eigen::Vector3d fprime = adapter.getCamRotation2(indices[i]) *
        adapter.getBearingVector2(indices[i]) - pointsCenter2;
    Hcross += fprime * f.transpose();
  }

  rotation_t startingRotation = math::arun(Hcross);

  //Do minimization
  modules::ge_main2(
      xxF, yyF, zzF, xyF, yzF, zxF,
      x1P, y1P, z1P, x2P, y2P, z2P,
      m11P, m12P, m22P, math::rot2cayley(startingRotation), output);

  //return output.rotation;
  transformation_t transformation;
  transformation.block<3,3>(0,0) = output.rotation;
  transformation.col(3) = output.translation.block<3,1>(0,0);

  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    geOutput_t & output,
    bool useWeights )
{
  Indices idx(adapter.getNumberCorrespondences());
  return ge(adapter,idx,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    geOutput_t & output,
    bool useWeights )
{
  Indices idx(indices);
  return ge(adapter,idx,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge( const RelativeAdapterBase & adapter, bool useWeights )
{
  geOutput_t output;
  //output.rotation = adapter.getR12(); //finding starting value using arun
  return ge(adapter,output,useWeights);
}

opengv::transformation_t
opengv::relative_pose::ge(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices,
    bool useWeights )
{
  geOutput_t output;
  //output.rotation = adapter.getR12(); //finding starting value using arun
  return ge(adapter,indices,output,useWeights);
}

namespace opengv
{
namespace relative_pose
{

transformation_t seventeenpt(
    const RelativeAdapterBase & adapter,
    const Indices & indices )
{
  size_t numberCorrespondences = indices.size();
  assert(numberCorrespondences > 16);

  Eigen::MatrixXd AE(numberCorrespondences,9);
  Eigen::MatrixXd AR(numberCorrespondences,9);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    bearingVector_t d1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t d2 = adapter.getBearingVector2(indices[i]);
    translation_t v1 = adapter.getCamOffset1(indices[i]);
    translation_t v2 = adapter.getCamOffset2(indices[i]);
    rotation_t R1 = adapter.getCamRotation1(indices[i]);
    rotation_t R2 = adapter.getCamRotation2(indices[i]);

    //unrotate the bearing-vectors to express everything in the body frame
    d1 = R1*d1;
    d2 = R2*d2;

    //generate the Plücker line coordinates
    Eigen::Matrix<double,6,1> l1;
    l1.block<3,1>(0,0) = d1;
    l1.block<3,1>(3,0) = v1.cross(d1);
    Eigen::Matrix<double,6,1> l2;
    l2.block<3,1>(0,0) = d2;
    l2.block<3,1>(3,0) = v2.cross(d2);

    //fill line of matrix A
    AE(i,0) = l2[0]*l1[0];
    AE(i,1) = l2[0]*l1[1];
    AE(i,2) = l2[0]*l1[2];
    AE(i,3) = l2[1]*l1[0];
    AE(i,4) = l2[1]*l1[1];
    AE(i,5) = l2[1]*l1[2];
    AE(i,6) = l2[2]*l1[0];
    AE(i,7) = l2[2]*l1[1];
    AE(i,8) = l2[2]*l1[2];

    AR(i,0) = l2[0]*l1[3]+l2[3]*l1[0];
    AR(i,1) = l2[0]*l1[4]+l2[3]*l1[1];
    AR(i,2) = l2[0]*l1[5]+l2[3]*l1[2];
    AR(i,3) = l2[1]*l1[3]+l2[4]*l1[0];
    AR(i,4) = l2[1]*l1[4]+l2[4]*l1[1];
    AR(i,5) = l2[1]*l1[5]+l2[4]*l1[2];
    AR(i,6) = l2[2]*l1[3]+l2[5]*l1[0];
    AR(i,7) = l2[2]*l1[4]+l2[5]*l1[1];
    AR(i,8) = l2[2]*l1[5]+l2[5]*l1[2];
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDARP(
      AR,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  Eigen::VectorXd sigma_ = SVDARP.singularValues();
  double pinvtoler_ =
      sigma_(0)*numberCorrespondences*NumTraits<double>::epsilon();
  Eigen::MatrixXd SigmaInverse_(9,9);
  SigmaInverse_ = Eigen::MatrixXd::Zero(9,9);
  for ( size_t i=0; i < 9; ++i)
  {
    double temp = sigma_(i);
    if( temp > pinvtoler_ )
      SigmaInverse_(i,i) = 1.0/temp;
  }

  Eigen::MatrixXd ARP(9,numberCorrespondences);
  ARP = SVDARP.matrixV()*SigmaInverse_*SVDARP.matrixU().transpose();

  Eigen::MatrixXd B(numberCorrespondences,numberCorrespondences);
  B = -Eigen::MatrixXd::Identity(numberCorrespondences,numberCorrespondences);
  B = B + AR*ARP;

  Eigen::MatrixXd C = B*AE;

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDE(
      C,
      Eigen::ComputeThinU | Eigen::ComputeThinV );
  Eigen::Matrix<double,9,1> e = SVDE.matrixV().col(8);

  Eigen::MatrixXd E_temp(3,3);
  E_temp.col(0) = e.block<3,1>(0,0);
  E_temp.col(1) = e.block<3,1>(3,0);
  E_temp.col(2) = e.block<3,1>(6,0);
  essential_t E = E_temp.transpose();

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDR(
      E,
      Eigen::ComputeFullV | Eigen::ComputeFullU );

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  W(0,1) = -1.0;
  W(1,0) = 1.0;
  W(2,2) = 1.0;

  // get possible rotation and translation vectors
  rotation_t Ra = SVDR.matrixU() * W * SVDR.matrixV().transpose();
  rotation_t Rb = SVDR.matrixU() * W.transpose() * SVDR.matrixV().transpose();

  // change sign if det = -1
  if( Ra.determinant() < 0 ) Ra = -Ra;
  if( Rb.determinant() < 0 ) Rb = -Rb;

  Ra.transposeInPlace();
  Rb.transposeInPlace();

  Eigen::MatrixXd A_tra(numberCorrespondences,3);
  Eigen::MatrixXd A_trb(numberCorrespondences,3);
  Eigen::VectorXd b_tra(numberCorrespondences);
  Eigen::VectorXd b_trb(numberCorrespondences);

  for( size_t i = 0; i < numberCorrespondences; i++ )
  {
    bearingVector_t d1 = adapter.getBearingVector1(indices[i]);
    bearingVector_t d2 = adapter.getBearingVector2(indices[i]);
    translation_t v1 = adapter.getCamOffset1(indices[i]);
    translation_t v2 = adapter.getCamOffset2(indices[i]);
    rotation_t R1 = adapter.getCamRotation1(indices[i]);
    rotation_t R2 = adapter.getCamRotation2(indices[i]);

    //unrotate the bearing-vectors to express everything in the body frame
    d1 = R1*d1;
    d2 = R2*d2;

    A_tra(i,0) = d1[2]*d2[0]*Ra(1,0)+d1[2]*d2[1]*Ra(1,1)+d1[2]*d2[2]*Ra(1,2)
                -d1[1]*d2[0]*Ra(2,0)-d1[1]*d2[1]*Ra(2,1)-d1[1]*d2[2]*Ra(2,2);
    A_tra(i,1) = d1[0]*d2[0]*Ra(2,0)+d1[0]*d2[1]*Ra(2,1)+d1[0]*d2[2]*Ra(2,2)
                -d1[2]*d2[0]*Ra(0,0)-d1[2]*d2[1]*Ra(0,1)-d1[2]*d2[2]*Ra(0,2);
    A_tra(i,2) = d1[1]*d2[0]*Ra(0,0)+d1[1]*d2[1]*Ra(0,1)+d1[1]*d2[2]*Ra(0,2)
                -d1[0]*d2[0]*Ra(1,0)-d1[0]*d2[1]*Ra(1,1)-d1[0]*d2[2]*Ra(1,2);

    A_trb(i,0) = d1[2]*d2[0]*Rb(1,0)+d1[2]*d2[1]*Rb(1,1)+d1[2]*d2[2]*Rb(1,2)
                -d1[1]*d2[0]*Rb(2,0)-d1[1]*d2[1]*Rb(2,1)-d1[1]*d2[2]*Rb(2,2);
    A_trb(i,1) = d1[0]*d2[0]*Rb(2,0)+d1[0]*d2[1]*Rb(2,1)+d1[0]*d2[2]*Rb(2,2)
                -d1[2]*d2[0]*Rb(0,0)-d1[2]*d2[1]*Rb(0,1)-d1[2]*d2[2]*Rb(0,2);
    A_trb(i,2) = d1[1]*d2[0]*Rb(0,0)+d1[1]*d2[1]*Rb(0,1)+d1[1]*d2[2]*Rb(0,2)
                -d1[0]*d2[0]*Rb(1,0)-d1[0]*d2[1]*Rb(1,1)-d1[0]*d2[2]*Rb(1,2);

    Eigen::Vector3d temp1 = v1.cross(d1);
    Eigen::Vector3d temp2 = v2.cross(d2);
    b_tra(i) = -d1.dot(Ra*temp2) -temp1.dot(Ra*d2);
    b_trb(i) = -d1.dot(Rb*temp2) -temp1.dot(Rb*d2);
  }

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDa(
      A_tra,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  Eigen::VectorXd sigma = SVDa.singularValues();
  double pinvtoler =
      numberCorrespondences*sigma(0)*NumTraits<double>::epsilon();
  Eigen::MatrixXd SigmaInverse(3,3);
  SigmaInverse = Eigen::MatrixXd::Zero(3,3);
  for ( size_t i=0; i < 3; ++i)
  {
    double temp = sigma(i);
    if( temp > pinvtoler )
      SigmaInverse(i,i) = 1.0/temp;
  }

  Eigen::MatrixXd PI(3,numberCorrespondences);
  PI = SVDa.matrixV()*SigmaInverse*SVDa.matrixU().transpose();
  Eigen::Vector3d ta = PI*b_tra;

  Eigen::JacobiSVD< Eigen::MatrixXd > SVDb(
      A_trb,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  sigma = SVDb.singularValues();
  pinvtoler = numberCorrespondences*sigma(0)*NumTraits<double>::epsilon();
  SigmaInverse = Eigen::MatrixXd::Zero(3,3);
  for ( size_t i=0; i < 3; ++i)
  {
    double temp = sigma(i);
    if( temp > pinvtoler )
      SigmaInverse(i,i) = 1.0/temp;
  }

  PI = SVDb.matrixV()*SigmaInverse*SVDb.matrixU().transpose();
  Eigen::Vector3d tb = PI*b_trb;

  Eigen::VectorXd fita = A_tra * ta - b_tra;
  Eigen::VectorXd fitb = A_trb * tb - b_trb;

  transformation_t transformation;
  if( fita.norm() < fitb.norm() )
  {
    transformation.block<3,3>(0,0) = Ra;
    transformation.col(3) = ta;
  }
  else
  {
    transformation.block<3,3>(0,0) = Rb;
    transformation.col(3) = tb;
  }

  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::seventeenpt( const RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return seventeenpt(adapter,idx);
}

opengv::transformation_t
opengv::relative_pose::seventeenpt(
    const RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return seventeenpt(adapter,idx);
}

namespace opengv
{
namespace relative_pose
{

struct OptimizeNonlinearFunctor1 : OptimizationFunctor<double>
{
  RelativeAdapterBase & _adapter;
  const Indices & _indices;

  OptimizeNonlinearFunctor1(
      RelativeAdapterBase & adapter,
      const Indices & indices ) :
      OptimizationFunctor<double>(6,indices.size()),
      _adapter(adapter),
      _indices(indices) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == 6 );
    assert( (unsigned int) fvec.size() == _indices.size());

    //compute the current position
    translation_t translation = x.block<3,1>(0,0);
    cayley_t cayley = x.block<3,1>(3,0);
    rotation_t rotation = math::cayley2rot(cayley);

    Eigen::Matrix<double,4,1> p_hom;
    p_hom[3] = 1.0;

    for( size_t i = 0; i < _indices.size(); i++ )
    {
      translation_t cam1Offset = _adapter.getCamOffset1(_indices[i]);
      rotation_t cam1Rotation = _adapter.getCamRotation1(_indices[i]);
      translation_t cam2Offset = _adapter.getCamOffset2(_indices[i]);
      rotation_t cam2Rotation = _adapter.getCamRotation2(_indices[i]);

      translation_t directTranslation =
          cam1Rotation.transpose() *
          ((translation - cam1Offset) + rotation * cam2Offset);
      rotation_t directRotation =
          cam1Rotation.transpose() * rotation * cam2Rotation;

      _adapter.sett12(directTranslation);
      _adapter.setR12(directRotation);

      transformation_t inverseSolution;
      inverseSolution.block<3,3>(0,0) = directRotation.transpose();
      inverseSolution.col(3) =
          -inverseSolution.block<3,3>(0,0)*directTranslation;

      p_hom.block<3,1>(0,0) =
          opengv::triangulation::triangulate2(_adapter,_indices[i]);
      bearingVector_t reprojection1 = p_hom.block<3,1>(0,0);
      bearingVector_t reprojection2 = inverseSolution * p_hom;
      reprojection1 = reprojection1 / reprojection1.norm();
      reprojection2 = reprojection2 / reprojection2.norm();
      bearingVector_t f1 = _adapter.getBearingVector1(_indices[i]);
      bearingVector_t f2 = _adapter.getBearingVector2(_indices[i]);

      //bearing-vector based outlier criterium (select threshold accordingly):
      //1-(f1'*f2) = 1-cos(alpha) \in [0:2]
      double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
      double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
      double factor = 1.0;
      fvec[i] = factor*(reprojError1 + reprojError2);
    }

    return 0;
  }
};

transformation_t optimize_nonlinear(
    RelativeAdapterBase & adapter,
    const Indices & indices )
{
  const int n=6;
  VectorXd x(n);

  x.block<3,1>(0,0) = adapter.gett12();
  x.block<3,1>(3,0) = math::rot2cayley(adapter.getR12());

  OptimizeNonlinearFunctor1 functor( adapter, indices );
  NumericalDiff<OptimizeNonlinearFunctor1> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<OptimizeNonlinearFunctor1> >
      lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  transformation_t transformation;
  transformation.col(3) = x.block<3,1>(0,0);
  transformation.block<3,3>(0,0) = math::cayley2rot(x.block<3,1>(3,0));
  return transformation;
}

}
}

opengv::transformation_t
opengv::relative_pose::optimize_nonlinear( RelativeAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return optimize_nonlinear(adapter,idx);
}

opengv::transformation_t
opengv::relative_pose::optimize_nonlinear(
    RelativeAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return optimize_nonlinear(adapter,idx);
}


#include <assert.h>
#include <unistd.h>

#include <sstream>
//#include <boost/algorithm/string.hpp>
#include <algorithm> // for is_permutation
#include <tuple>
#include <functional>
#include <iterator>

#include "./permutation.h"

#include "./dm_english.h"
#include "./dm_perm.h"
#include "./sagemath_alter.h"

Permutation::Permutation(){}

Permutation::Permutation(const int n)
{
	if(n==-1) non_perm();
	else identify(n);
}

Permutation::Permutation(const std::vector<int> vec)
{
	std::vector<int>::const_iterator it;
	for(it = vec.begin() ; it != vec.end() ; ++it){
		perm.push_back(*it);
	}
	label.resize( length() );
	computeInverse();
// 	canonization();
}

// construct a canonical permutation for G (induced by modules)
// for parallel, series, prime modules.
Permutation::Permutation(const int n, const ModuleType type)
{
	switch(type){
	case FEUILLE:
		identify(1);
		break;
		      /*
		    case PREMIER:
			// This case will not be reached
		      prime(G);
		      break;
		      */
	case PARALLELE:
		identify(n);
		break;
	case SERIE:
		reverse(n);
		break;
	default:
		std::cerr << "Error! Strange the module type.\n\n";
	}
}

  // For a prime module.
  // ここで sagemath を使っている．
  // sagemath を使わなければ，1000倍は早くなるはず．
void Permutation::prime(const graphe G)
{
	std::vector< std::vector< int > > adj_matrix( G.n, std::vector< int >( G.n ) );
	for ( int i = 0; i < G.n; ++i )
	{
		for ( adj* adj_list = G.G[i]; adj_list != NULL; adj_list = adj_list->suiv )
		{
			const int j = adj_list->s;
			adj_matrix[i][j] = adj_matrix[j][i] = 1;
		}
	}

	std::vector< int > vec1, vec2;
	std::tie( vec1, vec2 ) = is_permutation_certificate( adj_matrix );
	if ( vec1.empty() )
	{
		non_perm();
		return;
	}

// #define D_PRIME

	label = vec1;
	// 正規化（一方の permutation を(1, 2, 3, ..., n)に変換）し，perm に格納
	std::vector<int> v1(vec1.size());
	for(int i=0 ; i<vec1.size() ; i++){
		v1[vec1[i]-1] = i+1;
	}
	for(int i=0 ; i<vec2.size() ; i++){
		perm.push_back(v1[vec2[i]-1]);
	}
	// -----（ここまで）出力データから二つの permutation を読み込んで正規化 -----

	canonization(); // 標準形に変換

}

void Permutation::identify(const int n)
{
	for(int i=0 ; i<n ; i++){
		perm.push_back(i+1);
	}
	computeInverse();
}

void Permutation::reverse(const int n)
{
	for(int i=n ; i>0 ; i--){
		perm.push_back(i);
	}
	computeInverse();
}

void Permutation::non_perm(){
	std::vector<int>().swap(perm);
	perm.push_back(-1);
}

  /******************************************/

void Permutation::canonization()
{
	std::vector<int> vecH = computeHorizontalFlip();
	std::vector<int> vecV = computeVerticalFlip();
	std::vector<int> vecR = computeRotation();

	computeInverse();

	if(is_SmallerEqual(vecH) && is_SmallerEqual(vecV) && is_SmallerEqual(vecR)){
		return;
	}

	if(is_small(vecH, vecV)){ // checked vecH < vecV
		if(is_small(vecH, vecR)){ // vecH < vecR
			perm = vecH;
			computeHorizontalFlip_label();
		}
		else{ // vecR < vecH
			perm = vecR;
			computeRotation_label();
		}
	}
	else{ // vecV < vecH
		if(is_small(vecV, vecR)){ // vecV < vecR
			perm = vecV;
			computeVerticalFlip_label();
		}
		else{ // vecR < vecV
			perm = vecR;
			computeRotation_label();
		}
	}

	computeInverse();
}

void Permutation::computeInverse()
{
	inverse.resize( length() );
	for ( int i = 0; i < length(); ++i )
	{
		inverse[ perm[i] - 1 ] = i + 1;
	}
	return;
}

void Permutation::computeHorizontalFlip_label()
{
// 	cerr << "H-flip" << endl;
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[pi(i)-1] = label[i];
// 		cerr << i << " -> " << perm[i] << endl;
	}
	label = vec;
}

std::vector<int> Permutation::computeHorizontalFlip() const
{
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[pi(i)-1] = i+1;
	}
	//perm = vec;
	return vec;
}

void Permutation::computeVerticalFlip_label()
{
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[length()-i-1] = label[i];
	}
	label = vec;
}

std::vector<int> Permutation::computeVerticalFlip() const
{
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[length()-i-1] = length() - pi(i) + 1;
	}
	//perm = vec;
	return vec;
}

std::vector<int> Permutation::computeVerticalFlip(std::vector<int> p) const
{
	assert(p.size()==length());
	std::vector<int> vec(length());
	for(int i=0 ; i<length() ; i++){
		vec[length()-i-1] = length() - p[i] + 1;
	}
	//perm = vec;
	return vec;
}

void Permutation::computeRotation_label()
{
	vector< int > vec( length() );
	for ( int i = 0; i < length(); ++i )
	{
		vec[ length() - inverse[i] ] = label[i];
	}
	label = vec;

// 	computeHorizontalFlip_label();
// 	computeVerticalFlip_label();
}

std::vector<int> Permutation::computeRotation() const
{
	std::vector<int> vecH = computeHorizontalFlip();
	std::vector<int> vec = computeVerticalFlip(vecH);
	return vec;
}

bool Permutation::is_canonical()
{
	Permutation p(perm);
	p.canonization();
	if(is_Smaller(p.perm)) return true;
	else return false;
}

bool Permutation::is_v_symmetry() const
{
	for(int i=0 ; i<length()/2 ; i++){
		if (perm[i] != length() - perm[length()-i-1] + 1){
// 			cerr << "i : " << i << endl;
// 			cerr << "perm[i] : " << perm[i] << endl;
// 			cerr << "length() - perm[length()-i-1] + 1) : " << length() - perm[length()-i-1] + 1 << endl;
			return false;
		}
	}
	return true;
}

bool Permutation::is_h_symmetry() const
{
	for ( int i = 0; i < length(); ++i )
	{
		if ( i != perm[ perm[i] - 1 ] - 1 )
		{
			return false;
		}
	}
	return true;
}

bool Permutation::is_rotational_symmetry() const
{
	std::vector<int> vecR = computeRotation();
	return is_equal(vecR);
}

bool Permutation::is_permutation()
{
	//std::cout << perm[0] << std::endl;
	if(perm[0] == -1) return false;
	else return true;
}

int Permutation::length() const
{
	return perm.size();
}

int Permutation::pi(int k) const
{
	return perm[k];
}

bool Permutation::operator==(const Permutation& p) const
{
	if(length() != p.length()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] != p.pi(i)) return false;
	}
	return true;
}

bool Permutation::operator==(const std::vector<int>& vec) const
{
	if(length() != vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] != vec[i]) return false;
	}
	return true;
}

bool Permutation::is_equal(const std::vector<int>& vec) const
{
	if(length() != vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] != vec[i]) return false;
	}
	return true;
}

bool Permutation::operator<(const Permutation& p) const {

	if(length() < p.length()) return true;
	if(length() > p.length()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < p.pi(i)) return true;
		if(perm[i] > p.pi(i)) return false;
	}
	return false;
}

bool Permutation::is_Smaller(const std::vector<int>& vec) const
{
	if(length() < vec.size()) return true;
	if(length() > vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < vec[i]) return true;
		if(perm[i] > vec[i]) return false;
	}
	return false;
}

bool Permutation::is_SmallerEqual(const std::vector<int>& vec) const
{
	if(length() < vec.size()) return true;
	if(length() > vec.size()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < vec[i]) return true;
		if(perm[i] > vec[i]) return false;
	}
	return true;
}

bool Permutation::is_Smaller(const Permutation& p) const
{
	if(length() < p.length()) return true;
	if(length() > p.length()) return false;

	for(int i=0 ; i<length() ; i++){
		if(perm[i] < p.pi(i)) return true;
		if(perm[i] > p.pi(i)) return false;
	}
	return false;
}

bool Permutation::is_small(const std::vector<int> v1, const std::vector<int> v2) const
{
	if(v1.size() < v2.size()) return true;
	if(v1.size() > v2.size()) return false;

	for(int i=0 ; i<v1.size() ; i++){
		if(v1[i] < v2[i]) return true;
		if(v1[i] > v2[i]) return false;
	}
	return false;
}

bool Permutation::is_smallequal(const std::vector<int> v1, const std::vector<int> v2) const
{
	if(v1.size() < v2.size()) return true;
	if(v1.size() > v2.size()) return false;

	for(int i=0 ; i<v1.size() ; i++){
		if(v1[i] < v2[i]) return true;
		if(v1[i] > v2[i]) return false;
	}
	return true;
}

// for Prime module
void Permutation::merge(const std::vector<Permutation> p)
{

	vector< vector< int > > perms;
	for ( int i = 0; i < p.size(); ++i )
	{
		perms.push_back( p[i].perm );
	}

	vector< int > psum( length() + 1 );
	for ( int i = 0; i < length(); ++i )
	{
		psum[ i + 1 ] = perms[i].size();
	}
	//partial_sum( begin( psum ), end( psum ), begin( psum ) );

	for ( int i = 0, s = 0; i < length(); s += perms[ inverse[ i++ ] - 1 ].size() )
	{
		// 		cerr << "perms[ " << inverse[i] - 1 << " ]" << endl;
		// 		cerr << s << endl;
		transform( begin( perms[ inverse[i] - 1 ] ), end( perms[ inverse[i] - 1 ] ), begin( perms[ inverse[i] - 1 ] ), bind( plus< int >(), placeholders::_1, s ) );
	}

	vector< int > vec;
	for ( const auto &pp : perms )
	{

		copy( begin( pp ), end( pp ), back_inserter( vec ) );
// 				copy( begin( pp ), end( pp ), ostream_iterator< int >( cerr, " " ) );
// 				cerr << endl;
	}

	perm = vec;
	label.resize( perm.size() );
}

Permutation Permutation::parentPermutation() const
{
	std::vector< int > vec( perm );
	for ( int i = 0; i + 1 < length(); ++i )
	{
		if ( vec[i] < vec[ i + 1 ] )
		{
			std::swap( vec[i], vec[ i + 1 ] );
			break;
		}
	}

	Permutation p(vec);
// 	cerr << "parent permutation from : ";
// 	p.print();
	graphe G = p.makeGraphe();
	noeud *root = decomposition_modulaire( G );
	Permutation pp = createPermutation( root, G );
	return pp;
}

graphe Permutation::makeGraphe() const
{
	graphe G;
	G.n = length();
	G.G=(adj **)malloc(G.n*sizeof(adj *));
	for(int i=0 ; i<G.n ; i++) G.G[i] = NULL;

	for(int i=0 ; i<length() ; i++){
		for(int j=i+1 ; j<length() ; j++){
			if(perm[i] > perm[j]){
				// add an edge perm[i] to perm[j] (and its reversal).
				adj *a = (adj *)malloc(sizeof(adj));
				a->s = perm[j]-1;
				a->suiv = G.G[perm[i]-1];
				G.G[perm[i]-1] = a;

				a = (adj *)malloc(sizeof(adj)); a->s = perm[i]-1;
				a->suiv = G.G[perm[j]-1];
				G.G[perm[j]-1] = a;

			}
		}
	}
	return G;
}

void Permutation::print() const
{
	if(length()<1){
		std::cout << "length 0" << std::endl;
		return;
	}

	std::vector<int>::const_iterator it;
	for(it = perm.begin() ; it != perm.end() ; ++it){
		std::cout << *it << ( it + 1 != perm.end() ? ' ' : '\n' );
	}
	std::cout << std::flush;
}

void Permutation::print(std::ofstream &ofs) const
{
	if(length()<1){
		ofs << "length 0" << std::endl;
		return;
	}

	std::vector<int>::const_iterator it;
	for(it = perm.begin() ; it != perm.end() ; ++it){
		ofs << *it << ( it + 1 != perm.end() ? ' ' : '\n' );
	}
	ofs << std::flush;
}

void Permutation::print(std::ofstream &ofs, std::vector<int> vec) const
{
	if(vec.size()<1){
		ofs << "length 0" << std::endl;
		return;
	}

	std::vector<int>::const_iterator it;
	for(it = vec.begin() ; it != vec.end() ; ++it){
		ofs << *it << ( it + 1 != vec.end() ? ' ' : '\n' );
	}
	ofs << std::flush;
}

void Permutation::print(std::vector<int> vec)
{
	for(int i=0 ; i<vec.size() ; i++){
		std::cout << vec[i] << ( i + 1 < vec.size() ? ' ' : '\n' );
	}
	std::cout << std::endl;
}


#include <PCP/Geometry/Normal.h>
#include <PCP/Geometry/Geometry.h>

#include <PCP/Common/Assert.h>
#include <PCP/Common/Progress.h>
#include <PCP/Common/Log.h>

#include <PCP/SpacePartitioning/KnnGraph.h>

#include <set>

#include <Eigen/Eigenvalues>

// https://www.geeksforgeeks.org/kruskals-minimum-spanning-tree-using-stl-in-c/
namespace mst {

using namespace std;

// Creating shortcut for an integer pair
typedef pair<int, int> iPair;

// Structure to represent a graph
struct Graph
{
    int V, E;
    vector< pair<float, iPair> > edges;

    // Constructor
    Graph(int V, int E)
    {
        this->V = V;
        this->E = E;
    }

    // Utility function to add an edge
    void addEdge(int u, int v, float w)
    {
        edges.push_back({w, {u, v}});
    }

    // Function to find MST using Kruskal's
    // MST algorithm
    float kruskalMST(std::vector<iPair>& result);
};

// To represent Disjoint Sets
struct DisjointSets
{
    int *parent, *rnk;
    int n;

    // Constructor.
    DisjointSets(int n)
    {
        // Allocate memory
        this->n = n;
        parent = new int[n+1];
        rnk = new int[n+1];

        // Initially, all vertices are in
        // different sets and have rank 0.
        for (int i = 0; i <= n; i++)
        {
            rnk[i] = 0;

            //every element is parent of itself
            parent[i] = i;
        }
    }

    // Find the parent of a node 'u'
    // Path Compression
    int find(int u)
    {
        /* Make the parent of the nodes in the path
           from u--> parent[u] point to parent[u] */
        if (u != parent[u])
            parent[u] = find(parent[u]);
        return parent[u];
    }

    // Union by rank
    void merge(int x, int y)
    {
        x = find(x), y = find(y);

        /* Make tree with smaller height
           a subtree of the other tree  */
        if (rnk[x] > rnk[y])
            parent[y] = x;
        else // If rnk[x] <= rnk[y]
            parent[x] = y;

        if (rnk[x] == rnk[y])
            rnk[y]++;
    }
};

 /* Functions returns weight of the MST*/

float Graph::kruskalMST(std::vector<iPair>& result)
{
    result.clear();
    float mst_wt = 0; // Initialize result

    // Sort edges in increasing order on basis of cost
    sort(edges.begin(), edges.end());

    // Create disjoint sets
    DisjointSets ds(V);

    // Iterate through all sorted edges
    vector< pair<float, iPair> >::iterator it;
    for (it=edges.begin(); it!=edges.end(); it++)
    {
        int u = it->second.first;
        int v = it->second.second;

        int set_u = ds.find(u);
        int set_v = ds.find(v);

        // Check if the selected edge is creating
        // a cycle or not (Cycle is created if u
        // and v belong to same set)
        if (set_u != set_v)
        {
            // Current edge will be in the MST
            // so print it
//            cout << u << " - " << v << endl;
            result.push_back(std::make_pair(u, v));

            // Update MST weight
            mst_wt += it->first;

            // Merge two sets
            ds.merge(set_u, set_v);
        }
    }

    return mst_wt;
}

// for my std::set
struct Comp
{
    // compare only the first index
    bool operator()(const pair<float, iPair>& e1, const pair<float, iPair>& e2)
    {
        return e1.second.first < e2.second.first;
    }
};

} // namespace mst

namespace pcp {

void compute_normals(Geometry& g, bool v)
{
    PCP_ASSERT(g.has_knn_graph());

    const int point_count = g.size();

    info().iff(v) << "0/5 Computing normals";
    internal::compute_unoriented_normals(g, v);

    info().iff(v) << "1/5 Extracting unique edges";
    std::set<std::pair<int,int>> edges;
    auto prog = Progress(point_count, v);
    for(int i=0; i<point_count; ++i)
    {
        for(int j : g.knn_graph().k_nearest_neighbors(i))
        {
            auto e = std::make_pair(i,j);
            if(e.first > e.second) std::swap(e.first, e.second);

            edges.insert(e);
        }
        ++prog;
    }
    info().iff(v) << "  " << edges.size() << " edges found";

    info().iff(v) << "2/5 Building graph for mst";
    mst::Graph graph(point_count, edges.size());
    for(const auto& e : edges)
    {
        const int i = e.first;
        const int j = e.second;
        const Vector3& n_i = g.normal(i);
        const Vector3& n_j = g.normal(j);
        const Scalar w = 1 - std::abs(n_i.dot(n_j));
        graph.addEdge(i, j, w);
    }

    info().iff(v) << "3/5 Computing mst";
    std::vector<mst::iPair> result;
    graph.kruskalMST(result);
    info().iff(v) << "  " << result.size() << " edges found in mst";

    info().iff(v) << "4. Building mst";
    std::vector<std::vector<int>> mst(point_count);
    for(const auto& e : result)
    {
        mst[e.first].push_back(e.second);
        mst[e.second].push_back(e.first);
    }

    info().iff(v) << "5/5 Propagate orientention";
    int topmost = 0;
    for(int i=0; i<point_count; ++i)
    {
        if(g[i].z() > g[topmost].z()) topmost = i;
    }
    std::stack<mst::iPair> stack;
    std::vector<bool> traversed(point_count, false);
    traversed[topmost] = true;
    for(int i : mst[topmost]) stack.push(std::make_pair(topmost, i));

    while(!stack.empty())
    {
        const auto e = stack.top();
        stack.pop();

        const int source = e.first;
        const int target = e.second;

        const Vector3& n_source = g.normal(source);
        Vector3& n_target = g.normal(target);

        if(n_source.dot(n_target) < 0) n_target *= -1;

        traversed[target] = true;

        for(int i : mst[target]) if(!traversed[i]) stack.push(std::make_pair(target, i));
    }
}

void compute_normals_robust(Geometry& g, float sigma, int iter, bool v)
{
    PCP_ASSERT(g.has_knn_graph());

    const int point_count = g.size();

    info().iff(v) << "0/5 Computing normals";
    internal::compute_unoriented_normals_robust(g, sigma, iter, v);

    info().iff(v) << "1/5 Extracting unique edges";
    std::set<std::pair<int,int>> edges;
    auto prog = Progress(point_count, v);
    for(int i=0; i<point_count; ++i)
    {
        for(int j : g.knn_graph().k_nearest_neighbors(i))
        {
            auto e = std::make_pair(i,j);
            if(e.first > e.second) std::swap(e.first, e.second);

            edges.insert(e);
        }
        ++prog;
    }
    info().iff(v) << "  " << edges.size() << " edges found";

    info().iff(v) << "2/5 Building graph for mst";
    mst::Graph graph(point_count, edges.size());
    for(const auto& e : edges)
    {
        const int i = e.first;
        const int j = e.second;
        const Vector3& n_i = g.normal(i);
        const Vector3& n_j = g.normal(j);
        const Scalar w = 1 - std::abs(n_i.dot(n_j));
        graph.addEdge(i, j, w);
    }

    info().iff(v) << "3/5 Computing mst";
    std::vector<mst::iPair> result;
    graph.kruskalMST(result);
    info().iff(v) << "  " << result.size() << " edges found in mst";

    info().iff(v) << "4. Building mst";
    std::vector<std::vector<int>> mst(point_count);
    for(const auto& e : result)
    {
        mst[e.first].push_back(e.second);
        mst[e.second].push_back(e.first);
    }

    info().iff(v) << "5/5 Propagate orientention";
    int topmost = 0;
    for(int i=0; i<point_count; ++i)
    {
        if(g[i].z() > g[topmost].z()) topmost = i;
    }
    std::stack<mst::iPair> stack;
    std::vector<bool> traversed(point_count, false);
    traversed[topmost] = true;
    for(int i : mst[topmost]) stack.push(std::make_pair(topmost, i));

    while(!stack.empty())
    {
        const auto e = stack.top();
        stack.pop();

        const int source = e.first;
        const int target = e.second;

        const Vector3& n_source = g.normal(source);
        Vector3& n_target = g.normal(target);

        if(n_source.dot(n_target) < 0) n_target *= -1;

        traversed[target] = true;

        for(int i : mst[target]) if(!traversed[i]) stack.push(std::make_pair(target, i));
    }
}

namespace internal {

void compute_unoriented_normals(Geometry& g, bool v)
{
    const int point_count = g.size();
    const int K = g.knn_graph().k();

    g.request_normals();

    auto prog = Progress(point_count, v);

    #pragma omp parallel for
    for(int i=0; i<point_count; ++i)
    {
        Matrix3 C = Matrix3::Zero();
        Vector3 m = Vector3::Zero();

        for(int j : g.knn_graph().k_nearest_neighbors(i))
        {
            const Vector3 p = g[j] - g[i];

            C += p * p.transpose();
            m += p;
        }
        m /= K;
        C = C/K - m * m.transpose();

        Eigen::SelfAdjointEigenSolver<Matrix3> eig(C);
        g.normal(i) = eig.eigenvectors().col(0);

        ++prog;
    }
}

namespace internal {
Scalar robust_weight(const Vector3& p, Scalar uc, const Vector3& ul, Scalar sigma)
{
    const Scalar f = uc + ul.dot(p);
    return std::exp(-f*f/(sigma*sigma));
}
} // namespace internal

void compute_unoriented_normals_robust(Geometry& g, float sigma, int iter, bool v)
{
    const int point_count = g.size();
//    const int K = g.knn_graph().k();

    g.request_normals();

    auto prog = Progress(point_count, v);

    #pragma omp parallel for
    for(int i=0; i<point_count; ++i)
    {
        Scalar  uc = 0;
        Vector3 ul = Vector3::Zero();

        for(int n=0; n<iter; ++n)
        {
            Matrix3 C = Matrix3::Zero();
            Vector3 m = Vector3::Zero();
            Scalar sum_w = 0;

            for(int j : g.knn_graph().k_nearest_neighbors(i))
            {
                const Vector3 p = g[j] - g[i];
                const Scalar w = n == 0 ? 1 : internal::robust_weight(p, uc, ul, sigma);

                C += w * p * p.transpose();
                m += w * p;
                sum_w += w;
            }

            if(sum_w <= 0)
            {
                warning().iff(v) << "sum_w=0 at i=" << i << " (n=" << n << ")";
//                PCP_ERROR;
                break;
            }

            m /= sum_w;
            C = C/sum_w - m * m.transpose();

            Eigen::SelfAdjointEigenSolver<Matrix3> eig(C);
            ul = eig.eigenvectors().col(0);
            uc = - ul.dot(m);
        }

        g.normal(i) = ul;

        ++prog;
    }
}

} // namespace internal

} // namespace pcp


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

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

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

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/concepts/multi_linestring_concept.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST

template <typename P>
bg::model::linestring<P> create_linestring()
{
    bg::model::linestring<P> l1;
    P p1(1, 2);
    bg::append(l1, p1);
    return l1;
}

template <typename P, typename L>
bg::model::multi_linestring<L> create_multi_linestring()
{
    bg::model::multi_linestring<L> ml1;
    L l1(create_linestring<P>());
    ml1.push_back(l1);
    ml1.push_back(l1);
    return ml1;
}

template <typename ML, typename L>
void check_multi_linestring(ML& to_check, L l1)
{
    ML cur;
    cur.push_back(l1);
    cur.push_back(l1);

    std::ostringstream out1, out2;
    out1 << bg::dsv(to_check);
    out2 << bg::dsv(cur);
    BOOST_CHECK_EQUAL(out1.str(), out2.str());
}

template <typename P, typename L>
void test_default_constructor()
{
    bg::model::multi_linestring<L> ml1(create_multi_linestring<P, L>());
    check_multi_linestring(ml1, L(create_linestring<P>()));
}

template <typename P, typename L>
void test_copy_constructor()
{
    bg::model::multi_linestring<L> ml1 = create_multi_linestring<P, L>();
    check_multi_linestring(ml1, L(create_linestring<P>()));
}

template <typename P, typename L>
void test_copy_assignment()
{
    bg::model::multi_linestring<L> ml1(create_multi_linestring<P, L>()), ml2;
    ml2 = ml1;
    check_multi_linestring(ml2, L(create_linestring<P>()));
}

template <typename L>
void test_concept()
{
    typedef bg::model::multi_linestring<L> ML;

    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstMultiLinestring<ML>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::MultiLinestring<ML>) );

    typedef typename bg::coordinate_type<ML>::type T;
    typedef typename bg::point_type<ML>::type PML;
    boost::ignore_unused<T, PML>();
}

template <typename P>
void test_all()
{
    typedef bg::model::linestring<P> L;

    test_default_constructor<P, L>();
    test_copy_constructor<P, L>();
    test_copy_assignment<P, L>();
    test_concept<L>();
}

template <typename P>
void test_custom_multi_linestring(bg::model::linestring<P> IL)
{
    typedef bg::model::linestring<P> L;

    std::initializer_list<L> LIL = {IL};
    bg::model::multi_linestring<L> ml1(LIL);
    std::ostringstream out;
    out << bg::dsv(ml1);
    BOOST_CHECK_EQUAL(out.str(), "(((1, 1), (2, 2), (3, 3), (0, 0), (0, 2), (0, 3)))");
}

template <typename P>
void test_custom()
{
#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    std::initializer_list<P> IL1 = {P(1, 1), P(2, 2), P(3, 3)};
    std::initializer_list<P> IL2 = {P(0, 0), P(0, 2), P(0, 3)};
    bg::model::linestring<P> l1;
    bg::append(l1, IL1);
    bg::append(l1, IL2);
    test_custom_multi_linestring<P>(l1);
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST
}

template <typename CS>
void test_cs()
{
    test_all<bg::model::point<int, 2, CS> >();
    test_all<bg::model::point<float, 2, CS> >();
    test_all<bg::model::point<double, 2, CS> >();

    test_custom<bg::model::point<double, 2, CS> >();
}

int test_main(int, char* [])
{
    test_cs<bg::cs::cartesian>();
    test_cs<bg::cs::spherical<bg::degree> >();
    test_cs<bg::cs::spherical_equatorial<bg::degree> >();
    test_cs<bg::cs::geographic<bg::degree> >();

    test_custom<bg::model::d2::point_xy<double> >();

    return 0;
}


#include <iostream>
#include <boost/program_options.hpp>

#include "distr.h"
#include "management.h"

using namespace std;

namespace po = boost::program_options;

int main(int argc, char *argv[]) {
    double mu;
    double si;
    uint32_t minconnections;

    po::options_description desc("Allowed options");
    desc.add_options()
            ("help", "produce help message")
            ("m",po::value<double>(&mu)->default_value(4.5),"initial µ of lognormal distribution")
            ("s",po::value<double>(&si)->default_value(1),"initial sigma of lognormal distribution")
            ("measures",po::value<uint32_t>(&minconnections)->default_value(8),"amount of required measurements")
            ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

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

    // preparation
    distr sampledGuess(mu,si);

    //distr formulaGuess(mu,si);

    // Main Program
    management mngr;
    uint64_t counter = 0;
    std::string line;
    while(std::getline(std::cin,line)) {
        size_t c1 = line.find(',');
        size_t c2 = line.rfind(',');

        auto timestamp = std::stoull(line.substr(0,c1));
        // we do not need the host id
        // auto hostid = line.substr(c1+1,c2-c1-1);
        auto txid = line.substr(c2+1,line.length()-c2-1);

        auto len = mngr.addTx(timestamp, txid);
        if(len>= minconnections) {
            auto measures = mngr.popTx(txid);

            // adaption process
            normalise(measures);
            updateMuSampled(sampledGuess, measures, 100, 10);
            //updateMuFormula(formulaGuess, measures);

            updateSigma(sampledGuess, measures, sampledGuess.mu);
            //updateSigma(formulaGuess, measures, formulaGuess.mu);

            // output updated values
            //cout << counter << "\t";
            cout << timestamp << ",";
            cout << sampledGuess.mu << "," << sampledGuess.si;
            //cout << "\t|\tµ: " << formulaGuess.mu << " \tsigma: " << formulaGuess.si;
            //cout << "," << txid;
            cout << endl;
            counter ++;
        }
    }

    return 0;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/divround.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( idivround_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          , bd::generic_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return divround(a0, a1);
    }
  };

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( idivround_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return iround(a0/a1);
    }
  };

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

} } }
#endif