// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

// The computeRoots function included in this is based on materials
// covered by the following copyright and license:
//
// Geometric Tools, LLC
// Copyright (c) 1998-2010
// Distributed under the Boost Software License, Version 1.0.
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

using namespace Eigen;
using namespace std;

template<typename Matrix, typename Roots>
inline void computeRoots(const Matrix& m, Roots& roots)
{
  typedef typename Matrix::Scalar Scalar;
  const Scalar s_inv3 = 1.0/3.0;
  const Scalar s_sqrt3 = internal::sqrt(Scalar(3.0));

  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
  // eigenvalues are the roots to this equation, all guaranteed to be
  // real-valued, because the matrix is symmetric.
  Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(0,1)*m(0,2)*m(1,2) - m(0,0)*m(1,2)*m(1,2) - m(1,1)*m(0,2)*m(0,2) - m(2,2)*m(0,1)*m(0,1);
  Scalar c1 = m(0,0)*m(1,1) - m(0,1)*m(0,1) + m(0,0)*m(2,2) - m(0,2)*m(0,2) + m(1,1)*m(2,2) - m(1,2)*m(1,2);
  Scalar c2 = m(0,0) + m(1,1) + m(2,2);

  // Construct the parameters used in classifying the roots of the equation
  // and in solving the equation for the roots in closed form.
  Scalar c2_over_3 = c2*s_inv3;
  Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
  if (a_over_3 > Scalar(0))
    a_over_3 = Scalar(0);

  Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));

  Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
  if (q > Scalar(0))
    q = Scalar(0);

  // Compute the eigenvalues by solving for the roots of the polynomial.
  Scalar rho = internal::sqrt(-a_over_3);
  Scalar theta = std::atan2(internal::sqrt(-q),half_b)*s_inv3;
  Scalar cos_theta = internal::cos(theta);
  Scalar sin_theta = internal::sin(theta);
  roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
  roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
  roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);

  // Sort in increasing order.
  if (roots(0) >= roots(1))
    std::swap(roots(0),roots(1));
  if (roots(1) >= roots(2))
  {
    std::swap(roots(1),roots(2));
    if (roots(0) >= roots(1))
      std::swap(roots(0),roots(1));
  }
}

template<typename Matrix, typename Vector>
void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals)
{
  typedef typename Matrix::Scalar Scalar;
  // Scale the matrix so its entries are in [-1,1].  The scaling is applied
  // only when at least one matrix entry has magnitude larger than 1.

  Scalar scale = mat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff();
  scale = std::max(scale,Scalar(1));
  Matrix scaledMat = mat / scale;

  // Compute the eigenvalues
//   scaledMat.setZero();
  computeRoots(scaledMat,evals);

  // compute the eigen vectors
  // **here we assume 3 differents eigenvalues**

  // "optimized version" which appears to be slower with gcc!
//     Vector base;
//     Scalar alpha, beta;
//     base <<   scaledMat(1,0) * scaledMat(2,1),
//               scaledMat(1,0) * scaledMat(2,0),
//              -scaledMat(1,0) * scaledMat(1,0);
//     for(int k=0; k<2; ++k)
//     {
//       alpha = scaledMat(0,0) - evals(k);
//       beta  = scaledMat(1,1) - evals(k);
//       evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized();
//     }
//     evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized();

//   // naive version
//   Matrix tmp;
//   tmp = scaledMat;
//   tmp.diagonal().array() -= evals(0);
//   evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized();
//
//   tmp = scaledMat;
//   tmp.diagonal().array() -= evals(1);
//   evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized();
//
//   tmp = scaledMat;
//   tmp.diagonal().array() -= evals(2);
//   evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized();

  // a more stable version:
  if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
  {
    evecs.setIdentity();
  }
  else
  {
    Matrix tmp;
    tmp = scaledMat;
    tmp.diagonal ().array () -= evals (2);
    evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();

    tmp = scaledMat;
    tmp.diagonal ().array () -= evals (1);
    evecs.col(1) = tmp.row (0).cross(tmp.row (1));
    Scalar n1 = evecs.col(1).norm();
    if(n1<=Eigen::NumTraits<Scalar>::epsilon())
      evecs.col(1) = evecs.col(2).unitOrthogonal();
    else
      evecs.col(1) /= n1;

    // make sure that evecs[1] is orthogonal to evecs[2]
    evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized();
    evecs.col(0) = evecs.col(2).cross(evecs.col(1));
  }

  // Rescale back to the original size.
  evals *= scale;
}

int main()
{
  BenchTimer t;
  int tries = 10;
  int rep = 400000;
  typedef Matrix3f Mat;
  typedef Vector3f Vec;
  Mat A = Mat::Random(3,3);
  A = A.adjoint() * A;

  SelfAdjointEigenSolver<Mat> eig(A);
  BENCH(t, tries, rep, eig.compute(A));
  std::cout << "Eigen:  " << t.best() << "s\n";

  Mat evecs;
  Vec evals;
  BENCH(t, tries, rep, eigen33(A,evecs,evals));
  std::cout << "Direct: " << t.best() << "s\n\n";

  std::cerr << "Eigenvalue/eigenvector diffs:\n";
  std::cerr << (evals - eig.eigenvalues()).transpose() << "\n";
  for(int k=0;k<3;++k)
    if(evecs.col(k).dot(eig.eigenvectors().col(k))<0)
      evecs.col(k) = -evecs.col(k);
  std::cerr << evecs - eig.eigenvectors() << "\n\n";
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/monoid.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

int main() {

{

//! [make<Range>]
constexpr auto irange = make<Range>(int_<0>, int_<10>); // [0, 10) int
constexpr auto lrange = make<Range>(int_<0>, long_<10>); // [0, 10) long
BOOST_HANA_CONSTANT_CHECK(lrange == make<Range>(long_<0>, long_<10>));
//! [make<Range>]
(void)irange;

}{

//! [comparable]
// empty ranges are equal
BOOST_HANA_CONSTANT_CHECK(range(int_<6>, int_<6>) == range(int_<0>, int_<0>));

// otherwise, ranges are equal if and only if they span the same interval
BOOST_HANA_CONSTANT_CHECK(range(int_<2>, int_<5>) == range(int_<2>, int_<5>));
BOOST_HANA_CONSTANT_CHECK(range(int_<0>, int_<3>) != range(int_<-1>, int_<3>));
//! [comparable]

}{

//! [foldable]
BOOST_HANA_CONSTANT_CHECK(
    foldl(range(int_<0>, int_<4>), int_<0>, plus) == int_<6>
);

BOOST_HANA_CONSTANT_CHECK(
    unpack(range(int_<-2>, int_<2>), make<Tuple>) ==
    make<Tuple>(int_<-2>, int_<-1>, int_<0>, int_<1>)
);
//! [foldable]

}{

//! [iterable]
constexpr auto r = range(int_<0>, int_<1000>);
BOOST_HANA_CONSTANT_CHECK(head(r) == int_<0>);
BOOST_HANA_CONSTANT_CHECK(last(r) == int_<999>);
BOOST_HANA_CONSTANT_CHECK(tail(r) == range(int_<1>, int_<1000>));
BOOST_HANA_CONSTANT_CHECK(!is_empty(r));
BOOST_HANA_CONSTANT_CHECK(is_empty(range(int_<3>, int_<3>)));
//! [iterable]

}{

//! [searchable]
BOOST_HANA_CONSTANT_CHECK(find(range(int_<1>, int_<25>), int_<10>) == just(int_<10>));
BOOST_HANA_CONSTANT_CHECK(find(range(int_<1>, int_<25>), int_<200>) == nothing);
//! [searchable]

}{

//! [range_c]
BOOST_HANA_CONSTANT_CHECK(head(range_c<int, 0, 5>) == int_<0>);
BOOST_HANA_CONSTANT_CHECK(last(range_c<unsigned long, 0, 5>) == ulong<4>);
BOOST_HANA_CONSTANT_CHECK(tail(range_c<int, 0, 5>) == range(int_<1>, int_<5>));
//! [range_c]

}

}


//
//  dt_util.cpp
//  Classifer_RF
//
//  Created by jimmy on 2017-02-16.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "dt_util.hpp"
#include <Eigen/QR>
#include <iostream>

using std::cout;
using std::endl;

template <class T>
double DTUtil::spatialVariance(const vector<T> & labels, const vector<unsigned int> & indices)
{
    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);

    T mean = T::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        T dif = labels[index] - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j];
        }
    }
    return var;
}

template <class T>
void DTUtil::meanStddev(const vector<T> & labels, const vector<unsigned int> & indices, T & mean, T & sigma)
{
    assert(indices.size() > 0);

    mean = T::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    sigma = T::Zero(labels[0].size());
    if (indices.size() == 1) {
        return;
    }
    for (int i = 0; i<indices.size(); i++) {
        T dif = labels[indices[i]] - mean;
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] += dif[j] * dif[j];
        }
    }
    for (int j = 0; j<sigma.size(); j++) {
        sigma[j] = sqrt(fabs(sigma[j])/indices.size());
    }
}

template <class T>
T DTUtil::mean(const vector<T> & data, const vector<unsigned int> & indices)
{
    assert(indices.size() > 0);

    T m = T::Zero(data[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < data.size());
        m += data[index];
    }
    m /= indices.size();

    return m;
}

template <class T>
T DTUtil::mean(const vector<T> & data)
{
    assert(data.size() > 0);

    T m = T::Zero(data[0].size());

    for (int i = 0; i<data.size(); i++) {
        m += data[i];
    }
    m /= data.size();
    return m;
}

template <class T>
void DTUtil::meanMedianError(const vector<T> & errors,
                                  T & mean,
                                  T & median)
{
    assert(errors.size() > 0);
    const int dim = (int)errors[0].size();
    mean = T::Zero(dim);
    median = T::Zero(dim);

    vector<vector<double> > each_dim_data(dim);
    for (int i = 0; i<errors.size(); i++) {
        T err = errors[i].cwiseAbs();
        mean += err;
        for (int j = 0; j<err.size(); j++) {
            each_dim_data[j].push_back(err[j]);
        }
    }
    mean /= errors.size();

    for (int i = 0; i<each_dim_data.size(); i++) {
        std::sort(each_dim_data[i].begin(), each_dim_data[i].end());
        median[i] = each_dim_data[i][each_dim_data[i].size()/2];
    }
}

double DTUtil::balanceLoss(const int leftNodeSize, const int rightNodeSize)
{
    double dif = leftNodeSize - rightNodeSize;
    double num = leftNodeSize + rightNodeSize;
    double loss = fabs(dif)/num;
    assert(loss >= 0);
    return loss;
}

template double
DTUtil::spatialVariance(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices);

template void
DTUtil::meanStddev(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices, Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data, const vector<unsigned int> & indices);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data);

template void
DTUtil::meanMedianError(const vector<Eigen::VectorXf> & errors, Eigen::VectorXf & mean, Eigen::VectorXf & median);


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// 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)
//
// Polygon Example

#include <algorithm> // for reverse, unique
#include <iostream>
#include <string>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

std::string boolstr(bool v)
{
    return v ? "true" : "false";
}

int main(void)
{
    using namespace boost::geometry;

    typedef model::d2::point_xy<double> point_2d;
    typedef model::polygon<point_2d> polygon_2d;
    typedef model::box<point_2d> box_2d;

    // Define a polygon and fill the outer ring.
    // In most cases you will read it from a file or database
    polygon_2d poly;
    {
        const double coor[][2] = {
            {2.0, 1.3}, {2.4, 1.7}, {2.8, 1.8}, {3.4, 1.2}, {3.7, 1.6},
            {3.4, 2.0}, {4.1, 3.0}, {5.3, 2.6}, {5.4, 1.2}, {4.9, 0.8}, {2.9, 0.7},
            {2.0, 1.3} // closing point is opening point
            };
        assign_points(poly, coor);
    }

    // Polygons should be closed, and directed clockwise. If you're not sure if that is the case,
    // call the correct algorithm
    correct(poly);

    // Polygons can be streamed as text
    // (or more precisely: as DSV (delimiter separated values))
    std::cout << dsv(poly) << std::endl;

    // As with lines, bounding box of polygons can be calculated
    box_2d b;
    envelope(poly, b);
    std::cout << dsv(b) << std::endl;

    // The area of the polygon can be calulated
    std::cout << "area: " << area(poly) << std::endl;

    // And the centroid, which is the center of gravity
    point_2d cent;
    centroid(poly, cent);
    std::cout << "centroid: " << dsv(cent) << std::endl;

    // The number of points can be requested per ring (using .size())
    // or per polygon (using num_points)
    std::cout << "number of points in outer ring: " << poly.outer().size() << std::endl;

    // Polygons can have one or more inner rings, also called holes, islands, interior rings.
    // Let's add one
    {
        poly.inners().resize(1);
        model::ring<point_2d>& inner = poly.inners().back();

        const double coor[][2] = { {4.0, 2.0}, {4.2, 1.4}, {4.8, 1.9}, {4.4, 2.2}, {4.0, 2.0} };
        assign_points(inner, coor);
    }

    correct(poly);

    std::cout << "with inner ring:" << dsv(poly) << std::endl;
    // The area of the polygon is changed of course
    std::cout << "new area of polygon: " << area(poly) << std::endl;
    centroid(poly, cent);
    std::cout << "new centroid: " << dsv(cent) << std::endl;

    // You can test whether points are within a polygon
    std::cout << "point in polygon:"
        << " p1: "  << boolstr(within(make<point_2d>(3.0, 2.0), poly))
        << " p2: "  << boolstr(within(make<point_2d>(3.7, 2.0), poly))
        << " p3: "  << boolstr(within(make<point_2d>(4.4, 2.0), poly))
        << std::endl;

    // As with linestrings and points, you can derive from polygon to add, for example,
    // fill color and stroke color. Or SRID (spatial reference ID). Or Z-value. Or a property map.
    // We don't show this here.

    // Clip the polygon using a box
    box_2d cb(make<point_2d>(1.5, 1.5), make<point_2d>(4.5, 2.5));
    typedef std::vector<polygon_2d> polygon_list;
    polygon_list v;

    intersection(cb, poly, v);
    std::cout << "Clipped output polygons" << std::endl;
    for (polygon_list::const_iterator it = v.begin(); it != v.end(); ++it)
    {
        std::cout << dsv(*it) << std::endl;
    }

    typedef model::multi_polygon<polygon_2d> polygon_set;
    polygon_set ps;
    union_(cb, poly, ps);

    polygon_2d hull;
    convex_hull(poly, hull);
    std::cout << "Convex hull:" << dsv(hull) << std::endl;

    // If you really want:
    //   You don't have to use a vector, you can define a polygon with a deque
    //   You can specify the container for the points and for the inner rings independantly

    typedef model::polygon<point_2d, true, true, std::deque, std::deque> deque_polygon;
    deque_polygon poly2;
    ring_type<deque_polygon>::type& ring = exterior_ring(poly2);
    append(ring, make<point_2d>(2.8, 1.9));
    append(ring, make<point_2d>(2.9, 2.4));
    append(ring, make<point_2d>(3.3, 2.2));
    append(ring, make<point_2d>(3.2, 1.8));
    append(ring, make<point_2d>(2.8, 1.9));
    std::cout << dsv(poly2) << std::endl;

    return 0;
}


// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// 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.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/solver.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/types/sim3/types_seven_dof_expmap.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/types/slam3d/vertex_se3.h"

using namespace std;
using namespace g2o;

extern "C" void G2O_FACTORY_EXPORT g2o_type_VertexSE3(void);

// Convert SE3 Vertex to Sim3 Vertex
void ToVertexSim3(const g2o::VertexSE3& v_se3,
                  g2o::VertexSim3Expmap* const v_sim3) {
  Eigen::Isometry3d se3 = v_se3.estimate().inverse();
  Eigen::Matrix3d r = se3.rotation();
  Eigen::Vector3d t = se3.translation();
  g2o::Sim3 sim3(r, t, 1.0);

  v_sim3->setEstimate(sim3);
}

// Convert Sim3 Vertex to SE3 Vertex
void ToVertexSE3(const g2o::VertexSim3Expmap& v_sim3,
                 g2o::VertexSE3* const v_se3) {
  g2o::Sim3 sim3 = v_sim3.estimate().inverse();
  Eigen::Matrix3d r = sim3.rotation().toRotationMatrix();
  Eigen::Vector3d t = sim3.translation();
  Eigen::Isometry3d se3;
  se3 = r;
  se3.translation() = t;

  v_se3->setEstimate(se3);
}

// Converte EdgeSE3 to EdgeSim3
void ToEdgeSim3(const g2o::EdgeSE3& e_se3, g2o::EdgeSim3* const e_sim3) {
  Eigen::Isometry3d se3 = e_se3.measurement().inverse();
  Eigen::Matrix3d r = se3.rotation();
  Eigen::Vector3d t = se3.translation();
  g2o::Sim3 sim3(r, t, 1.0);

  e_sim3->setMeasurement(sim3);
}

// Using VertexSim3 and EdgeSim3 is the core of this example.
// This example optimize the data created by create_sphere.
// Because the data is recore by VertexSE3 and EdgeSE3, SE3 is used for
// interface and Sim is used for optimization.
// g2o_viewer is avaliable to the result.

int main(int argc, char** argv) {
  g2o_type_VertexSE3();
  if (argc != 2) {
    cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
    return 1;
  }
  ifstream fin(argv[1]);
  if (!fin) {
    cout << "file " << argv[1] << " does not exist." << endl;
    return 1;
  }

  //  define the optimizer
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<7, 7>> BlockSolverType;
  typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>
      LinearSolverType;
  auto solver = new g2o::OptimizationAlgorithmLevenberg(
      g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

  g2o::SparseOptimizer optimizer;
  optimizer.setAlgorithm(solver);
  optimizer.setVerbose(true);

  // Load and Save in SE3
  g2o::SparseOptimizer interface;
  interface.load(argv[1]);

  // Convert all vertices
  for (auto& tmp : interface.vertices()) {
    const int& id = tmp.first;
    g2o::VertexSE3* v_se3 = static_cast<g2o::VertexSE3*>(tmp.second);
    g2o::VertexSim3Expmap* v_sim3 = new g2o::VertexSim3Expmap();
    v_sim3->setId(id);
    v_sim3->setMarginalized(false);

    ToVertexSim3(*v_se3, v_sim3);
    optimizer.addVertex(v_sim3);
    if (id == 0) {
      v_sim3->setFixed(true);
    }
  }

  // Convert all edges
  int edge_index = 0;
  for (auto& tmp : interface.edges()) {
    g2o::EdgeSE3* e_se3 = static_cast<g2o::EdgeSE3*>(tmp);
    int idx0 = e_se3->vertex(0)->id();
    int idx1 = e_se3->vertex(1)->id();
    g2o::EdgeSim3* e_sim3 = new g2o::EdgeSim3();

    ToEdgeSim3(*e_se3, e_sim3);
    e_sim3->setId(edge_index++);
    e_sim3->setVertex(0, optimizer.vertices()[idx0]);
    e_sim3->setVertex(1, optimizer.vertices()[idx1]);
    e_sim3->information() = Eigen::Matrix<double, 7, 7>::Identity();

    optimizer.addEdge(e_sim3);
  }

  cout << "optimizing ..." << endl;
  optimizer.initializeOptimization();
  optimizer.optimize(30);

  cout << "saving optimization results in VertexSE3..." << endl;
  auto vertices_sim3 = optimizer.vertices();
  auto vertices_se3 = interface.vertices();

  for (auto& tmp : vertices_sim3) {
    const int& id = tmp.first;
    g2o::VertexSim3Expmap* v_sim3 =
        static_cast<g2o::VertexSim3Expmap*>(tmp.second);
    g2o::VertexSE3* v_se3 = static_cast<g2o::VertexSE3*>(vertices_se3[id]);

    ToVertexSE3(*v_sim3, v_se3);
  }

  interface.save("result.g2o");
  return 0;
}


/*
 * AttitudeESKF.cpp
 *
 *  Copyright (c) 2013 Gareth Cross. Apache 2 License.
 *
 *  This file is part of kr_attitude_eskf.
 *
 *	Created on: 12/24/2013
 *		  Author: gareth
 */

#ifndef NDEBUG
#define NDEBUG
#endif

#include "AttitudeESKF.hpp"
#include <Eigen/LU>
#include <Eigen/Cholesky>
#include <iostream>
#include <cmath>

using namespace Eigen;

namespace kr {

//	skew symmetric matrix
template <typename T>
static inline Matrix<T, 3, 3> crossSkew(const Matrix<T, 3, 1> &w) {
  Matrix<T, 3, 3> W;

  W(0, 0) = 0;
  W(0, 1) = -w(2);
  W(0, 2) = w(1);

  W(1, 0) = w(2);
  W(1, 1) = 0;
  W(1, 2) = -w(0);

  W(2, 0) = -w(1);
  W(2, 1) = w(0);
  W(2, 2) = 0;

  return W;
}

//	hardcoded 3x3 invert (unchecked)
template <typename T>
static inline Matrix<T, 3, 3> invert(const Matrix<T, 3, 3> &A, T det) {
  Matrix<T, 3, 3> C;
  det = 1 / det;

  C(0, 0) = (-A(2, 1) * A(1, 2) + A(1, 1) * A(2, 2)) * det;
  C(0, 1) = (-A(0, 1) * A(2, 2) + A(0, 2) * A(2, 1)) * det;
  C(0, 2) = (A(0, 1) * A(1, 2) - A(0, 2) * A(1, 1)) * det;

  C(1, 0) = (A(2, 0) * A(1, 2) - A(1, 0) * A(2, 2)) * det;
  C(1, 1) = (-A(2, 0) * A(0, 2) + A(0, 0) * A(2, 2)) * det;
  C(1, 2) = (A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2)) * det;

  C(2, 0) = (-A(2, 0) * A(1, 1) + A(1, 0) * A(2, 1)) * det;
  C(2, 1) = (A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1)) * det;
  C(2, 2) = (-A(1, 0) * A(0, 1) + A(0, 0) * A(1, 1)) * det;

  return C;
}

//	hardcoded determinant
template <typename T> static inline T determinant(const Matrix<T, 3, 3> &A) {
  return A(0, 0) * (A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1)) -
         A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) +
         A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0));
}

//  Eigen does not define these operators, which we use for integration
template <typename Scalar>
static inline Eigen::Quaternion<Scalar> operator + (const Eigen::Quaternion<Scalar>& a,
                                      const Eigen::Quaternion<Scalar>& b) {
  return Eigen::Quaternion<Scalar>(a.w()+b.w(),
                                   a.x()+b.x(),
                                   a.y()+b.y(),
                                   a.z()+b.z());
}

template <typename Scalar>
static inline Eigen::Quaternion<Scalar> operator * (const Eigen::Quaternion<Scalar>& q,
                                      Scalar s) {
  return Eigen::Quaternion<Scalar>(q.w() * s,
                                   q.x() * s,
                                   q.y() * s,
                                   q.z() * s);
}

/**
 *  @brief Integrate a rotation quaterion using Euler integration
 *  @param q Quaternion to integrate
 *  @param w Angular velocity (body frame), stored in 3 complex terms
 *  @param dt Time interval in seconds
 *  @param normalize If True, quaternion is normalized after integration
 */
template <typename Scalar>
static inline void integrateEuler(Eigen::Quaternion<Scalar> &q, Eigen::Quaternion<Scalar> &w, Scalar dt,
                    bool normalize = true) {
  q = q + (q * w * static_cast<Scalar>(0.5)) * dt;

  if (normalize) {
    q.normalize();
  }
}

/**
 *  @brief Integrate a rotation quaternion using 4th order Runge Kutta
 *  @param q Quaternion to integrate
 *  @param w Angular velocity (body frame), stored in 3 complex terms
 *  @param dt Time interval in seconds
 *  @param normalize If true, quaternion is normalized after integration
 */
template <typename Scalar>
static inline void integrateRungeKutta4(Eigen::Quaternion<Scalar> &q, const Eigen::Quaternion<Scalar> &w, Scalar dt,
                          bool normalize = true) {
  const static Scalar half = static_cast<Scalar>(0.5);
  const static Scalar two = static_cast<Scalar>(2);

  Eigen::Quaternion<Scalar> qw = q * w * half;
  Eigen::Quaternion<Scalar> k2 = (q + qw * dt * half) * w * half;
  Eigen::Quaternion<Scalar> k3 = (q + k2 * dt * half) * w * half;
  Eigen::Quaternion<Scalar> k4 = (q + k3 * dt) * w * half;

  q = q + (qw + k2 * two + k3 * two + k4) * (dt / 6);

  if (normalize) {
    q.normalize();
  }
}

template <typename Scalar>
static inline Eigen::Matrix<Scalar,3,3>
rodrigues(const Eigen::Matrix<Scalar,3,1>& w) {
  const auto norm = w.norm();
  if (norm < std::numeric_limits<Scalar>::epsilon()*10) {
    return Eigen::Matrix<Scalar,3,3>::Identity() + crossSkew(w);
  }
  return Eigen::AngleAxis<Scalar>(norm, w / norm).matrix();
}

AttitudeESKF::AttitudeESKF()
    : q_(1,0,0,0), steadyCount_(0), biasThresh_(0), isStable_(true) {
  P_.setZero();
  b_.setZero();
  w_.setZero();
  dx_.setZero();

  magRef_.setZero();
  predMag_.setZero();

  estBias_ = false;
  ignoreZ_ = false;
  useMag_ = false;
}

void AttitudeESKF::predict(const AttitudeESKF::vec3 &wb,
                           AttitudeESKF::scalar_t dt,
                           const AttitudeESKF::mat3 &cov,
                           bool useRK4) {
  static const Matrix<scalar_t, 3, 3> I3 =
      Matrix<scalar_t, 3, 3>::Identity(); //  identity R3

  scalar_t wb2 = wb[0] * wb[0] + wb[1] * wb[1] + wb[2] * wb[2];
  if (wb2 < biasThresh_ * biasThresh_) {
    steadyCount_++; //  not rotating, update moving average

    if (estBias_ && steadyCount_ > 20) {
      b_ = (b_ * (steadyCount_ - 1) + wb) / steadyCount_;
    }
  } else {
    steadyCount_ = 0;
  }

  w_ = (wb - b_); //	true gyro reading

  //	error-state jacobian
  const Matrix<scalar_t, 3, 3> F = I3 - crossSkew<scalar_t>(w_ * dt);

  //  integrate state and covariance
  Eigen::Quaternion<scalar_t> wQuat(0, w_[0], w_[1], w_[2]);
  if (!useRK4) {
    integrateEuler(q_, wQuat, dt, true);
  } else {
    integrateRungeKutta4(q_, wQuat, dt, true);
  }

  //  noise jacobian
  const Matrix <scalar_t,3,3> G = -I3 * dt;
  P_ = F*P_*F.transpose() + G*cov*G.transpose();
}

void AttitudeESKF::update(const AttitudeESKF::vec3 &ab,
                          const mat3 &aCov,
                          const AttitudeESKF::vec3 &mb,
                          const mat3 &mCov) {
  Matrix<scalar_t, 3, 3> A;

  //  rotation matrix: world -> body
  const Matrix<scalar_t, 3, 3> bRw = q_.conjugate().matrix();

  vec3 gravity;
  gravity[0] = 0.0;
  gravity[1] = 0.0;
  gravity[2] = kOneG;

  //  predicted gravity vector
  const vec3 aPred = bRw * gravity;

  if (!useMag_) {
    //  calculate jacobian
    Matrix<scalar_t, 3, 3> H = crossSkew(aPred);
    Matrix<scalar_t, 3, 1> r = ab - aPred;

    //  solve for the kalman gain
    const Matrix<scalar_t, 3, 3> S = H * P_ * H.transpose() + aCov;
    Matrix<scalar_t, 3, 3> Sinv;

    const scalar_t det = determinant(S);
    if (std::abs(det) < static_cast<scalar_t>(1e-5)) {
      isStable_ = false;
      return;
    } else {
      isStable_ = true;
    }
    Sinv = invert(S, det);

    const Matrix<scalar_t, 3, 3> K = P_ * H.transpose() * Sinv;

    A = K * H;
    dx_ = K * r;
  }
  else {
#ifdef ATTITUDE_ESKF_BUILD_MAG  //  stop compilation of FullPivLU
    //  m-field prediction
    vec3 field = bRw * magRef_;
    predMag_ = field;

    Matrix<scalar_t, 6, 1> r;
    r.block<3, 1>(0, 0) = ab - aPred;
    r.block<3, 1>(3, 0) = mb - field;

    Matrix<scalar_t, 6, 3> H;
    H.setZero();

    //  jacobians for gravity and magnetic field
    H.block<3, 3>(0, 0) = crossSkew(aPred);
    H.block<3, 3>(3, 0) = crossSkew(field);

    //  covariance for both sensors
    Matrix<scalar_t, 6, 6> covR;
    covR.setZero();
    covR.block<3,3>(0,0) = aCov;
    covR.block<3,3>(3,3) = mCov;

    const Matrix<scalar_t, 6, 6> S = H * P_ * H.transpose() + covR;
    Matrix<scalar_t, 6, 6> Sinv;

    Eigen::FullPivLU<Matrix<scalar_t,6,6>> LU(S);
    isStable_ = LU.isInvertible();

    if (!isStable_) {
      return;
    }
    Sinv = LU.inverse();

    //  generate update
    const Matrix<scalar_t, 3, 6> K = P_ * H.transpose() * Sinv;
    dx_ = K * r;
    A = K * H;
#else
    dx_.setZero();
    A.setZero();
#endif
  }

  if (ignoreZ_) {
    //  cancel body-frame z update
    dx_[2] = 0;
  }

  //  perform state update
  P_ = (Matrix<scalar_t, 3, 3>::Identity() - A) * P_;

  q_ = q_ * quat(1, dx_[0]/2, dx_[1]/2, dx_[2]/2);
  q_.normalize();
}

void AttitudeESKF::externalYawUpdate(scalar_t yaw, scalar_t alpha) {
  //  check if we are near the hover state
  const Matrix<scalar_t,3,3> wRb = q_.matrix();
  Matrix<scalar_t,3,1> g;
  g[0] = 0;
  g[1] = 0;
  g[2] = 1;

  g = wRb.transpose() * g;
  if (g[2] > 0.85) {
    //  break into roll pitch yaw
    Matrix<scalar_t,3,1> rpy = getRPY(wRb);
    //  interpolate between prediction and estimate
    rpy[2] = rpy[2]*(1-alpha) + yaw*alpha;
    q_ = Eigen::AngleAxis<scalar_t>(rpy[2],vec3(0,0,1)) *
    Eigen::AngleAxis<scalar_t>(rpy[1],vec3(0,1,0)) *
    Eigen::AngleAxis<scalar_t>(rpy[0],vec3(1,0,0));
  }
}

bool AttitudeESKF::initialize(const vec3 &ab,
                              const vec3 &aCov,
                              const vec3 &mb,
                              const vec3 &mCov) {
  if (!useMag_) {
    //  determine attitude angles
    scalar_t ay = ab[1];
    if (ay > kOneG) { ay = kOneG; }
    else if (ay < -kOneG) { ay = -kOneG; }
    const scalar_t& ax = ab[0];
    const scalar_t& az = ab[2];

    const scalar_t phi = std::asin(-ay / kOneG);  //  roll
    const scalar_t theta = std::atan2(ax, az);    //  pitch

    q_ = Eigen::AngleAxis<scalar_t>(theta, vec3(0,1,0)) *
         Eigen::AngleAxis<scalar_t>(phi, vec3(1,0,0));
  }
  else {
    ///  @todo: This is kind of ugly, find some simpler mechanism to do this.

#ifdef ATTITUDE_ESKF_BUILD_MAG
    const static scalar_t eps(1e-6);
    for (int i=0; i < 3; i++) {
      if (aCov[i] < eps || mCov[i] < eps) {
        return false;
      }
    }
    //  jacobian
    Eigen::Matrix <scalar_t,6,3> J;
    J.block<3,3>(0,0) = crossSkew(ab);
    J.block<3,3>(3,0) = crossSkew(mb);

    //  weight matrix
    Eigen::Matrix <scalar_t,6,6> S;
    S.setZero();
    for (int i=0; i < 3; i++) {
      S(i,i) = 1 / aCov[i];
      S(i+3,i+3) = 1 / mCov[i];
    }

    //  hessian
    const mat3 H = J.transpose() * S * J;
    const Eigen::LDLT<mat3> ldlt(H);

    //  optimize
    vec3 w(0,0,0);
    Matrix<scalar_t,6,1> r;
    for (unsigned int iter=0; iter < 5; iter++) {
      const mat3 W = rodrigues(w);
      //  residuals
      r.block<3,1>(0,0) = (W * vec3(0,0,kOneG)) - ab;
      r.block<3,1>(3,0) = (W * magRef_) - mb;
      //  step
      w.noalias() += ldlt.solve(J.transpose() * S * r);
    }
    q_ = quat(rodrigues(w).transpose());
#endif
  }
  //  start w/ a large uncertainty
  P_.setIdentity();
  P_ *= M_PI*M_PI;

  return true;
}

AttitudeESKF::vec3 AttitudeESKF::getRPY(const mat3& R) {
  vec3 rpy;
  scalar_t sth = -R(2, 0);
  if (sth > 1) {
    sth = 1;
  } else if (sth < -1) {
    sth = -1;
  }

  const scalar_t theta = std::asin(sth);
  const scalar_t cth = std::sqrt(1 - sth*sth);

  scalar_t phi, psi;
  if (cth < static_cast<scalar_t>(1.0e-6)) {
    phi = std::atan2(R(0, 1), R(1, 1));
    psi = 0;
  } else {
    phi = std::atan2(R(2, 1), R(2, 2));
    psi = std::atan2(R(1, 0), R(0, 0));
  }

  rpy[0] = phi;    //  x, [-pi,pi]
  rpy[1] = theta;  //  y, [-pi/2,pi/2]
  rpy[2] = psi;    //  z, [-pi,pi]
  return rpy;
}

} //  namespace kr


#include <iostream>
#include <functional>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <map>
#include <Eigen\dense>

#include "markov.h"
#include "TransitionMatrix.h"

// Hint: Set N - number of simulations low until you have it working
//       Then set it much much higher, and run in release mode so its faster

int main() {

	SetTransitionMatrix();
	setGameMatrix();
	// Print Results to File
	std::ofstream myfile;
	myfile.open("dtmc_results.txt");

	int start = 0;

	//simulate discrete time Markov Chain
	unsigned int N = 100;
	std::map<int, int> hist;

	std::vector<int> discreteMC;
	//std::vector< std::vector<double> > matrix(3, std::vector<double>(3)); //initializes a 3x3 matrix with zeros
	for (unsigned int i = 0; i < N; ++i) {

		//TODO (add DTMC, and histogram lines.)
		discreteMC = DTMC(TransitionMatrix,ROLLS,start);
		++hist[std::round(discreteMC.back())];
		int counter = 0;
		// Code if you wanted to print out results at each step
		for (auto elem : discreteMC) {
			if (elem < 100) counter++;
			std::cout << elem << std::endl;
			std::cout << counter << std::endl;
			myfile << elem << std::endl;
			myfile << counter << std::endl;

		}
		std::cout << "****New Game*****" << i << std::endl;
		myfile << "******New Game*******" << std::endl;
	}
	//Returns an array discreteMC with the states at each step of the discrete-time Markov Chain
	//The number of transitions is given by steps. The initial state is given by start
	//(the states are indexed from 0 to n-1 where n is the number of arrays in transMatrix).
	//hist is the histogram

	// (double)p.second / N    - (decimal) percentage.
	for (auto p : hist) {
		std::cout << p.first << "\t" << (double)p.second / N << std::endl;
	}

	myfile.close();

	return 1;
}


/***************************************************************************
 *   Copyright (C) 2016 by Саша Миленковић                                 *
 *   sasa.milenkovic.xyz@gmail.com                                         *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *   ( http://www.gnu.org/licenses/gpl-3.0.en.html )                       *
 *                                     *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <Eigen/Dense>
#include <cmath>
#include "quartic.hpp"
const double PI = 3.141592653589793238463L;
const double M_2PI = 2*PI;
const double eps=1e-12;

//---------------------------------------------------------------------------
// solve cubic equation x^3 + a*x^2 + b*x + c
// x - array of size 3
// In case 3 real roots: => x[0], x[1], x[2], return 3
//         2 real roots: x[0], x[1],          return 2
//         1 real root : x[0], x[1] ± i*x[2], return 1
inline unsigned int solveP3(double *x,double a,double b,double c) {

    double a2 = a * a;
    double q = (a2 - 3 * b) / 9;
    double r = (a * ( 2 * a2 - 9 * b) + 27 * c) / 54;
    double r2 = r*r;
    double q3 = q*q*q;
    double A,B;
        if(r2<q3)
        {
            double t=r/sqrt(q3);
            if( t<-1) t=-1;
            if( t> 1) t= 1;
            t=acos(t);
            a/=3; q=-2*sqrt(q);
            x[0]=q*cos(t/3)-a;
            x[1]=q*cos((t+M_2PI)/3)-a;
            x[2]=q*cos((t-M_2PI)/3)-a;
            return 3;
        }
        else
        {
            A =-pow(fabs(r)+sqrt(r2-q3),1./3);
            if( r<0 ) A=-A;
            B = (0==A ? 0 : q/A);

        a/=3;
        x[0] =(A+B)-a;
        x[1] =-0.5*(A+B)-a;
        x[2] = 0.5*sqrt(3.)*(A-B);
        if(fabs(x[2])<eps) { x[2]=x[1]; return 2; }

        return 1;
        }
}

//---------------------------------------------------------------------------
// solve quartic equation x^4 + a*x^3 + b*x^2 + c*x + d
Eigen::Vector4cd solve_quartic(double a, double b, double c, double d)
{
    double a3 = -b;
    double b3 =  a*c -4.*d;
    double c3 = -a*a*d - c*c + 4.*b*d;

    // cubic resolvent
    // y^3 − b*y^2 + (ac−4d)*y − a^2*d−c^2+4*b*d = 0

    double x3[3];
    unsigned int iZeroes = solveP3(x3, a3, b3, c3);

    double q1, q2, p1, p2, D, sqD, y;

    y = x3[0];
    // THE ESSENCE - choosing Y with maximal absolute value !
    if(iZeroes != 1)
    {
        if(fabs(x3[1]) > fabs(y)) y = x3[1];
        if(fabs(x3[2]) > fabs(y)) y = x3[2];
    }

    // h1+h2 = y && h1*h2 = d  <=>  h^2 -y*h + d = 0    (h === q)

    D = y*y - 4*d;
    if(fabs(D) < eps) //in other words - D==0
    {
        q1 = q2 = y * 0.5;
        // g1+g2 = a && g1+g2 = b-y   <=>   g^2 - a*g + b-y = 0    (p === g)
        D = a*a - 4*(b-y);
        if(fabs(D) < eps) //in other words - D==0
            p1 = p2 = a * 0.5;

        else
        {
            sqD = sqrt(D);
            p1 = (a + sqD) * 0.5;
            p2 = (a - sqD) * 0.5;
        }
    }
    else
    {
        sqD = sqrt(D);
        q1 = (y + sqD) * 0.5;
        q2 = (y - sqD) * 0.5;
        // g1+g2 = a && g1*h2 + g2*h1 = c       ( && g === p )  Krammer
        p1 = (a*q1-c)/(q1-q2);
        p2 = (c-a*q2)/(q1-q2);
    }

    Eigen::Vector4cd retval;
    std::complex<double> tmp;
    // solving quadratic eq. - x^2 + p1*x + q1 = 0
    D = p1*p1 - 4*q1;
    if(D < 0.0)
    {
        tmp.real( -p1 * 0.5 );
        tmp.imag( sqrt(-D) * 0.5 );
        retval[0] = tmp;
        tmp = std::conj(tmp);
        retval[1] = tmp;
    }
    else
    {
        tmp.imag(0);
        sqD = sqrt(D);
        tmp.real( (-p1 + sqD) * 0.5 );
        retval[0] = tmp;
        tmp.real( (-p1 - sqD) * 0.5 );
        retval[1] = tmp;
    }

    // solving quadratic eq. - x^2 + p2*x + q2 = 0
    D = p2*p2 - 4*q2;
    if(D < 0.0)
    {
        tmp.real( -p2 * 0.5 );
        tmp.imag( sqrt(-D) * 0.5 );
        retval[2] = tmp;
        tmp = std::conj(tmp);
        retval[3] = tmp;
    }
    else
    {
        tmp.imag(0);
        sqD = sqrt(D);
        tmp.real( (-p2 + sqD) * 0.5 );
        retval[2] = tmp;
        tmp.real( (-p2 - sqD) * 0.5 );
        retval[3] = tmp;
    }

    return retval;
}


//===============================================================================//
// Name			: utils.hpp
// Author(s)	: Barbara Bruno, Antonello Scalmato
// Affiliation	: University of Genova, Italy - dept. DIBRIS
// Version		: 1.1
// Description	: Frequently used functions (for Creator and Classifier)
//===============================================================================//

#include <armadillo>

using namespace arma;

#ifndef UTILS_HPP_
#define UTILS_HPP_

//===============================================================================//
// BASIC MATRIX-HANDLING FUNCTIONS
// create a row-vector of the form: start:1:stop
mat createInterval(int start, int stop);
// convert a matrix in MAT format to float format
float** matToFloat(mat &matrix);
// convert a matrix in float format to MAT format
mat floatToMat(float** matrix, int Nrows, int Ncols);
//===============================================================================//

//===============================================================================//
// FILTERING FUNCTIONS
// compute the median value of a vector
double median(rowvec &vector);
// perform median filtering on a matrix
void medianFilter(mat &matrix, int size);
// apply ChebyshevI filter on a matrix
mat ChebyshevFilter(mat matrix);
//===============================================================================//

#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file uniform_sensor.hpp
 * \date September 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/descriptor.hpp>
#include <fl/distribution/uniform_distribution.hpp>
#include <fl/model/sensor/interface/sensor_density.hpp>
#include <fl/model/sensor/interface/sensor_function.hpp>

namespace fl
{

template <typename State_>
class UniformSensor
    : public SensorFunction<Vector1d, State_, Vector1d>,
      public SensorDensity<Vector1d, State_>,
      public Descriptor
{
public:
    typedef Vector1d Obsrv;
    typedef Vector1d Noise;
    typedef State_   State;

public:
    UniformSensor(
        Real min_value,
        Real max_value,
        int state_dim = DimensionOf<State>::Value)
        : state_dim_(state_dim),
          density_(min_value, max_value)
    { }

    Real log_probability(const Obsrv& obsrv, const State& state) const override
    {
        return density_.log_probability(obsrv);
    }

    Real probability(const Obsrv& obsrv, const State& state) const override
    {
        return density_.probability(obsrv);
    }

    Obsrv observation(const State& state, const Noise& noise) const override
    {
        Obsrv y = density_.map_standard_normal(noise);
        return y;
    }

    virtual int obsrv_dimension() const { return 1; }
    virtual int noise_dimension() const { return 1; }
    virtual int state_dimension() const { return state_dim_; }

    virtual std::string name() const
    {
        return "UniformSensor";
    }

    virtual std::string description() const
    {
        return "UniformSensor";
    }

private:
    int state_dim_;
    UniformDistribution density_;
};

}


/* test_fisher_f.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: test_fisher_f.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/fisher_f_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/fisher_f.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::fisher_f_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME fisher_f
#define BOOST_MATH_DISTRIBUTION boost::math::fisher_f
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME m
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME n
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [remquo]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 4>;
using pack_it =  bs::pack <std::int32_t, 4>;

int main()
{
  pack_ft xf = { 3.0f, -2.0f, -3.0f, 1.0f };
  pack_ft yf = { 4.0f, -1.0f, -3.0f, 2.0f };
  pack_ft rf;
  pack_it qi;
  std::tie(rf, qi) = bs::remquo(xf, yf);

  std::cout
    <<  "---- simd:  std::tie(xf, yf) = bs::remquo(xf, yf)\n"
    << " <- xf = " << xf << '\n'
    << " <- yf = " << yf << '\n'
    << " -> rf = " << rf << '\n'
    << " -> qi = " << qi << '\n';

  float sxf = 3.0f, syf = 4.0f;
  float srf;
  std::int32_t sqi;
  std::tie(srf, sqi) = bs::remquo(sxf, syf);

  std::cout
    << "---- scalar: std::tie(srf, sqi) = bs::remquo(sxf, syf)\n"
    << " <- sxf =  " << sxf << '\n'
    << " <- syf =  " << syf << '\n'
    << " -> srf = " << srf << '\n'
    << " -> sqi = " << sqi << '\n';
  return 0;
}
//! [remquo]


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, log1p_exp) {
  using stan::math::log1p_exp;

  // exp(10000.0) overflows
  EXPECT_FLOAT_EQ(10000.0,log1p_exp(10000.0));
  EXPECT_FLOAT_EQ(0.0,log1p_exp(-10000.0));
}

TEST(MathFunctions, log1p_exp_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::log1p_exp(nan));
}


/* CHOMP class implementation
 *
 * Copyright (C) 2016 Rafael Valencia. All rights reserved.
 * License (3-Cluase BSD): https://github.com/rafaelvalencia
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **
 * \file chomp.cpp
 *
 * CHOMP for point vehicles (x,y) moving holonomously in the plane. It will
 * plan a trajectory (xi) connecting start point (qs_) to end point (qe) while
 * avoiding obstacles (obs)
 */
#include "path_adaptor.hpp"
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <stdlib.h>
#include <sys/time.h>
#include <err.h>

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;
static size_t const obs_dim(3); 	// obstacle dimensions (x,y,Radius)

using namespace std;
#define PI (3.141592653589793)

////////////////////////////////////////////////////////////////////////////
// Auxiliar functions

void pi2pi(double &angle)
{
	double ang;
    /* Process angle*/
    if( (angle < -2*PI) ||  (angle > 2* PI) )
		ang = fmod(angle, 2*PI);
    else
        ang = angle;
    if (ang > PI) ang = ang - 2*PI;
    if (ang < -PI) ang = ang + 2*PI;
    angle = ang;
}

////////////////////////////////////////////////////////////////////////////

CHOMP_SE2::CHOMP_SE2(double dt_input, double eta_input, double lambda_input, size_t nq_input, size_t numIt_input, double gain) :
CHOMP(dt_input, eta_input, lambda_input, nq_input, 3, numIt_input, gain)
{
	cout << "-----------------------------------------------------:"<< endl;
	cout << "CHOMP SE2 started "<< endl;
	cout << "-----------------------------------------------------:"<< endl;
}

void CHOMP_SE2::boundTrajectoryAngles (void)
{
	for (size_t iq (0); iq < nq_; ++iq)
	{
		//Makes the angles to be between Pi to -Pi
		Vector q (xi_.block  (iq * cdim_, 0, cdim_, 1) );
		pi2pi ( q(2) );
		xi_.block  (iq * cdim_, 0, cdim_, 1) = q;
	}
}

void CHOMP_SE2::boundVectorAngles (Vector& VV)
{
    static size_t const VVsize = 	(VV.size()/cdim_) - 1;
	for (size_t iq (0); iq < VVsize; ++iq)
	{
		//Makes the angles to be between Pi to -Pi
		Vector q (VV.block  (iq * cdim_, 0, cdim_, 1) );
		pi2pi ( q(2));
		VV.block  (iq * cdim_, 0, cdim_, 1) = q;
	}
}

double CHOMP_SE2::chompIteration(Vector  &xi)
{

	// Before performing the iteration check if a path has been given
	if (PATH_INIT_==false)
	{
		cout << "A path was not initialized. Leaving CHOMP iteration! " << endl;
		return NAN;
	}

	//////////////////////////////////////////////////
	// beginning of "the" constrained CHOMP iteration

	Vector nabla_smooth (AA_ * xi_ + bb_);
	Vector const & xidd (nabla_smooth); // indeed, it is the same in this formulation...

	// Constrained CHOMP.
	// Impose nonholonmic (NH) restrictions with the rolling constraint.
	// Next we evaluate the constraint functional
	// and its Jacobian b and C, respectively,
	// as it appears in CHOMP's IJRR paper.
	//
	Matrix CC ( Matrix::Zero (xidim_, 1) );
	double b = 0;
	Vector c1 (Vector::Zero (3));
	Vector c2 (Vector::Zero (3));
	Vector cf1 (Vector::Zero (3));
	Vector cf2 (Vector::Zero (3));

	for (size_t iq (0); iq < (nq_-1); ++iq)
	{

		//Evaluate the constraint functional. It is defined by a sum of auxiliar functions
		//that depend on only two consecutive robot poses.
		Vector const q1 (xi_.block  (iq * cdim_, 0, cdim_, 1) );
		Vector const q2 (xi_.block  ((iq+1) * cdim_, 0, cdim_, 1) );

		double nhc =  ( q2(0)  -  q1(0) )* sin(q1(2)) - ( q2(1)  -  q1(1) )* cos(q1(2)); //nonholonomic constraint
		double fmc =   cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)) + sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2)); //forward motion constraint

		b +=  nhc * nhc + fmc; //square of the rolling constraint + forward motion constrain

		//Computation of the Jacobian of the NH constraint.
		//  Jacobian of the auxiliar functions
		c1(0) = -2*sin(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c1(1) =  2*cos(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c1(2) =  -2*(cos(q1(2))*( q1(0)  -  q2(0) ) + sin(q1(2))*( q1(1)  -  q2(1) ))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c2(0) =  2*sin(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c2(1) = -2*cos(q1(2))*(cos(q1(2))*( q1(1)  -  q2(1) ) - sin(q1(2))*( q1(0)  -  q2(0) ));
		c2(2) =  0;
		//end of NHC Jacobian

		//Computation of the Jacobian of the NH constraint.
		//  Jacobian of the auxiliar functions
		cf1(0)=     cos(q1(2)) + (cos(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf1(1)=     sin(q1(2)) + (sin(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf1(2)=     cos(q1(2))*(q1(1) - q2(1)) - sin(q1(2))*(q1(0) - q2(0)) + ((cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)))*(cos(q1(2))*(q1(1) - q2(1)) - sin(q1(2))*(q1(0) - q2(0))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf2(0)=     -cos(q1(2)) - (cos(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf2(1) =    - sin(q1(2)) - (sin(q1(2))*(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1))))/sqrt(pow(cos(q1(2))*(q1(0) - q2(0)) + sin(q1(2))*(q1(1) - q2(1)),2));
		cf2(2)=     0;
		//end of FMC Jacobian

		if (iq == 0 )
		{
			  c2(0) +=  2*sin(qs_(2))*(cos(qs_(2))*( qs_(1)  -  q1(1) ) - sin(qs_(2))*( qs_(0)  -  q1(0) ));
			  c2(1) += -2*cos(qs_(2))*(cos(qs_(2))*( qs_(1)  -  q1(1) ) - sin(qs_(2))*( qs_(0)  -  q1(0) ));
			  c2(2) +=  0;

			  cf2(0) +=     -cos(qs_(2)) - (cos(qs_(2))*(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1))))/sqrt(pow(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)),2));
			  cf2(1) +=    - sin(qs_(2)) - (sin(qs_(2))*(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1))))/sqrt(pow(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)),2));
			  cf2(2) +=     0;

			  double nhstart  =  ( q1(0)  -  qs_(0) )* sin(qs_(2)) - ( q1(1)  -  qs_(0) )* cos(qs_(2));
			  double fmstart =   cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)) + sqrt(pow(cos(qs_(2))*(qs_(0) - q1(0)) + sin(qs_(2))*(qs_(1) - q1(1)),2)); //forward motion constraint
			  b +=  nhstart * nhstart + fmstart; //square of the rolling constraint
		}

		//Update Jacobian with the contributions from the Jacobian of the auxiliar functions
		CC.block  (iq * cdim_, 0, cdim_, 1) = CC.block  (iq * cdim_, 0, cdim_, 1) + c1 + cf1;
		CC.block  ((iq+1) * cdim_, 0, cdim_, 1) = CC.block  ((iq+1) * cdim_, 0, cdim_, 1) + c2 + cf2;
	}

	CC /=  dt_ * dt_ * (nq_ + 1) ;
	b /=  dt_ * dt_ * (nq_ + 1);

	Matrix CCtrans = CC.transpose();
    // end of the computation of C and b

	Vector nabla_obs (Vector::Zero (xidim_));

	for (size_t iq (0); iq < nq_; ++iq)
	{
		Vector const qq (xi_.block (iq * cdim_, 0, cdim_, 1));
		Vector qd;
		if (0 == iq)
		{
			qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - qs_);
		}
		else if (iq == nq_ - 1)
		{
			qd = 0.5 * (qe_ - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));
		}
		else
		{
			qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));;
		}

		Vector const & xx (qq.block (0,0,2,1));
		Vector const & xd (qd.block (0,0,2,1));

		// In this case, C and W are NOT the same
		Matrix JJ(Matrix::Zero (2, 3));
		JJ(0,0)=1;
		JJ(1,1)=1;

		double const vel (xd.norm());
		if (vel < 1.0e-3)
		{
			// avoid div by zero further down
			continue;
		}
		Vector const xdn (xd / vel);

		Vector const xdd (JJ * xidd.block (iq * cdim_, 0, cdim_ , 1));

		Matrix const prj (Matrix::Identity (2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa (prj * xdd / pow (vel, 2.0));

		//Add obstacles
		for (int ii = 0; ii < OBS_.cols(); ii++)
		{
			Vector delta(xx - OBS_.block(0, ii, 2, 1));
			double const dist(delta.norm());
			if ((dist >= OBS_(2, ii)) || (dist < 1e-9))
				continue;
			double const cost(costGain_ * OBS_(2, ii) * pow(1.0 - dist / OBS_(2, ii), 3.0) / 3.0);
			delta *= - costGain_ *pow(1.0 - dist / OBS_(2, ii), 2.0) / dist;
			nabla_obs.block(iq * cdim_, 0, cdim_, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
		}

	}
    double residual;

	Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth)); //unconstrained step

	if (b < 1.0e-10)
	{
		//unconstrained update to initialize trajectory (it starts with b aprox to zero)
		//cout << " One unconstrained update to initialize trajectory  " << endl;
		xi_ -= dxi / eta_;

		Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth));
		residual =  dxi.norm() / eta_;
	}
	else
	{
		// Constrained optimization update
		Vector CAC (CCtrans * Ainv_ * CC);

		Matrix CACinv (CAC.inverse());

		Vector Proj (Ainv_ * CC * CACinv); //auxiliar matrix for the update equation

		//cout << "b =" << b << "\n";
		Vector cdxi ( - dxi/eta_  +  Proj*CCtrans*dxi / eta_ -  Proj * b  );

		xi_ += cdxi; //constrained update

		residual = cdxi.norm() / eta_;
	}
	// end of "the" constrainedCHOMP iteration
	//////////////////////////////////////////////////
	boundTrajectoryAngles();

	res_ = residual;
	xi = xi_; //updated path

	return res_;

}