#include <cassert>

#include <dlib/random_forest.h>
#include <dlib/global_optimization.h>

#include "common.hpp"
#include "data.hpp"

namespace SAMPML_NAMESPACE {
    namespace trainer {
        template <class sample_type>
        class random_forest {
        public:
            random_forest() { }

            template <class SamplesContainer, class LabelsContainer>
            void set_samples(const SamplesContainer& _samples, const LabelsContainer& _labels) {
                samples.clear();
                samples.insert(samples.end(), _samples.cbegin(), _samples.cend());
                labels.clear();
                labels.insert(labels.end(), _labels.end(), _labels.size());
            }

            template <class SamplesContainer>
            void set_samples(const SamplesContainer& positives, const SamplesContainer& negatives) {
                static_assert(std::is_same<typename SamplesContainer::value_type, sample_type>());

                samples.clear();
                samples.insert(samples.end(), positives.cbegin(), positives.cend());
                samples.insert(samples.end(), negatives.cbegin(), negatives.cend());
                labels.clear();
                labels.insert(labels.end(), positives.size(), 1.0);
                labels.insert(labels.end(), negatives.size(), 0.0);
            }

            void cross_validate(double min_trees = 10, double max_trees = 1000,
                                double min_frac = 0.9, double max_frac = 1.0,
                                double min_min_per_leaf = 5, double max_min_per_leaf = 50,
                                int folds = 10,
                                int max_function_calls = 50) {
                if(samples.size() == 0) {
                    throw bad_input("Bad Input: no samples were provided for training");
                }
                assert(labels.size() == samples.size());

                dlib::vector_normalizer<sample_type> normalizer;
                normalizer.train(samples);
                for(auto& vector : samples)
                    vector = normalizer(vector);

                dlib::randomize_samples(samples, labels);

                auto cross_validation_score =
                [this, folds](int trees, double subsample_frac, int min_sampels_per_leaf) {
                    dlib::random_forest_regression_trainer<dlib::dense_feature_extractor> trainer;
                    trainer.set_num_trees(trees);
                    trainer.set_feature_subsampling_fraction(subsample_frac);
                    trainer.set_min_samples_per_leaf(min_sampels_per_leaf);

                    dlib::matrix<double> result = dlib::cross_validate_regression_trainer(trainer, this->samples, this->labels, folds);
                    std::cout << "trees: " << trees << ", cross validation accuracy: " << result << '\n';

                    return result(0);
                };

                auto result = dlib::find_min_global(dlib::default_thread_pool(),
                                                    cross_validation_score,
                                                    {min_trees, min_frac, min_min_per_leaf},
                                                    {max_trees, max_frac, max_min_per_leaf},
                                                    dlib::max_function_calls(max_function_calls));

                best_num_trees = result.x(0);
                best_subsample_fraction = result.x(1);
                best_min_samples_per_leaf = result.x(2);
                std::cout << result.x << '\n';
            }

            void train () {
                dlib::random_forest_regression_trainer<feature_extractor_type> trainer;
                trainer.set_num_trees(best_num_trees);
                trainer.set_feature_subsampling_fraction(best_subsample_fraction);
                trainer.set_min_samples_per_leaf(best_min_samples_per_leaf);

                classifier = trainer.train(samples, labels, oobs);
            }

            void serialize(std::string classifier) {
                dlib::serialize(classifier) << this->classifier;
            }

            void deserialize(std::string classifier) {
                dlib::deserialize(classifier) >> this->classifier;
            }

            double test(const sample_type& sample) {
                return classifier(sample);
            }

        protected:
            std::vector<dlib::matrix<double, 0, 1>> samples;
            std::vector<double> labels;
            std::vector<double> oobs;

            using feature_extractor_type = dlib::dense_feature_extractor;
            using decision_funct_type = dlib::random_forest_regression_function<feature_extractor_type>;
            //using normalized_decision_funct_type = dlib::normalized_function<decision_funct_type>;
            decision_funct_type classifier;

            double best_num_trees = 100;
            double best_subsample_fraction = 0.25;
            double best_min_samples_per_leaf = 5;
        };
    }
}


// clang: MatousFormat

// Include the LKF header
#include <mrs_lib/geometry/cyclic.h>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <complex>
#include <Eigen/Dense>

using namespace mrs_lib::geometry;
using cx = std::complex<double>;

double wrap(double r)
{
  return std::arg(std::polar<double>(1, r));
}

double diff(const double a, const double b)
{
  return std::arg(std::polar<double>(1, a) * std::conj(std::polar<double>(1, b)));
}

static double dist(const double a, const double b)
{
  return std::abs(diff(a, b));
}

/* interpolateAngles() //{ */

double interpolateAngles(const double& a1, const double& a2, const double& coeff)
{

  // interpolate the yaw
  Eigen::Vector3d axis = Eigen::Vector3d(0, 0, 1);

  Eigen::Quaterniond quat1 = Eigen::Quaterniond(Eigen::AngleAxis<double>(a1, axis));
  Eigen::Quaterniond quat2 = Eigen::Quaterniond(Eigen::AngleAxis<double>(a2, axis));

  Eigen::Quaterniond new_quat = quat1.slerp(coeff, quat2);

  Eigen::Vector3d vecx = new_quat * Eigen::Vector3d(1, 0, 0);

  return atan2(vecx[1], vecx[0]);
}

//}

int main()
{
  std::function correct(interpolateAngles);
  auto totest = std::bind<double(double, double,double)>(sradians::interpUnwrapped, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

  const std::vector<double> tst {-0.0, 0.0, 1, -1, M_PI, -M_PI, 2*M_PI, -2*M_PI};
  std::cout <<
    "┌────────────┬────────────┬────────────┐\n"
    "│  correct   │  mrs_lib   │ difference │\n"
    "├────────────┼────────────┼────────────┤\n"
    ;
  for (const auto& a : tst)
  {
    for (const auto& b : tst)
    {
      for (const auto& c : tst)
      {
        const auto cor = correct(a, b, c);
        const auto res = totest(a, b, c);
        const auto dif = dist(cor, res);
        const bool ok = dif < 1e-9;
        if (!ok)
          std::cout << "\033[1;31m";
        std::cout << std::left << std::showpos << std::setprecision(4)
        << "│ " << std::setw(10) << cor << " │ " << std::setw(10) << res << " │ " << std::setw(10) << dif << " |";
        if (!ok)
          std::cout << "\tfor values: " << std::left << std::showpos << std::setprecision(4) << a << "\tand\t" << b << "\tand\t" << c << "\033[0m";
        std::cout << std::endl;
      }
    }
  }
  std::cout <<
  "└────────────┴────────────┴────────────┘" << std::endl;

  constexpr int N = 1e4;
  const auto start1 = std::chrono::high_resolution_clock::now();
  for (int it = 0; it < N; it++)
  {
    for (const auto& a : tst)
    {
      for (const auto& b : tst)
      {
        for (const auto& c : tst)
        {
          [[maybe_unused]] volatile auto cor = correct(a, b, c);
        }
      }
    }
  }
  const auto stop1 = std::chrono::high_resolution_clock::now();

  const auto start2 = std::chrono::high_resolution_clock::now();
  for (int it = 0; it < N; it++)
  {
    for (const auto& a : tst)
    {
      for (const auto& b : tst)
      {
        for (const auto& c : tst)
        {
          [[maybe_unused]] volatile auto cor = totest(a, b, c);
        }
      }
    }
  }
  const auto stop2 = std::chrono::high_resolution_clock::now();

  std::cout << "dur1: " << std::chrono::duration_cast<std::chrono::microseconds>(stop1-start1).count()/double(N) << "us" << std::endl;
  std::cout << "dur2: " << std::chrono::duration_cast<std::chrono::microseconds>(stop2-start2).count()/double(N) << "us" << std::endl;

  return 0;
}


#include <boost/graph/circle_layout.hpp>


//==================================================================================================
/*!
  @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_FUNCTION_GENERIC_INDEG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_GENERIC_INDEG_HPP_INCLUDED

#include <boost/simd/constant/radindeg.hpp>
#include <boost/simd/constant/radindegr.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/indeg.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( indeg_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_t = A0;
    A0 operator() ( A0 const& a0) const
    {
      return (a0*Radindeg<result_t>())-(a0*Radindegr<result_t>());
    }
  };
} } }

#endif


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

#include <ompl/base/spaces/RealVectorStateSpace.h>
#include "ompl/base/SpaceInformation.h"
#include <ompl/geometric/planners/GSE/GSE.h>
#include "ompl/base/spaces/RealVectorStateProjections.h"

using namespace std;
namespace ob = ompl::base;
namespace og = ompl::geometric;

void genSphere(Eigen::MatrixXd &sphere);
bool isStateValid(const ob::State *state);

int main()
{
    // construct the state space we are planning in
    auto space(std::make_shared<ob::RealVectorStateSpace>(3));

    // set the bounds for the R^3 part of SE(3)
    space->setBounds(-2.1,2.1);

    // construct an instance of  space information from this state space
    auto si(std::make_shared<ob::SpaceInformation>(space));
    // set state validity checking for this space
    si->setStateValidityChecker(isStateValid);

    // create a start state and goal state
    ob::ScopedState<> start(space),goal(space);
    start = {-2.0,-2.0,-2.0};
    goal  = {2.0,2.0,2.0};

    // create a problem instance, set the start and goal states
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));
    pdef->setStartAndGoalStates(start, goal);

    // create a GSE planner for the defined space
    auto planner(std::make_shared<og::GSE>(si,3));
    planner->setProblemDefinition(pdef);

    // Add obstacles to planner for planning
    Eigen::MatrixXd sphere(1000,3);
    genSphere(sphere); // generate a sphere obstacle
    vector<vector<double>> obs{ {0,0,0},
                            {0,-2,0},
                            {0,2,0},
                            {-2,0,0},
                            {2,0,0},
                            {0,0,-2},
                            {0,0,2},
                            {1,1,1}}; // Locations where we want to place the spherical obstacle
    for(auto row:obs){
        Eigen::VectorXd centre = Eigen::Map<Eigen::VectorXd>(row.data(),row.size());
        Eigen::MatrixXd obsi = (0.5*sphere.transpose()).colwise()+centre;
        planner->addObstacle(obsi);
    }

    // Set the Projector to extract an Eigen::Vector state from base::state
    std::vector<uint> projectionDimensions{0,1,2};
    auto projector(std::make_shared<ob::RealVectorOrthogonalProjectionEvaluator>(space,projectionDimensions));
    planner->setProjector(projector);

    // Setup the planner
    planner->setup();

    // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = planner->ob::Planner::solve(1);

    if (solved)
    {
        ob::PathPtr path = pdef->getSolutionPath();
        cout << "Found solution:" << std::endl;

        // print the path to screen
        path->as<og::PathGeometric>()->printAsMatrix(cout);
    }
    else
        cout << "No solution found" << std::endl;

    return 0;
}

// generate Uniform randomn Points on a sphere of raidius 1
// http://corysimon.github.io/articles/uniformdistn-on-sphere/
void genSphere(Eigen::MatrixXd &sphere){
    int N = sphere.rows();
    auto theta = 2*M_PI*(Eigen::ArrayXd::Random(N)*0.5+0.5);
    auto phi = Eigen::ArrayXd::Random(N).acos();

    Eigen::VectorXd x = phi.sin()*theta.cos();
    Eigen::VectorXd y = phi.sin()*theta.sin();
    Eigen::VectorXd z = phi.cos();

    sphere<<x,y,z;
}

bool isStateValid(const ob::State *state)
{
    // return a value that is always true but uses the argument, so we avoid compiler warnings
    return state;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef std::complex<double> complex;
  typedef ublas::vector<complex> vector;
  typedef ublas::vector<double> d_vector;
  typedef ublas::banded_matrix<complex, ublas::column_major> matrix;
  typedef ublas::matrix<complex, ublas::column_major> dense_matrix;
  typedef typename std::make_signed<vector::size_type>::type size_type;

  rand_normal<complex>::reset();
  size_type n=1024;
  matrix A(n, n, 1, 1);
  for (size_type i=0; i<n; ++i)
    A(i, i)=std::abs(rand_normal<complex>::get())+1;
  for (size_type i=0; i<n-1; ++i) {
    A(i+1, i)=complex(0);
    A(i, i+1)=complex(0);
  }
  for (int k=0; k<n-1; ++k) {
    // generate a random 2x2 unitary matrix
    double phi(rand_uniform<double>::get(0, 1.5707963267948966192));
    double alpha(rand_uniform<double>::get(0, 6.2831853071795864770));
    double psi(rand_uniform<double>::get(0, 6.2831853071795864770));
    double chi(rand_uniform<double>::get(0, 6.2831853071795864770));
    dense_matrix u(2, 2);
    u(0, 0)=complex(std::cos(alpha+psi), std::sin(alpha+psi))*std::cos(phi);
    u(1, 0)=-complex(std::cos(alpha-chi), std::sin(alpha-chi))*std::sin(phi);
    u(0, 1)=complex(std::cos(alpha+chi), std::sin(alpha+chi))*std::sin(phi);
    u(1, 1)=complex(std::cos(alpha-psi), std::sin(alpha-psi))*std::cos(phi);
    dense_matrix a(2, 2);
    a(0, 0)=A(k, k);
    a(1, 0)=A(k+1, k);
    a(0, 1)=A(k, k+1);
     a(1, 1)=A(k+1, k+1);
    a=ublas::prod(ublas::trans(ublas::conj(u)), a);
    a=ublas::prod(a, u);
    A(k, k)=a(0, 0);
    A(k+1, k)=a(1, 0);
    A(k, k+1)=a(0, 1);
    A(k+1, k+1)=a(1, 1);
  }
  vector b(n);
  for (size_type i=0; i<n; ++i)
    b(i)=rand_normal<complex>::get();
  vector x(b);
  d_vector d(n);
  vector e(n-1);
  for (size_type i=0; i<n; ++i)
    d(i)=A(i ,i).real();
  for (size_type i=0; i<n-1; ++i)
    e(i)=A(i+1, i);
  int info=lapack::ptsv(d, e, x); // solve
  if (info==0) {
    // res <- A*x - b
    vector res(b);
    blas::gbmv(complex(1, 0), A, x, complex(-1, 0), res);
    std::cout << "norm of residual : " << blas::nrm2(res) << '\n';
  } else
    if (info>0)
      std::cout << "singular matrix\n";
    else
      std::cout << "illegal arguments\n";
  return EXIT_SUCCESS;
}


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

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#include <ql/experimental/xva/gaussian1dscenariogenerator.hpp>
#include <ql/experimental/models/gaussian1dyieldtermstructure.hpp>
#include <ql/processes/forwardmeasureprocess.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

Gaussian1dSingleCurveScenarioGenerator::Gaussian1dSingleCurveScenarioGenerator(
    const boost::shared_ptr<Gaussian1dModel> &model, const unsigned long seed)
    : ScenarioGenerator<YieldTermStructure>(
          model->termStructure()->calendar(),
          model->termStructure()->dayCounter()),
      model_(model) {
    modelTime_ = time();
    modelState_ = model_->stateProcess()->x0();
    mt_ = boost::make_shared<MersenneTwisterUniformRng>(seed);
    icrng_ = boost::make_shared<InverseCumulativeRng<MersenneTwisterUniformRng,
                                                     InverseCumulativeNormal> >(
        *mt_);
}

bool Gaussian1dSingleCurveScenarioGenerator::nextPath() {
    ScenarioGenerator<YieldTermStructure>::nextPath();
    modelTime_ = time();
    return true;
}

const Date
Gaussian1dSingleCurveScenarioGenerator::advance(const Period &suggestedStep) {
    ScenarioGenerator<YieldTermStructure>::advance(suggestedStep);
    Real newModelTime_ = time();
    boost::shared_ptr<ForwardMeasureProcess> fmp =
        boost::dynamic_pointer_cast<ForwardMeasureProcess>(
            model_->stateProcess());
    if (fmp != NULL)
        if (newModelTime_ > fmp->getForwardMeasureTime())
            validHorizonDate_ = false;
    if (validHorizonDate_) {
        Real dt = newModelTime_ - modelTime_;
        Real dw = std::sqrt(dt) * icrng_->next().value;
        modelState_ =
            model_->stateProcess()->evolve(modelTime_, modelState_, dt, dw);
    }
    modelTime_ = newModelTime_;
    return horizonDate();
}

const boost::shared_ptr<YieldTermStructure>
Gaussian1dSingleCurveScenarioGenerator::state() const {
    return boost::make_shared<Gaussian1dYieldTermStructure>(
        model_, modelTime_, model_->y(modelState_, modelTime_));
}
} // namespace QuantLib


// ---------------------------------------------------------------------
//
// Copyright (c) 2014 - 2019 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Test that we get the same thing with the boost version of Gauss-Kronrod
// quadrature that we did with the GSL version.
//
// This test doesn't actually use IBAMR.

#include <boost/math/quadrature/gauss_kronrod.hpp>

#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <numeric>
#include <utility>
#include <vector>

// Set Fish Related Parameters.
static const double PII = 3.1415926535897932384626433832795;
static const double LENGTH_FISH = 1.0;
static const double WIDTH_HEAD = 0.04 * LENGTH_FISH;
static const double LENGTH_TILLHEAD = 0.04 * LENGTH_FISH;
static const double MAJOR_AXIS = 0.51 * LENGTH_FISH;
static const double MINOR_AXIS = 0.08 * LENGTH_FISH;

double
xPosition(double s, void* params)
{
    double* input = static_cast<double*>(params);
    double a0 = input[0];
    double a1 = input[1];
    double a2 = input[2];
    double a3 = input[3];
    double tau = input[4];
    double t = input[5];
    double T = input[6];

    double x =
        std::cos((1 / (8 * std::pow(PII * tau, 4))) * (std::tanh(PII * t / T)) *
                 (2 * PII * tau * (a2 - 2 * a0 * std::pow(PII * tau, 2)) * std::cos((2 * PII * t) / T) +
                  2 * PII * tau * (-a2 - 3 * a3 * s + 2 * PII * PII * (a0 + s * (a1 + s * (a2 + a3 * s))) * tau * tau) *
                      cos((2 * PII * (t - s * T * tau)) / T) +
                  (3 * a3 - 2 * a1 * std::pow(PII * tau, 2)) * sin((2 * PII * t) / T) +
                  (-3 * a3 + 2 * PII * PII * (a1 + 2 * a2 * s + 3 * a3 * s * s) * tau * tau) *
                      std::sin((2 * PII * (t - s * T * tau)) / T)));

    return x;

} // xposition

double
yPosition(double s, void* params)
{
    double* input = static_cast<double*>(params);
    double a0 = input[0];
    double a1 = input[1];
    double a2 = input[2];
    double a3 = input[3];
    double tau = input[4];
    double t = input[5];
    double T = input[6];

    double y =
        std::sin((1 / (8 * std::pow(PII * tau, 4))) * (std::tanh(PII * t / T)) *
                 (2 * PII * tau * (a2 - 2 * a0 * std::pow(PII * tau, 2)) * std::cos((2 * PII * t) / T) +
                  2 * PII * tau * (-a2 - 3 * a3 * s + 2 * PII * PII * (a0 + s * (a1 + s * (a2 + a3 * s))) * tau * tau) *
                      cos((2 * PII * (t - s * T * tau)) / T) +
                  (3 * a3 - 2 * a1 * std::pow(PII * tau, 2)) * sin((2 * PII * t) / T) +
                  (-3 * a3 + 2 * PII * PII * (a1 + 2 * a2 * s + 3 * a3 * s * s) * tau * tau) *
                      std::sin((2 * PII * (t - s * T * tau)) / T)));

    return y;

} // yPosition

int
main()
{
    const double Lx = 8;
    const double Ly = 4;
    const double Lz = 1;
    const int Nx = 16 * 4 * 4 * 2;
    const int Ny = 8 * 4 * 4 * 2;
    const int Nz = 4 * 4 * 4 * 2;

    const double dx = Lx / Nx;
    const double dy = Ly / Ny;
    const double dz = Lz / Nz;

    const double tau_tail = 1.52;
    const double a0 = 1.29, a1 = -22.57, a2 = 78.39, a3 = -52.83;
    const double t = 0.0;
    const double time_period = 1.0;

    double interp_coefs[4];
    interp_coefs[0] = a0;
    interp_coefs[1] = a1;
    interp_coefs[2] = a2;
    interp_coefs[3] = a3;

    // No. of points on the backbone and till head.
    const int headNs = static_cast<int>(ceil(LENGTH_TILLHEAD / dx));
    const int tailNs = static_cast<int>(ceil((LENGTH_FISH - LENGTH_TILLHEAD) / dx));
    const int bodyNs = headNs + tailNs + 1;

    std::vector<std::pair<int, int> > immersedBodyData(bodyNs);
    std::vector<std::pair<double, double> > immersedBodyWidthHeight(bodyNs);

    for (int i = 1; i <= headNs + 1; ++i)
    {
        const double s = (i - 1) * dx;
        const double section = sqrt(2 * WIDTH_HEAD * s - s * s);
        const double height = MINOR_AXIS * std::sqrt(1 - pow((s - MAJOR_AXIS) / MAJOR_AXIS, 2));
        const int numPtsInSection = static_cast<int>(ceil(section / dy));
        const int numPtsInHeight = static_cast<int>(ceil(height / dz));
        immersedBodyData[i - 1] = std::make_pair(numPtsInSection, numPtsInHeight);
        immersedBodyWidthHeight[i - 1] = std::make_pair(section, height);
    }

    for (int i = headNs + 2; i <= bodyNs; ++i)
    {
        const double s = (i - 1) * dx;
        const double section = WIDTH_HEAD * (LENGTH_FISH - s) / (LENGTH_FISH - LENGTH_TILLHEAD);
        const double height = MINOR_AXIS * std::sqrt(1 - pow((s - MAJOR_AXIS) / MAJOR_AXIS, 2));
        const int numPtsInSection = static_cast<int>(ceil(section / dy));
        const int numPtsInHeight = static_cast<int>(ceil(height / dz));
        immersedBodyData[i - 1] = std::make_pair(numPtsInSection, numPtsInHeight);
        immersedBodyWidthHeight[i - 1] = std::make_pair(section, height);
    }

    int total_lag_pts = 0;

    double input[7];
    input[0] = interp_coefs[0];
    input[1] = interp_coefs[1];
    input[2] = interp_coefs[2];
    input[3] = interp_coefs[3];
    input[4] = tau_tail;
    input[5] = t;
    input[6] = time_period;

    // Find the deformed shape. Rotate the shape about center of mass.
    std::vector<std::vector<double> > shape_new(3);
    for (int i = 1; i <= bodyNs; ++i)
    {
        const int numPtsInSection = immersedBodyData[i - 1].first;
        const int numPtsInHeight = immersedBodyData[i - 1].second;
        const double width = immersedBodyWidthHeight[i - 1].first;
        const double depth = immersedBodyWidthHeight[i - 1].second;
        const double s = (i - 1) * dx;

        auto f_x = [&](const double x) { return xPosition(x, input); };

        auto f_y = [&](const double y) { return yPosition(y, input); };

        namespace bmq = boost::math::quadrature;
        const double xbase = bmq::gauss_kronrod<double, 15>::integrate(f_x, 0.0, s, 15, 1e-12, nullptr);
        const double ybase = bmq::gauss_kronrod<double, 15>::integrate(f_y, 0.0, s, 15, 1e-12, nullptr);

        if (numPtsInSection && numPtsInHeight)
        {
            // Fill the middle line first.
            for (int k = -numPtsInHeight; k <= numPtsInHeight; ++k)
            {
                shape_new[0].push_back(xbase);
                shape_new[1].push_back(ybase);
                shape_new[2].push_back(k * dz);
            } // middle line filled.

            total_lag_pts += 2 * numPtsInHeight + 1;

            // Fill the rest of the cross section next.
            for (int j = 1; j <= numPtsInSection; ++j)
            {
                const double y = j * dy;
                for (int k = -numPtsInHeight; k <= numPtsInHeight; ++k)
                {
                    const double z = k * dz;
                    if ((std::pow(y / width, 2) + std::pow(z / depth, 2)) <= 1) // use elliptical cross sections
                    {
                        shape_new[0].push_back(xbase); // right side.
                        shape_new[1].push_back(ybase + y);
                        shape_new[2].push_back(z);

                        shape_new[0].push_back(xbase); // left side.
                        shape_new[1].push_back(ybase - y);
                        shape_new[2].push_back(z);

                        total_lag_pts += 2;
                    }
                }
            } // cross section filled
        }
    }
    std::ofstream eelstream("output");
    eelstream.precision(12);
    assert(static_cast<std::size_t>(total_lag_pts) == shape_new[0].size());
    eelstream << total_lag_pts << "\n";

    for (int k = 1; k <= total_lag_pts; ++k)
        eelstream << shape_new[0][k - 1] + 5.5 << "\t" << shape_new[1][k - 1] << "\t" << shape_new[2][k - 1] << "\n";

    return 0;
}


/*    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.
 *
 *    References
 *      An example of a heritage code which uses such a mesh is found in:
 *          The Mark IV Supersonic-Hypersonic Arbitrary Body Program, Volume
 *          II-Program Formulation, Douglas Aircraft Company, AFFDL-TR-73-159,
 *          Volume II.
 *
 *    Notes
 *      The numberOfLines_ and numberOfPoints_ member variables denote the number of mesh points.
 *      The number of panels in the mesh will be numberOfLines_ - 1 by numberOfPoints_ - 1.
 *
 */

#include <boost/multi_array.hpp>
#include <iostream>
#include <limits>
#include <Eigen/Geometry>

#include "Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.h"

namespace tudat
{
namespace geometric_shapes
{

//! Calculate panel characteristics.
void QuadrilateralMeshedSurfaceGeometry::performPanelCalculations( )
{
    // Allocate memory for panel properties.
    panelCentroids_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelSurfaceNormals_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelAreas_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);

    // Declare local variables for normal and area determination.
    Eigen::Vector3d crossVector1;
    Eigen::Vector3d crossVector2;

    // Reset total area.
    totalArea_ = 0.0;

    // Loop over all panels to determine properties.
    for ( int i = 0; i < numberOfLines_ - 1; i++ )
    {
        for ( int j = 0; j < numberOfPoints_ - 1; j++ )
        {
            // Set panel centroid.
            panelCentroids_[ i ][ j ] = ( meshPoints_[ i ][ j ] +
                                          meshPoints_[ i + 1 ][ j ] +
                                          meshPoints_[ i ][ j + 1 ] +
                                          meshPoints_[ i + 1 ][ j + 1 ] ) / 4;

            // Set panel cross vectors.
            crossVector1 = meshPoints_[ i + 1 ][ j + 1 ] - meshPoints_[ i ][ j ];
            crossVector2 = meshPoints_[ i + 1 ][ j ] - meshPoints_[ i ][ j + 1 ];

            // Set panel normal (not yet normalized).
            panelSurfaceNormals_[ i ][ j ] = crossVector1.cross( crossVector2 );

            // Set panel area (not yet correct size).
            panelAreas_[ i ][ j ] = panelSurfaceNormals_[ i ][ j ].norm( );
            if ( panelAreas_[ i ][ j ] < std::numeric_limits< double >::epsilon( ) )
            {
                std::cerr << "Warning, panel area is zero in part at panel" << i
                          << ", " << j << std::endl;
            }

            // Normalize panel normal and, if necessary, invert normal direction.
            panelSurfaceNormals_[ i ][ j ] *= reversalOperator_;
            panelSurfaceNormals_[ i ][ j ].normalize( );

            // Set panel area to correct size.
            panelAreas_[ i ][ j ] *= 0.5;

            // Add panel area to total area.
            totalArea_ += panelAreas_[ i ][ j ];
        }
    }
}

//! Set reversal operator.
void QuadrilateralMeshedSurfaceGeometry::setReversalOperator( const bool isMeshInverted )
{
    if ( isMeshInverted == 0 )
    {
        reversalOperator_ = 1;
    }

    else
    {
        reversalOperator_ = -1;
    }
}

//! Get boolean denoting if the mesh is inverted.
bool QuadrilateralMeshedSurfaceGeometry::getReversalOperator( )
{
    bool isMeshInverted;
    if ( reversalOperator_ == 1 )
    {
        isMeshInverted = 0;
    }

    else
    {
        isMeshInverted = 1;
    }

    return isMeshInverted;
}

//! Overload ostream to print class information.
std::ostream& operator << ( std::ostream& stream,
                          QuadrilateralMeshedSurfaceGeometry& quadrilateralMeshedSurfaceGeometry )
{
    stream << "This is a quadrilateral meshed surface geometry"
           << " of a single part." << std::endl;
    stream << "The number of lines ( contours ) is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfLines_ << std::endl;
    stream << "The number of points per line is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfPoints_ << std::endl;

    // Return stream.
    return stream;
}

} // namespace geometric_shapes
} // namespace tudat


#include <path_follower/controller/robotcontroller_ackermann_inputscaling.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/visualizer.h>
#include <cslibs_navigation_utilities/MathHelper.h>
#include <deque>
#include <limits>
#include <boost/algorithm/clamp.hpp>
#include <time.h>
#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_Ackermann_Inputscaling, ackermann_inputscaling, ackermann);

RobotController_Ackermann_Inputscaling::RobotController_Ackermann_Inputscaling() :
    RobotController(),
    phi_(0.),
	v1_(0.), v2_(0.)
{

	const double k = params_.k_forward();
	setTuningParameters(k);

	ROS_INFO("Parameters: k_forward=%f, k_backward=%f\n"
				"factor_k1=%f, k2=%f, k3=%f\n"
				"vehicle_length=%f\n"
				"factor_steering_angle=%f\n"
				"goal_tolerance=%f\nmax_steering_angle=%f",
				params_.k_forward(), params_.k_backward(),
				params_.factor_k1(), params_.factor_k2(), params_.factor_k3(),
				params_.vehicle_length(),
				params_.factor_steering_angle(),
				params_.goal_tolerance(), params_.max_steering_angle());

}

void RobotController_Ackermann_Inputscaling::setTuningParameters(const double k) {
	k1_ = params_.factor_k1() * k * k * k;
	k2_ = params_.factor_k2() * k * k;
	k3_ = params_.factor_k3() * k;
}

void RobotController_Ackermann_Inputscaling::stopMotion() {

	move_cmd_.setVelocity(0.f);
	move_cmd_.setDirection(0.f);

	phi_ = 0.;

	MoveCommand cmd = move_cmd_;
	publishMoveCommand(cmd);
}

void RobotController_Ackermann_Inputscaling::start() {

}

void RobotController_Ackermann_Inputscaling::reset() {
	old_time_ = ros::Time::now();

	v1_ = v2_ = 0.;

	s_prim_ = 0.001; // TODO: good starting value

    RobotController::reset();
}

void RobotController_Ackermann_Inputscaling::setPath(Path::Ptr path)
{
    RobotController::setPath(path);
}

RobotController::MoveCommandStatus RobotController_Ackermann_Inputscaling::computeMoveCommand(
        MoveCommand* cmd)
{
	if(path_interpol.n() <= 2)
		return RobotController::MoveCommandStatus::ERROR;

    const Eigen::Vector3d pose = pose_tracker_->getRobotPose();
    const geometry_msgs::Twist velocity_measured = pose_tracker_->getVelocity();

	ROS_DEBUG("velocity_measured: x=%f, y=%f, z=%f", velocity_measured.linear.x,
				velocity_measured.linear.y, velocity_measured.linear.z);

    RobotController::findOrthogonalProjection();
    double d = orth_proj_;

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

	// draw a line to the orthogonal projection
	geometry_msgs::Point from, to;
    from.x = pose[0]; from.y = pose[1];
    to.x = path_interpol.p(proj_ind_); to.y = path_interpol.q(proj_ind_);
    visualizer_->drawLine(12341234, from, to, getFixedFrame(), "Inputscaling", 1, 0, 0, 1, 0.01);

	// theta_e = theta_vehicle - theta_path (orientation error)
    double theta_e = MathHelper::AngleDelta(path_interpol.theta_p(proj_ind_), pose[2]);

	// if dir_sign is negative we drive backwards and set theta_e to the complementary angle
    if (getDirSign() < 0.) {
        d = -d;
		setTuningParameters(params_.k_backward());
        theta_e = theta_e > 0.? M_PI - theta_e : -M_PI - theta_e;
	} else {
		setTuningParameters(params_.k_forward());
	}

	// curvature and first two derivations
    const double c = path_interpol.curvature(proj_ind_);
    const double c_prim = path_interpol.curvature_prim(proj_ind_);
    const double c_sek = path_interpol.curvature_sek(proj_ind_);

	// 1 - dc(s)
	const double _1_dc = 1. - d * c;
	const double _1_dc_2 = _1_dc * _1_dc;

	// cos, sin, tan of theta_e and phi
	const double cos_theta_e = cos(theta_e);
	const double cos_theta_e_2 = cos_theta_e * cos_theta_e;
	const double cos_theta_e_3 = cos_theta_e_2 * cos_theta_e;

	const double sin_theta_e = sin(theta_e);
	const double sin_theta_e_2 = sin_theta_e * sin_theta_e;

	const double tan_theta_e = tan(theta_e);
	const double tan_theta_e_2 = tan_theta_e * tan_theta_e;

	const double tan_phi = tan(phi_);
	const double tan_phi_2 = tan_phi * tan_phi;

	const double time_passed = (ros::Time::now() - old_time_).toSec();
	old_time_ = ros::Time::now();

	v1_ = abs(velocity_measured.linear.x);

	s_prim_ = cos_theta_e / _1_dc;

	const double dd_ds = sin_theta_e * v1_ / s_prim_;

    //theta_e_prim_ = (theta_e - old_theta_e_) * delta_s_inverse;
	const double dtheta_e_ds = ((tan_phi / params_.vehicle_length() - c * cos_theta_e / _1_dc) * v1_)
			/ s_prim_;

    // follows from: phi_prim = phi / t, s_prim = s / t, v2 = phi / t
	const double dphi_ds = v2_ / s_prim_;

	ROS_DEBUG("s_prim=%f, delta_s=%f", s_prim_, s_prim_ * time_passed);
	ROS_DEBUG("d'=%f, theta_e'=%f, phi'=%f", dd_ds, dtheta_e_ds, dphi_ds);

	//
	// actual controller formulas begin here
	//

	// x1 - x4
	//	const double x1 = s;
	const double x2 = -c_prim * d * tan_theta_e
			- c * _1_dc * (1. + sin_theta_e_2) / cos_theta_e_2
			+ _1_dc_2 * tan_phi / (params_.vehicle_length() * cos_theta_e_3);

	const double x3 = _1_dc * tan_theta_e;
	const double x4 = d;

	// u1, u2
	// u1 is taken from "Feedback control for a path following robotic car" by Mellodge,
	// p. 108 (u1_actual)

	// TODO: use measured velocity
	const double u1 = velocity_ * cos_theta_e / _1_dc;
	const double u2 =
            - k1_ * u1 * x4
            - k2_ * u1 * x3
            - k3_ * u1 * x2;

	// derivations of x2 (for alpha1)
	const double dx2_dd = -c_prim * tan_theta_e
			+ c * c * (1 + sin_theta_e_2) / cos_theta_e_2
			- 2. * _1_dc * c * tan_phi / (params_.vehicle_length() * cos_theta_e_3);

	const double dx2_dtheta_p = -c_prim * (tan_theta_e_2 + 1.)
			- 4. * c * _1_dc * tan_theta_e / cos_theta_e_2
            + 3. * _1_dc_2 * tan_phi * tan_theta_e / (params_.vehicle_length() * cos_theta_e_3);

	const double dx2_ds =
			-tan_theta_e * (c_sek * d + c_prim * dd_ds)
			- c_prim * d * dtheta_e_ds * (1. + tan_theta_e_2)
			+ ((1. + sin_theta_e_2) / cos_theta_e_2) * (c_prim * _1_dc + c * (dd_ds * c + d * c_prim))
			- 4. * c * _1_dc * tan_theta_e / cos_theta_e_2
			+ (cos_theta_e * _1_dc * (-2. * (dd_ds * c + d * c_prim) * tan_phi
											  +_1_dc * (1. + tan_phi_2) * dphi_ds)
				- 3. * dtheta_e_ds * sin_theta_e * _1_dc_2 * tan_phi)
			/ (params_.vehicle_length() * pow(cos_theta_e_2, 2)); // OK

	// alpha1
	const double alpha1 =
			dx2_ds
			+ dx2_dd * _1_dc * tan_theta_e
			+ dx2_dtheta_p * (tan_phi * _1_dc / (params_.vehicle_length() * cos_theta_e) - c);

	// alpha2
	const double alpha2 =
			params_.vehicle_length() * cos_theta_e_3 * pow(cos(phi_), 2) / _1_dc_2;

	// longitudinal velocity
    v1_ = velocity_;

	// steering angle velocity
	v2_ = alpha2 * (u2 - alpha1 * u1);

	// limit steering angle velocity
	v2_ = boost::algorithm::clamp(v2_, -params_.max_steering_angle_speed(), params_.max_steering_angle_speed());

	// update delta according to the time that has passed since the last update
	phi_ += v2_ * time_passed;

	// also limit the steering angle
	phi_ = boost::algorithm::clamp(phi_, -params_.max_steering_angle(), params_.max_steering_angle());

	ROS_DEBUG("d=%f, thetaP=%f, c=%f, c'=%f, c''=%f", d, theta_e, c, c_prim, c_sek);
	ROS_DEBUG("d'=%f, thetaP'=%f", dd_ds, dtheta_e_ds);
	ROS_DEBUG("1 - dc(s)=%f", _1_dc);
	ROS_DEBUG("dx2dd=%f, dx2dthetaP=%f, dx2ds=%f", dx2_dd, dx2_dtheta_p, dx2_ds);
	ROS_DEBUG("alpha1=%f, alpha2=%f, u1=%f, u2=%f", alpha1, alpha2, u1, u2);
	ROS_DEBUG("Time passed: %fs, command: v1=%f, v2=%f, phi_=%f",
				 time_passed, v1_, v2_, phi_);

	// This is the accurate steering angle for 4 wheel steering (TODO: wrong!!!)
	const float delta = (float) asin(params_.factor_steering_angle() * sin(phi_));

    double exp_factor = RobotController::exponentialSpeedControl();
	move_cmd_.setDirection(delta);
    move_cmd_.setVelocity(getDirSign() * (float) v1_ * exp_factor);
	*cmd = move_cmd_;

	return RobotController::MoveCommandStatus::OKAY;
}

void RobotController_Ackermann_Inputscaling::publishMoveCommand(
		const MoveCommand& cmd) const {

	geometry_msgs::Twist msg;
	msg.linear.x  = cmd.getVelocity();
	msg.linear.y  = 0;
	msg.angular.z = cmd.getDirectionAngle();

	cmd_pub_.publish(msg);
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NTHROOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NTHROOT_HPP_INCLUDED
#include <boost/simd/function/raw.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_INVALID
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    inline A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
#ifndef BOOST_SIMD_NO_INVALID
      if (is_nan(a0)) return a0;
#endif
      auto is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
#ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0)) return (a1) ? a0 : One<A0>();
#endif
      A0 aa1 = static_cast<A0>(a1);
      A0 y = bs::raw_(bs::pow_abs)(x,rec(aa1));
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      if (y)
      {
       A0 p = raw_(bs::pow_abs)(y, aa1);
       y -= (p - x) / (aa1*p/y);
      }

      return (is_ltza0 && is_odda1)? -y : y;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    inline A0 operator() (const raw_tag &,  A0 a0, A1 a1
                         ) const BOOST_NOEXCEPT
    {
       auto is_ltza0 = is_ltz(a0);
       auto is_odda1 = is_odd(a1);
       if (is_ltza0 && !is_odda1) return Nan<A0>();
       A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
      A0 aa1 = static_cast<A0>(a1);
      A0 y = raw_(bs::pow_abs)(x,rec(aa1));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };
} } }

#endif


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

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/.

*/

#ifndef PIC_COMPUTER_VISION_FIND_CHECKER_BOARD_HPP
#define PIC_COMPUTER_VISION_FIND_CHECKER_BOARD_HPP

#include "../filtering/filter_luminance.hpp"
#include "../filtering/filter_bilateral_2ds.hpp"

#include "../computer_vision/iterative_closest_point_2D.hpp"
#include "../computer_vision/nelder_mead_opt_ICP_2D.hpp"

#include "../features_matching/harris_corner_detector.hpp"
#include "../features_matching/orb_descriptor.hpp"

#include "../util/rasterizer.hpp"
#include "../util/eigen_util.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

namespace pic {

/**
 * @brief getMinDistance
 * @param points
 * @return
 */
#ifndef PIC_DISABLE_EIGEN

PIC_INLINE float getMinDistance(std::vector< Eigen::Vector2f > &points)
{
    float ret = FLT_MAX;
    for(unsigned int i = 0; i < points.size(); i++) {

        auto p_i = points[i];

        for(unsigned int j = 0; j < points.size(); j++) {
            if(j == i) {
                continue;
            }

            auto delta_ij = p_i - points[j];
            float dist = delta_ij.norm();

            if(dist < ret) {
                ret = dist;
            }
        }
    }

    return ret;
}

/**
 * @brief estimateCheckerBoardSize
 * @param points
 * @return
 */
PIC_INLINE float estimateCheckerBoardSize(std::vector< Eigen::Vector2f > &points)
{
    if(points.size() < 2) {
        return -1.0f;
    }

    float ret = 0.0f;

    int n = int(points.size());

    std::vector<float> m_d;
    for(int i = 0; i < n; i++) {
        auto p_i = points[i];

        float closest = FLT_MAX;

        for(int j = 0; j < n; j++) {
            if(j == i) {
                continue;
            }

            auto delta_ij = p_i - points[j];

            float dist = delta_ij.norm();

            if(dist < closest) {
                closest = dist;
            }
        }

        if(closest < FLT_MAX) {
            m_d.push_back(closest);
        }
    }

    if(!m_d.empty()) {
        std::sort(m_d.begin(), m_d.end());

        ret = m_d[m_d.size() / 2];
    }

    return ret;
}

/**
 * @brief estimateCheckerBoardSizeCross
 * @param points
 * @return
 */
PIC_INLINE float estimateCheckerBoardSizeCross(std::vector< Eigen::Vector2f > &points)
{
    if(points.size() < 2) {
        return -1.0f;
    }

    float ret = 0.0f;

    int n = int(points.size());

    std::vector<float> m_d;
    for(int i = 0; i < n; i++) {
        auto p_i = points[i];

        float c[] = {FLT_MAX, FLT_MAX, FLT_MAX};
        int ci[] = {-1, -1, -1};

        for(int j = 0; j < n; j++) {
            if(j == i) {
                continue;
            }

            auto delta_ij = p_i - points[j];

            float dist = delta_ij.norm();

            if(dist < c[0]) {
                c[0] = dist;
                ci[0] = j;
            } else {
                if(dist < c[1]) {
                    c[1] = dist;
                    ci[1] = j;
                } else {
                    if(dist < c[2]) {
                        c[2] = dist;
                        ci[2] = j;
                    }
                }
            }
        }

        Eigen::Vector2f v0 = (points[ci[0]] - p_i) / c[0];
        Eigen::Vector2f v1 = (points[ci[1]] - p_i) / c[1];
        Eigen::Vector2f v2 = (points[ci[2]] - p_i) / c[2];

        float v01 = fabsf(v0.dot(v1));
        float v02 = fabsf(v0.dot(v2));
        float v12 = fabsf(v1.dot(v2));

        if((v01 < 0.1f) || (v02 < 0.1f) || (v12 < 0.1f))
        {
            m_d.push_back(c[0]);
        }
    }

    if(!m_d.empty()) {
        std::sort(m_d.begin(), m_d.end());

        ret = m_d[m_d.size() / 2];
    }

    return ret;
}

/**
 * @brief getCheckerBoardModel
 * @param chekers_x
 * @param checkers_y
 * @param checkers_size
 * @param out
 * @return
 */
PIC_INLINE Image *getCheckerBoardModel(int checkers_x, int checkers_y, int checkers_size, std::vector< Eigen::Vector2f > &out)
{
    Image *ret = new Image(1, (checkers_x + 1) * checkers_size, (checkers_y + 1) * checkers_size, 1);
    *ret = 1.0f;

    for(int i = 1; i <= checkers_y; i++) {
        Eigen::Vector2f point;

        int y = i * checkers_size;
        point[1] = float(y);

        for(int j = 1; j <= checkers_x; j++) {

            int x = j * checkers_size;
            point[0] = float(x);

            bool bDraw = false;
            if(j < checkers_x) {
                if(((j % 2) == 0) && ((i % 2) == 0)) {
                    bDraw = true;
                }
            }

            if(i < checkers_y) {
                if(((j % 2) == 1) && ((i % 2) == 1)) {
                    bDraw = true;
                }
            }

            if(bDraw) {
                for(int yy = y; yy < (y + checkers_size); yy++) {
                    for(int xx = x; xx < (x + checkers_size); xx++) {
                        float *pixel_value = (*ret)(xx, yy);
                        pixel_value[0] = 0.0f;
                    }
                }
            }

            out.push_back(point);
        }
    }

    return ret;
}

/**
 * @brief findCheckerBoard
 * @param img
 * @param corners_model
 * @param checkerBoardSizeX
 * @param checkerBoardSizeY
 */
PIC_INLINE void findCheckerBoard(Image *img, std::vector< Eigen::Vector2f > &corners_model, int checkerBoardSizeX = 4, int checkerBoardSizeY = 7)
{
     corners_model.clear();

    //get corners
#ifdef PIC_DEBUG
    printf("Extracting corners...\n");
#endif

    //compute the luminance images
    HarrisCornerDetector hcd(2.5f, 5);
    std::vector< Eigen::Vector2f > corners_from_img;
    hcd.execute(img, &corners_from_img);

    #ifdef PIC_DEBUG
        //automatic white balance
        float *col_mu = img->getMeanVal(NULL, NULL);
        float *scaling = FilterWhiteBalance::getScalingFactors(col_mu, img->channels);
        FilterWhiteBalance fwb(scaling, img->channels, true);

        Image *img_wb = fwb.Process(Single(img), NULL);

        float red[] = {1.0f, 0.0f, 0.0f};
        float green[] = {0.0f, 1.0f, 0.0f};
        float blue[] = {1.0f, 0.0f, 1.0f};
        float yellow[] = {1.0f, 1.0f, 0.0f};

        (*img_wb) *= 0.125f;
    #endif

    std::vector< Eigen::Vector2f > cfi_out;
    GeneralCornerDetector::removeClosestCorners(&corners_from_img, &cfi_out, 16.0f, 64);

    //compute checkerboard size
    float checker_size = estimateCheckerBoardSize(corners_from_img);

    #ifdef PIC_DEBUG
        //drawPoints(img_wb, cfi_out, blue);
    #endif

    //
    // remove very closed points
    //

    std::vector< Eigen::Vector2f > cfi_valid2;
    auto n =  cfi_out.size();
    for(unsigned int i = 0; i < n; i++) {
        auto p_i = cfi_out[i];

        bool bFlag = true;

        for(unsigned int j = 0; j < n; j++) {
            if(j != i) {
                auto delta_ij = p_i - cfi_out[j];
                float dist = delta_ij.norm();

                if(dist < (checker_size)) {
                    bFlag = false;
                    break;
                }
            }
        }

        if(bFlag) {
            cfi_valid2.push_back(p_i);
        }
    }

    //
    // remove very far away points
    //

    std::vector< Eigen::Vector2f > cfi_valid;
    n =  cfi_valid2.size();
    for(unsigned int i = 0; i < n; i++) {
        auto p_i = cfi_valid2[i];

        float dist = 1e32f;
        for(unsigned int j = 0; j < n; j++) {
            if(j != i) {
                auto delta_ij = p_i - cfi_valid2[j];
                float t_dist = delta_ij.norm();

                if(t_dist < dist) {
                    dist = t_dist;
                }
            }
        }

        if(dist < (checker_size * 3)) {
            cfi_valid.push_back(p_i);
        }
    }

    #ifdef PIC_DEBUG
        printf("Checker size: %f\n", checker_size);
        drawPoints(img_wb, cfi_valid, green);
    #endif

    checker_size = estimateCheckerBoardSizeCross(cfi_valid);

#ifdef PIC_DEBUG
    printf("Re-fit Checker size: %f\n", checker_size);
#endif
    //pattern image

    int checkers_size = 32;
    Image *img_pattern = getCheckerBoardModel(checkerBoardSizeX, checkerBoardSizeY, checkers_size, corners_model);
//    corners_model.erase(corners_model.begin() + 3);
//    corners_model.erase(corners_model.begin());

    ORBDescriptor b_desc(checkers_size, 256);

    std::vector< unsigned int *> descs_model, descs_cfi_valid;
    b_desc.getAll(img_pattern, corners_model, descs_model);
    b_desc.getAll(img, cfi_valid, descs_cfi_valid);

    //scale the model using the checker size
    float min_dist = getMinDistance(corners_model);
    float scaling_factor = checker_size / min_dist;

    ICP2DTransform t_init;
    t_init.scale = scaling_factor;
    t_init.applyC(corners_model);

    //run 2D ICP
    iterativeClosestPoints2D(corners_model, cfi_valid, descs_model, descs_cfi_valid, b_desc.getDescriptorSize(), 3000);

#ifdef PIC_DEBUG
    drawPoints(img_wb, corners_model, red);
#endif

    //At this point, the rotation may be wrong so
    //this brute-force trick does the job.
    NelderMeadOptICP2D opt(corners_model, cfi_valid);

    float prev_err = FLT_MAX;
    float *x = new float[3];
    int nSample = 72;

    float *tmp = new float[4];
    for(float i = 0; i < nSample; i++) {
        float angle = float(i) * C_PI_2 / float(nSample);
        float start[] = {0.0f, 0.0f, angle};
        opt.run(start, 3, 1e-9f, 100, tmp);

        if(opt.output_error < prev_err) {
            memcpy(x, tmp, sizeof(float) * 3);
            prev_err = opt.output_error;
        }
    }

    #ifdef PIC_DEBUG
        for(int i = 0; i < 4; i++) {
            printf("%f\n", x[i]);
        }
    #endif

    float start[] = {x[0], x[1], x[2], 1.0f};
    opt.run(start, 4, 1e-12f, 100, tmp);
    ICP2DTransform t2(tmp[0], tmp[1], tmp[2], tmp[3]);

    #ifdef PIC_DEBUG
        for(int i = 0; i < 4; i++) {
            printf("%f\n", tmp[i]);
        }
    #endif

    t2.applyC(corners_model);

    #ifdef PIC_DEBUG
        drawPoints(img_wb, corners_model, yellow);
        img_wb->Write("../data/output/img_wb.bmp");

        if(img_wb != NULL) {
            delete img_wb;
        }
    #endif
}

/**
 * @brief estimateLengthInPixelOfCheckers
 * @param corners_model
 * @param p0
 * @param p1
 * @return
 */
PIC_INLINE float estimateLengthOfCheckers(std::vector< Eigen::Vector2f > &corners_model, Eigen::Vector2f &p0, Eigen::Vector2f &p1)
{
    if(corners_model.size() < 8) {
        return -1.0f;
    }

    int selected = 5;
    auto p_0 = corners_model[selected];

    int closest = -1;
    float ret = FLT_MAX;
    for(auto j = 0; j < corners_model.size(); j++) {
        if(j != selected) {
            auto delta_ij = p_0 - corners_model[j];
            float dist = delta_ij.norm();

            if(dist < ret) {
                ret = dist;
                closest = j;
            }
        }
    }

    p0 = p_0;
    p1 = corners_model[closest];

    return ret;
}

/**
 * @brief estimateCoordinatesWhitePointFromCheckerBoard
 * @param img
 * @param corners_model
 * @param checkerBoardSizeX
 * @param checkerBoardSizeY
 * @return
 */
PIC_INLINE Eigen::Vector2f estimateCoordinatesWhitePointFromCheckerBoard(Image *img, std::vector< Eigen::Vector2f > &corners_model, int checkerBoardSizeX = 4, int checkerBoardSizeY = 6)
{
    Eigen::Vector2f ret(-1.0f, -1.0f);

    if(img == NULL || corners_model.empty()) {
        return ret;
    }

    float maxVal = 0.0f;

    for(int i = 0; i < (checkerBoardSizeY -1) ; i++) {
        for(int j = 0; j < (checkerBoardSizeX - 1); j++) {

            int ind0 = (i * checkerBoardSizeX) + j;
            int ind1 = (i + 1) * checkerBoardSizeX + j + 1;

            auto p0 = corners_model[ind0];
            auto p1 = corners_model[ind1];

            auto pMid = (p0 + p1) / 2.0f;

            int x = int(pMid[0]);
            int y = int(pMid[1]);
            float *color = (*img)(x, y);

            float meanColor = 0.0f;
            for(auto c = 0; c < img->channels; c++) {
                meanColor += color[c];
            }

            if(meanColor > maxVal) {
                maxVal = meanColor;
                ret = pMid;
            }
        }
    }

    return ret;
}

/**
 * @brief estimateWhitePointFromCheckerBoard
 * @param img
 * @param corners_model
 * @param checkerBoardSizeX
 * @param checkerBoardSizeY
 * @return
 */
PIC_INLINE float *estimateWhitePointFromCheckerBoard(Image *img, std::vector< Eigen::Vector2f > &corners_model, int checkerBoardSizeX = 4, int checkerBoardSizeY = 6)
{
    Eigen::Vector2f point = estimateCoordinatesWhitePointFromCheckerBoard(img, corners_model, checkerBoardSizeX, checkerBoardSizeY);

    if(point[0] >= 0.0f && point[1] >= 0.0f) {

        float *ret = new float[img->channels];
        float *color = (*img)(int(point[0]), int(point[1]));
        memcpy(ret, color, img->channels * sizeof(float));

        return ret;
    } else {
        return NULL;
    }
}

#endif

} // end namespace pic

#endif // PIC_COMPUTER_VISION_FIND_CHECKER_BOARD_HPP


#ifndef MLT_MODELS_TRANSFORMERS_SPARSE_AUTOENCODER_HPP
#define MLT_MODELS_TRANSFORMERS_SPARSE_AUTOENCODER_HPP

#include <cmath>
#include <tuple>
#include <type_traits>

#include <Eigen/Core>

#include "transformer.hpp"
#include "../implementations/autoencoder.hpp"
#include "../../utils/eigen.hpp"

namespace mlt {
namespace models {
namespace transformers {
	using namespace utils::eigen;

	template <class HiddenActivation, class ReconstructionActivation, class Optimizer>
	class SparseAutoencoder : public Transformer<SparseAutoencoder<HiddenActivation, ReconstructionActivation, Optimizer>> {
	public:
		template <typename H, typename R, typename O,
			class = enable_if<is_convertible<decay_t<H>, HiddenActivation>::value
			&& is_convertible<decay_t<R>, ReconstructionActivation>::value
			&& is_convertible<decay_t<O>, Optimizer>::value>>
		explicit SparseAutoencoder(int hidden_units, H&& hidden_activation, R&& reconstruction_activation, O&& optimizer, double regularization,
		double sparsity, double sparsity_weight) : _hidden_units(hidden_units), _hidden_activation(forward<H>(hidden_activation)),
			_reconstruction_activation(forward<R>(reconstruction_activation)), _optimizer(forward<O>(optimizer)), _regularization(regularization),
			_sparsity(sparsity), _sparsity_weight(sparsity_weight) {}

		Result transform(Features input) const {
			assert(_fitted);
			return _hidden_activation.compute((_hidden_weights * input).colwise() + _hidden_intercepts);
		}

		Self& fit(Features input, bool cold_start = true) {
			VectorXd init(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units + input.rows());

			if (_fitted && !cold_start) {
				init.block(0, 0, _hidden_weights.size(), 1) = ravel(_hidden_weights);
				init.block(_hidden_weights.size(), 0, _hidden_intercepts.size(), 1) = _hidden_intercepts;

				init.block(_hidden_weights.size() + _hidden_intercepts.size(), 0, _reconstruction_weights.size(), 1) =
					ravel(_reconstruction_weights);
				init.block(_hidden_weights.size() + _hidden_intercepts.size() + _reconstruction_weights.size(),
					0, _reconstruction_intercepts.size(), 1) = _reconstruction_intercepts;
			} else {
				init = (init.setRandom() * 4 / sqrt(6.0 / (_hidden_units + input.rows())));
			}

			VectorXd coeffs = _optimizer(*this, input, input, init, cold_start);

			_hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			_hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			_reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, _hidden_units * input.rows(), 1), input.rows(), _hidden_units);
			_reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			_fitted = true;

			return *this;
		}

		using Transformer<Self>::fit;

		auto loss(VectorXdRef coeffs, Features input, MatrixXdRef target) const {
			auto hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows() * _hidden_units, 1), input.rows(), _hidden_units);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			return implementations::autoencoder::sparse_loss(_hidden_activation, _reconstruction_activation, hidden_weights, hidden_intercepts,
				reconstruction_weights, reconstruction_intercepts, _regularization, _sparsity, _sparsity_weight, input, target);
		}

		auto gradient(VectorXdRef coeffs, Features input, MatrixXdRef target) const {
			auto hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows() * _hidden_units, 1), input.rows(), _hidden_units);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			MatrixXd hid_weights_grad, rec_weights_grad;
			VectorXd hid_inter_grad, rec_inter_grad;

			tie(hid_weights_grad, hid_inter_grad, rec_weights_grad, rec_inter_grad) = implementations::autoencoder::sparse_gradient(_hidden_activation,
				_reconstruction_activation, hidden_weights, hidden_intercepts, reconstruction_weights, reconstruction_intercepts, _regularization,
				_sparsity, _sparsity_weight, input, target);

			VectorXd gradient(coeffs.rows());

			gradient.block(0, 0, hid_weights_grad.size(), 1) = ravel(hid_weights_grad);
			gradient.block(hid_weights_grad.size(), 0, hid_inter_grad.size(), 1) = hid_inter_grad;

			gradient.block(hid_weights_grad.size() + hid_inter_grad.size(), 0, rec_weights_grad.size(), 1) =
					ravel(rec_weights_grad);
			gradient.block(hid_weights_grad.size() + hid_inter_grad.size() + rec_weights_grad.size(),
					0, rec_inter_grad.size(), 1) = rec_inter_grad;

			return gradient;
		}

		tuple<double, VectorXd> loss_and_gradient(VectorXdRef coeffs, Features input, MatrixXdRef target) const {
			auto hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows() * _hidden_units, 1), input.rows(), _hidden_units);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			double loss;
			MatrixXd hid_weights_grad, rec_weights_grad;
			VectorXd hid_inter_grad, rec_inter_grad;

			tie(loss, hid_weights_grad, hid_inter_grad, rec_weights_grad, rec_inter_grad) = implementations::autoencoder::sparse_loss_and_gradient(_hidden_activation,
				_reconstruction_activation, hidden_weights, hidden_intercepts, reconstruction_weights, reconstruction_intercepts, _regularization,
				_sparsity, _sparsity_weight, input, target);

			VectorXd gradient(coeffs.rows());

			gradient.block(0, 0, hid_weights_grad.size(), 1) = ravel(hid_weights_grad);
			gradient.block(hid_weights_grad.size(), 0, hid_inter_grad.size(), 1) = hid_inter_grad;

			gradient.block(hid_weights_grad.size() + hid_inter_grad.size(), 0, rec_weights_grad.size(), 1) =
				ravel(rec_weights_grad);
			gradient.block(hid_weights_grad.size() + hid_inter_grad.size() + rec_weights_grad.size(),
				0, rec_inter_grad.size(), 1) = rec_inter_grad;

			return { loss, gradient };
		}

	protected:
		int _hidden_units;
		HiddenActivation _hidden_activation;
		ReconstructionActivation _reconstruction_activation;
		Optimizer _optimizer;
		double _regularization;
		double _sparsity;
		double _sparsity_weight;

		MatrixXd _hidden_weights;
		VectorXd _hidden_intercepts;
		MatrixXd _reconstruction_weights;
		VectorXd _reconstruction_intercepts;
	};

	template <class HiddenActivation, class ReconstructionActivation, class Optimizer>
	auto create_sparse_autoencoder(int hidden_units, HiddenActivation&& hidden_activation,
	ReconstructionActivation&& reconstruction_activation, Optimizer&& optimizer,
	double regularization, double sparsity, double sparsity_weight) {
		return SparseAutoencoder<HiddenActivation, ReconstructionActivation, Optimizer>(
			hidden_units,
			forward<HiddenActivation>(hidden_activation),
			forward<ReconstructionActivation>(reconstruction_activation),
			forward<Optimizer>(optimizer),
			regularization, sparsity, sparsity_weight);
	}
}
}
}
#endif


#include "wave/optimization/ceres/odom_linear/point_to_line_interpolated_transform.hpp"
#include <Eigen/QR>

namespace wave {

SE3PointToLine::SE3PointToLine(const double *const p, const double *const pA, const double *const pB, const double *const scal, const Mat3 &CovZ, bool calculate_weight)
        : pt(p), ptA(pA), ptB(pB), scale(scal) {
    Eigen::Matrix<double, 3, 12> JP_T;
    Eigen::Matrix<double, 3, 3> Jres_P;

    JP_T << this->pt[0], 0, 0, this->pt[1], 0, 0, this->pt[2], 0, 0, 1, 0, 0, //
            0, this->pt[0], 0, 0, this->pt[1], 0, 0, this->pt[2], 0, 0, 1, 0, //
            0, 0, this->pt[0], 0, 0, this->pt[1], 0, 0, this->pt[2], 0, 0, 1;

    this->diff[0] = this->ptB[0] - this->ptA[0];
    this->diff[1] = this->ptB[1] - this->ptA[1];
    this->diff[2] = this->ptB[2] - this->ptA[2];
    this->bottom = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

    if (this->bottom < 1e-10) {
        // The points defining the line are too close to each other
        throw std::out_of_range("Points defining line are too close!");
    }

    Jres_P(0,0) = 1 - (diff[0] * diff[0] / bottom);
    Jres_P(0,1) = -(diff[0] * diff[1] / bottom);
    Jres_P(0,2) = -(diff[0] * diff[2] / bottom);
    Jres_P(1,0) = -(diff[1] * diff[0] / bottom);
    Jres_P(1,1) = 1 - (diff[1] * diff[1] / bottom);
    Jres_P(1,2) = -(diff[1] * diff[2] / bottom);
    Jres_P(2,0) = -(diff[2] * diff[0] / bottom);
    Jres_P(2,1) = -(diff[2] * diff[1] / bottom);
    Jres_P(2,2) = 1 -(diff[2] * diff[2] / bottom);

    Eigen::Vector3d unitdiff;
    double invlength = 1.0/sqrt(this->bottom);
    if(this->diff[2] > 0) {
        unitdiff[0] = this->diff[0] * invlength;
        unitdiff[1] = this->diff[1] * invlength;
        unitdiff[2] = this->diff[2] * invlength;
    } else {
        unitdiff[0] = -this->diff[0] * invlength;
        unitdiff[1] = -this->diff[1] * invlength;
        unitdiff[2] = -this->diff[2] * invlength;
    }

    Eigen::Vector3d unitz;
    unitz << 0, 0, 1;

    auto v = unitdiff.cross(unitz);
    auto s = v.norm();
    auto c = unitz.dot(unitdiff);
    Mat3 skew;
    Transformation<>::skewSymmetric3(v, skew);
    this->rotation = Eigen::Matrix3d::Identity() + skew + skew*skew*((1-c)/(s*s));

    this->Jres_T = this->rotation * Jres_P * JP_T;

    if(calculate_weight) {
        auto rotated = (this->rotation * Jres_P * CovZ * Jres_P.transpose() * this->rotation.transpose());
        this->weight_matrix = rotated.block<2,2>(0,0).inverse().sqrt();
    } else {
        this->weight_matrix.setIdentity();
    }
}

bool SE3PointToLine::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    Eigen::Map<const Mat34> Tmap(parameters[0], 3, 4);
    Transformation<Eigen::Map<const Mat34>> Tk(Tmap);

    Transformation<Eigen::Matrix<double, 3, 4>> interpolated;

    Eigen::Map<const Vec3> PT(pt, 3, 1);
    auto twist = Tk.logMap();
    interpolated.setFromExpMap(*(this->scale) * twist);
    Vec3 POINT = interpolated.transform(PT);
    double point[3];
    Eigen::Map<Vec3>(point, 3, 1) = POINT;

    double p_A[3] = {point[0] - this->ptA[0], point[1] - this->ptA[1], point[2] - this->ptA[2]};

    double scaling = ceres::DotProduct(p_A, diff);
    // point on line closest to point
    double p_Tl[3] = {this->ptA[0] + (scaling / bottom) * diff[0],
                      this->ptA[1] + (scaling / bottom) * diff[1],
                      this->ptA[2] + (scaling / bottom) * diff[2]};

    double residual_o[3];
    residual_o[0] = point[0] - p_Tl[0];
    residual_o[1] = point[1] - p_Tl[1];
    residual_o[2] = point[2] - p_Tl[2];
    Eigen::Map<Vec3> res(residual_o, 3, 1);
    Eigen::Vector2d reduced = (this->rotation * res).block<2,1>(0,0);
    reduced = this->weight_matrix * reduced;
    Eigen::Map<Eigen::Vector2d>(residuals, 2, 1) = reduced;

    if (jacobians != NULL) {
        // Have to apply the "lift" jacobian to the Interpolation Jacobian because
        // of Ceres local parameterization
        Transformation<>::Jinterpolated(twist, *(this->scale), this->J_int);
        interpolated.J_lift(this->J_lift);
        Tk.J_lift(this->J_lift_full);

        this->J_lift_full_pinv = (this->J_lift_full.transpose() * this->J_lift_full).inverse() * this->J_lift_full.transpose();

        this->Jr_T = this->Jres_T * this->J_lift * this->J_int * this->J_lift_full_pinv;
        this->Jr_T_reduced = this->weight_matrix * this->Jr_T.block<2,12>(0,0);

        Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>>(jacobians[0], 2, 12) = this->Jr_T_reduced;
    }

    return true;
}

}  // namespace wave


#include <iostream>
#include <vector>
#include <numeric>
#include <Eigen/Dense>
#include "mex.h"
#include "matrix.h"

#if $(use_sparse_template)
#include <Eigen/Sparse>
#endif
#if $(use_sturm_eigensolver)
#define MAX_DEG  $(num_basis)
#include "sturm.h"
#include "charpoly.h"
#endif
#if $(use_sturm_dani_eigensolver)
#define MAX_DEG  $(num_basis)
// #include "sturm.h"
#define DEG  $(num_basis)
#include "sturm_mart.h"
#include "charpoly.h"
#endif

using namespace Eigen;

#if $(use_reduced_eigenvector_solver)
void fast_eigenvector_solver(double * eigv, int neig, Eigen::Matrix<double,$(num_basis),$(num_basis)> &AM, MatrixXcd &sols);
#endif

void solver_$(solv_name)($(function_param_declaration))
{
    // Compute coefficients

$(code_compute_coefficients)

    // Setup elimination template
    static const int coeffs0_ind[] = { $(coeffs0_ind) };
    static const int coeffs1_ind[] = { $(coeffs1_ind) };

$(code_setup_template)

    // Setup action matrix
    Matrix<double,$(num_available), $(num_basis)> RR;
    RR << -C12.bottomRows($(num_reducible)), Matrix<double,$(num_basis),$(num_basis)>::Identity($(num_basis), $(num_basis));

    static const int AM_ind[] = { $(AM_ind) };
    Matrix<double, $(num_basis), $(num_basis)> AM;
    for (int i = 0; i < $(num_basis); i++) {
        AM.row(i) = RR.row(AM_ind[i]);
    }

    MatrixXcd sols($(num_vars), $(num_basis));
    sols.setZero();

    // Solve eigenvalue problem
#if $(use_standard_eigensolver)
    EigenSolver<Matrix<double, $(num_basis), $(num_basis)> > es(AM);
    ArrayXcd D = es.eigenvalues();
    ArrayXXcd V = es.eigenvectors();

    $(code_normalize_eigenvectors)
    $(code_extract_solutions)
#endif
#if $(use_eigsonly_eigensolver)

    EigenSolver<MatrixXd> es(AM, false);
    ArrayXcd D = es.eigenvalues();

    int nroots = 0;
    double eigv[$(num_basis)];
    for (int i = 0; i < $(num_basis); i++) {
        if (std::abs(D(i).imag()) < 1e-6)
            eigv[nroots++] = D(i).real();
    }

    fast_eigenvector_solver(eigv, nroots, AM, sols);
#endif
#if $(use_sturm_eigensolver)
    double p[1+$(num_basis)];
    Matrix<double, $(num_basis), $(num_basis)> AMp = AM;
    charpoly_$(charpoly_method)(AMp, p);
    double roots[$(num_basis)];
    int nroots;
    find_real_roots_sturm(p, $(num_basis), roots, &nroots, 8, 0);
    fast_eigenvector_solver(roots, nroots, AM, sols);
#endif

#if $(use_sturm_dani_eigensolver)

    double p[1 + $(num_basis)];
    Matrix<double, $(num_basis), $(num_basis)> T;
    charpoly_danilevsky_piv_T(AM, p, T);
    double roots[$(num_basis)];
    int nroots;
    // find_real_roots_sturm(p, $(num_basis), roots, &nroots, 8, 0);
    nroots = realRoots(p, roots);
    sols.resize($(num_vars), nroots);

    Eigen::MatrixXd V($(num_basis), nroots);
    Eigen::Map<Eigen::MatrixXd> D(roots, 1, nroots);
    V.bottomRows(1).setConstant(1);
    for (int j = $(num_basis) - 2; j >= 0; j--) {
        V.row(j) = V.row(j + 1).array() * D.array();
    }
    V = T * V;
    Eigen::RowVectorXd row = V.row(0);
    V.array().rowwise() /= row.array();

    D.transposeInPlace();
    $(code_extract_solutions)

#endif
    $(code_pack_outputs)

}
$(debug_comments)

#if $(use_reduced_eigenvector_solver)
    void fast_eigenvector_solver(double * eigv, int neig, Eigen::Matrix<double,$(num_basis),$(num_basis)> &AM, MatrixXcd &sols) {
    static const int ind[] = { $(ind_non_trivial) };
    // Truncated action matrix containing non-trivial rows
    Matrix<double, $(length_ind_non_trivial), $(num_basis)> AMs;
    double zi[$(max_power)];

    for (int i = 0; i < $(length_ind_non_trivial); i++)    {
        AMs.row(i) = AM.row(ind[i]);
    }
    for (int i = 0; i < neig; i++) {
        zi[0] = eigv[i];
        for (int j = 1; j < $(max_power); j++)
        {
            zi[j] = zi[j - 1] * eigv[i];
        }
        Matrix<double, $(AA_sz)> AA;
$(code_setup_reduced_eigenvalue_eq)

        Matrix<double, $(ind_unit), 1>  s = AA.leftCols($(ind_unit)).colPivHouseholderQr().solve(-AA.col($(ind_unit)));
$(code_extract_solutions)
    }
}
#endif

mxArray* convertToMatlabCell(std::vector<Eigen::MatrixXcd> const& sols) {
    mxArray* cell = mxCreateCellMatrix(1, sols.size());
    for (int i = 0; i < sols.size(); ++i) {
        Eigen::MatrixXcd sol = sols.at(i);
        mxArray* mat = mxCreateDoubleMatrix(sol.rows(), sol.cols(), mxCOMPLEX);
        memcpy(mxGetComplexDoubles(mat), sol.data(), sizeof(mxComplexDouble) * sol.size());
        // double* mat_r = mxGetPr(mat);
        // double* mat_i = mxGetPi(mat);
        // for (int r = 0; r < sol.rows(); ++r) {
        //     for (int c = 0; c < sol.cols(); ++c) {
        //         mat_r[c * sol.rows() + r] = sol(r, c).real();
        //         mat_i[c * sol.rows() + r] = sol(r, c).imag();
        //     }
        // }
        mxSetCell(cell, i, mat);
    }
    return cell;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    std::ios_base::sync_with_stdio(false);
    if (nrhs != $(input_size)) {
        mexErrMsgIdAndTxt("automatic_generator_cvpr:$(solv_name):nrhs", "One input required.");
    }
    if (nlhs != $(output_size)) {
        mexErrMsgIdAndTxt("automatic_generator_cvpr:$(solv_name):nlhs", "One output required.");
    }
    if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
        mexErrMsgIdAndTxt("automatic_generator_cvpr:$(solv_name):notDouble", "Input data must be type double.");
    }
    $(code_mex_function_body)
}


#include "PriorityQueue.h"
#include <vector>
#include <ctime>
#include <cstdlib>
#include <utility>
#include <iostream>
#include <string>
#include <sstream>
#include <cmath>
#include <algorithm>
#include <boost/bind.hpp>

double temperature(unsigned long t) {
	return 100.0*std::exp((-8.0*t*t) / (2600.0*2600.0)) + 0.1;//0.1 is an offset
	//	^
	//	|
	//make this large
}

int selectAction(PriorityQueue<int, double>& a_queue, unsigned long iterations) {

	typedef std::vector<std::pair<int, double> > VecPair ;

	//turn priority queue into a vector of pairs
	VecPair vec = a_queue.saveOrderedQueueAsVector();

    //sum for partition function
	double sum = 0.0;

	// calculate partition function by iterating over action-values
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
		sum += std::exp((iter->second) / temperature(iterations));
	}

	// compute Boltzmann factors for action-values and enqueue to vec
	for (VecPair::iterator iter = vec.begin(); iter < vec.end(); ++iter) {
		iter->second = std::exp(iter->second / temperature(iterations)) / sum;
	}

	// calculate cumulative probability distribution
	for (VecPair::iterator iter = vec.begin()++, end = vec.end(); iter < end; ++iter) {
        //second member of pair becomes addition of its current value
        //and that of the index before it
		iter->second += (iter-1)->second;
	}

	//generate RN between 0 and 1
	double rand_num = static_cast<double>(rand()) / RAND_MAX;

	// choose action based on random number relation to priorities within action queue
	for (VecPair::iterator iter = vec.begin(), end = vec.end(); iter < end; ++iter) {
		if (rand_num < iter->second)
			return iter->first;
	}

	return -1; //note that this line should never be reached
}
int main(){
    std::vector< std::pair<int, double> > vec;
    std::pair<int, double> pear;
    //size of vector
    int size = 10;
    std::vector< int > vec2(size, 0);

    //make priority queue full of zeroes
    //except for one optimal action
    for(int i = 0; i < size; i++)
    {
        pear = std::make_pair(i, 0.0);
        //optimal action
        if(i ==3){pear = std::make_pair(i, 100);}
        vec.push_back(pear);
    }

    unsigned long i = 0;
    int chosen_action;

    //coinstruct PQ from vector of pairs
    PriorityQueue<int, double> a_queue(vec, MAX);

    //choses action many times
	for(int j=0; j < 1000; j++){
	    //finds action
		chosen_action = selectAction(a_queue, i);
		//increases value of vec2 in bin corresponding
		//to chosen action
		vec2[chosen_action] ++;
		i++;
	}
	std::cout<< "action" << "\tNumber of times selected" << std::endl;
	//prints contents of vector
	int j = 0;
	for(std::vector< int >::iterator it = vec2.begin(); it < vec2.end(); it++)
	{
	    std::cout << j << "\t" << (*it) << std::endl;
	    j++;
	}

    return 0;
}


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

#include <boost/shared_ptr.hpp>
#include <boost/math/special_functions.hpp>
#include <vector>
#include <cmath>

namespace HDRCreation
{

using namespace std;

HDRCreator::HDRCreator(const GlobalArgs_t & globalArgs)
    : globalArgs(globalArgs)
{
    // TODO Auto-generated constructor stub
}

typedef float(*luminanceCorrection_t)(float);

float logLumiCorrection(float value) {
    double base = 10; // TODO:

    double baseFraction = log(base);
    double logValue = log1p(value);

    return logValue / baseFraction;
}

float powLumiCorrection(float value) {
    double power = 3.3; // TODO:

    double powValue = pow(value + 1, power);

    return powValue;
}

void correctLuminocity(luminanceCorrection_t lumiCorrection, pfs::FramePtr frame) {
    using namespace pfs;
    pfs::Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    for (auto itX = X->begin(), itY = Y->begin(), itZ = Z->begin();
            itX != X->end(); itX++, itY++, itZ++) {
        (*itX) = lumiCorrection(*itX);
        (*itY) = lumiCorrection(*itY);
        (*itZ) = lumiCorrection(*itZ);
    }
}

typedef float(*agrFun_t)(pfs::FramePtr);

float avgAgr(pfs::FramePtr frame) {
    using namespace pfs;
    pfs::Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    double sumX=0, sumY=0, sumZ=0;
    for (auto itX = X->begin(), itY = Y->begin(), itZ = Z->begin();
            itX != X->end(); itX++, itY++, itZ++) {
        sumX += *itX;
        sumY += *itY;
        sumZ += *itZ;
    }
    sumX /= frame->getWidth() * frame->getHeight();
    sumY /= frame->getWidth() * frame->getHeight();
    sumZ /= frame->getWidth() * frame->getHeight();

    return (sumX + sumY + sumZ) / 3; // dummy heuristic, use colorspace characteristic
}

float heuristicRelativeEV(agrFun_t aggregation, pfs::FramePtr frame)
{
    return aggregation(frame);
}

void getChannelsIterators(pfs::Channel *X, pfs::Channel *Y, pfs::Channel *Z,
        pfs::Channel::iterator & itX, pfs::Channel::iterator & itY, pfs::Channel::iterator & itZ)
{
    itX = X->begin();
    itY = Y->begin();
    itZ = Z->begin();
}

void getChannelsIterators(pfs::FramePtr frame,
        pfs::Channel::iterator & itX, pfs::Channel::iterator & itY, pfs::Channel::iterator & itZ)
{
    using pfs::Channel;
    Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    getChannelsIterators(X, Y, Z, itX, itY, itZ);
}

void getChannelsIterators(pfs::FramePtr frame,
        pfs::Channel::iterator & itX, pfs::Channel::iterator & itY, pfs::Channel::iterator & itZ,
        pfs::Channel::iterator & endX)
{
    using pfs::Channel;
    Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    getChannelsIterators(X, Y, Z, itX, itY, itZ);
    endX = X->end();
}

void createBeginIterators(vector<pfs::FramePtr> & frames,
        vector<pfs::Channel::iterator> & XChannelsIt,
        vector<pfs::Channel::iterator> & YChannelsIt,
        vector<pfs::Channel::iterator> & ZChannelsIt)
{
    XChannelsIt.resize(frames.size());
    YChannelsIt.resize(frames.size());
    ZChannelsIt.resize(frames.size());
    int expNo = 0;
    for (auto frameIt = frames.begin(); frameIt != frames.end(); ++frameIt, ++expNo)
    {
        pfs::Channel::iterator itX, itY, itZ;
        getChannelsIterators(*frameIt, itX, itY, itZ);
        XChannelsIt[expNo] = itX;
        YChannelsIt[expNo] = itY;
        ZChannelsIt[expNo] = itZ;
    }
}

/**
 * Assumptions:
 * 1 - all frames are aligned, with fixed size.
 * 2 - frames are exposure value ordered
 */
bool HDRCreator::create(kernel::GenericFramePtr output, std::vector<kernel::GenericFramePtr> & framesGeneric)
{
    /**
     * Assertions
     */
    debug_print(LVL_INFO, "Found %d frame(s) to process.\n", framesGeneric.size());
    if (output == 0) {
    	return false;
    }

    if (!output->isValid()) {
    	if (framesGeneric.front()->isValid())
    	{
    		if (!output->setEmptyFrameFrom(framesGeneric.front()->getRawFramePFS())) {
    		    return false;
    		}
    	} else {
    		return false;
    	}
    }
    assert(output->isValid());
    pfs::FramePtr outputFrame = output->getRawFramePFS();

    /**
     * Create pointers.
     * Map luminocity in each frame.
     */
    int noOfExposures = framesGeneric.size();
    size_t frameSize = outputFrame->getWidth() * outputFrame->getHeight();
    vector<pfs::FramePtr> frames;
    vector<double> framesRelativeEVComputed; // X translation
    vector<double> avgLuminocityRelativeFractionX, avgLuminocityRelativeFractionY,avgLuminocityRelativeFractionZ; // Y translation, than frame 1
    for (auto frameGenIt = framesGeneric.begin(); frameGenIt != framesGeneric.end(); frameGenIt++) {
        pfs::FramePtr frame = (*frameGenIt)->getRawFramePFS();
        frames.push_back(frame);
        correctLuminocity(&logLumiCorrection, frame);
        framesRelativeEVComputed.push_back(heuristicRelativeEV(&avgAgr, frame));

        avgLuminocityRelativeFractionX.push_back(0.0f);
        avgLuminocityRelativeFractionY.push_back(0.0f);
        avgLuminocityRelativeFractionZ.push_back(0.0f);
    }

    /**
     * Prepare vectors of channels. Channel data iterators.
     */
    pfs::Channel::iterator itX, itY, itZ, endX;

    /**
     *
     */
    vector<pfs::Channel::iterator> XChannelsIt, YChannelsIt, ZChannelsIt;
    createBeginIterators(frames, XChannelsIt, YChannelsIt, ZChannelsIt);
    float relativeX = **XChannelsIt.begin(), relativeY = **YChannelsIt.begin(), relativeZ = **ZChannelsIt.begin();
    int expNo = 1;
    for (auto XChannelsDataIt = ++XChannelsIt.begin(), YChannelsDataIt = ++YChannelsIt.begin(), ZChannelsDataIt = ++ZChannelsIt.begin();
            XChannelsDataIt != XChannelsIt.end();
            XChannelsDataIt++, YChannelsDataIt++, ZChannelsDataIt++, expNo++)
    {
        auto XValueIt = *XChannelsDataIt, YValueIt = *YChannelsDataIt, ZValueIt = *ZChannelsDataIt;
        for (size_t pixel = 0; pixel < frameSize;
                ++pixel, ++XValueIt, ++YValueIt, ++ZValueIt)
        {
            auto XValue = *XValueIt, YValue = *YValueIt, ZValue = *ZValueIt;
            avgLuminocityRelativeFractionX[expNo] += XValue - relativeX;
            avgLuminocityRelativeFractionY[expNo] += YValue - relativeY;
            avgLuminocityRelativeFractionZ[expNo] += ZValue - relativeZ;
        }
    }
    // Normalize
    for(auto avgLRFXIt = avgLuminocityRelativeFractionX.begin(), avgLRFYIt = avgLuminocityRelativeFractionY.begin(),
            avgLRFZIt = avgLuminocityRelativeFractionZ.begin();
            avgLRFXIt != avgLuminocityRelativeFractionX.end();
            avgLRFXIt++, avgLRFYIt++, avgLRFZIt++)
    {
        (*avgLRFXIt) /= frameSize;
        (*avgLRFYIt) /= frameSize;
        (*avgLRFZIt) /= frameSize;
        if (boost::math::isnan((*avgLRFXIt))) { debug_print(LVL_LOW, "*avgLRFXIt isn't a number = %f\n", *avgLRFXIt); }
        if (boost::math::isnan((*avgLRFYIt))) { debug_print(LVL_LOW, "*avgLRFYIt isn't a number = %f\n", *avgLRFYIt); }
        if (boost::math::isnan((*avgLRFZIt))) { debug_print(LVL_LOW, "*avgLRFZIt isn't a number = %f\n", *avgLRFZIt); }
    }

    /**
     * Translate vector.
     */
    createBeginIterators(frames, XChannelsIt, YChannelsIt, ZChannelsIt);
    expNo = 0;
    for (auto XChannelsDataIt = XChannelsIt.begin(), YChannelsDataIt = YChannelsIt.begin(), ZChannelsDataIt = ZChannelsIt.begin();
            XChannelsDataIt != XChannelsIt.end();
            XChannelsDataIt++, YChannelsDataIt++, ZChannelsDataIt++, expNo++)
    {
        auto & XValueIt = *XChannelsDataIt, & YValueIt = *YChannelsDataIt, & ZValueIt = *ZChannelsDataIt;
        auto expLumin = avgLuminocityRelativeFractionX[expNo];
        auto frameRelEV = framesRelativeEVComputed[expNo];
        auto pixelFraction = expLumin + frameRelEV;
        if (boost::math::isnan(pixelFraction)) { debug_print(LVL_LOW, "pixelFraction isn't a number = %f\n", pixelFraction); }
        for (size_t pixel = 0; pixel < frameSize;
                ++pixel, ++XValueIt, ++YValueIt, ++ZValueIt)
        {
            auto & XValue = *XValueIt, YValue = *YValueIt, ZValue = *ZValueIt;
            XValue += pixelFraction;
            YValue += pixelFraction;
            ZValue += pixelFraction;
        }
    }

    // Create HDR Image
    int it = 0;
    createBeginIterators(frames, XChannelsIt, YChannelsIt, ZChannelsIt);
    auto XChannelsDataIt = XChannelsIt.begin(), YChannelsDataIt = YChannelsIt.begin(), ZChannelsDataIt = ZChannelsIt.begin();
    float minXV = 1000, minYV = 1000, minZV = 1000;
    float maxXV = -1, maxYV = -1, maxZV = -1;
    pfs::Channel *X, *Y, *Z;
    outputFrame->createXYZChannels(X, Y, Z);
    endX = X->end();
    for(getChannelsIterators(X, Y, Z, itX, itY, itZ); itX != endX;
            ++itX, ++itY, ++itZ, it++) {
        auto & XOutputVal = *itX, & YOutputVal = *itY, & ZOutputVal = *itZ;
        XOutputVal = 0, YOutputVal = 0, ZOutputVal = 0;
        XChannelsDataIt = XChannelsIt.begin(), YChannelsDataIt = YChannelsIt.begin(), ZChannelsDataIt = ZChannelsIt.begin();
        for (int expNo = 0; expNo < noOfExposures; XChannelsDataIt++, YChannelsDataIt++, ZChannelsDataIt++, expNo++)
        {
            auto & XValueIt = *XChannelsDataIt, & YValueIt = *YChannelsDataIt, & ZValueIt = *ZChannelsDataIt;
            auto XValue = *XValueIt, YValue = *YValueIt, ZValue = *ZValueIt;

            XOutputVal += XValue;
            YOutputVal += YValue;
            ZOutputVal += ZValue;

            ++XValueIt;
            ++YValueIt;
            ++ZValueIt;
        }
        if (minXV > XOutputVal) minXV = XOutputVal;
        if (minYV > YOutputVal) minYV = YOutputVal;
        if (minZV > ZOutputVal) minZV = ZOutputVal;
        if (maxXV < XOutputVal) maxXV = XOutputVal;
        if (maxYV < YOutputVal) maxYV = YOutputVal;
        if (maxZV < ZOutputVal) maxZV = ZOutputVal;
        //debug_print(LVL_LOW, "X = %f, Y = %f, Z = %f,\n", XOutputVal, YOutputVal, ZOutputVal);
        if (boost::math::isnan(XOutputVal)) { debug_print(LVL_LOW, "XOutputVal isn't a number = %f\n", XOutputVal); }
        if (boost::math::isnan(YOutputVal)) { debug_print(LVL_LOW, "YOutputVal isn't a number = %f\n", YOutputVal); }
        if (boost::math::isnan(ZOutputVal)) { debug_print(LVL_LOW, "ZOutputVal isn't a number = %f\n", ZOutputVal); }
    }

    float rangeX = abs(maxXV - minXV), rangeY = abs(maxYV - minYV), rangeZ = abs(maxZV - minZV);
    debug_print(LVL_DEBUG, "rangeX=(%f - %f) rangeY=(%f - %f) rangeZ=(%f - %f)\n", minXV, maxXV, minYV, maxYV, minZV, maxZV);
    long nanX = 0, nanY = 0, nanZ = 0;

    // Normalize output (TONE MAP)
    for(getChannelsIterators(outputFrame, itX, itY, itZ, endX); itX != endX;
            ++itX, ++itY, ++itZ) {
        auto & XOutputVal = *itX, & YOutputVal = *itY, & ZOutputVal = *itZ;
        XOutputVal = (XOutputVal - minXV) / rangeX;
        YOutputVal = (YOutputVal - minYV) / rangeY;
        ZOutputVal = (ZOutputVal - minZV) / rangeZ;

        if (boost::math::isnan(XOutputVal)) {
            XOutputVal = 0;
            nanX++;
        }
        if (boost::math::isnan(YOutputVal)) {
            YOutputVal = 0;
            nanY++;
        }
        if (boost::math::isnan(ZOutputVal)) {
            ZOutputVal = 0;
            nanZ++;
        }
    }
    debug_print(LVL_DEBUG, "NanX = %f, NanY = %f, NanZ = %f\n", (float)nanX / (float)frameSize,(float)nanY / (float)frameSize,(float)nanZ / (float)frameSize);

    // Create output.
    return true; // XXX
}

} /* namespace HDRCreation */


#pragma once

#include <Eigen/Dense>
#include <glm/glm.hpp>

namespace draw::transform {

/**
 * Contains the decomposed parts of a transformation
 */
struct Decomposed {
  /**
   * The rotation angle
   */
  double theta;

  /**
   * The scaling in x and y directions
   */
  Eigen::Vector2f scaling;

  /**
   * The translation in x and y directions
   */
  Eigen::Vector2f translation;
};

/**
 * Represents a transformation that can be applied to a shape
 */
class Transformation {
 public:
  /**
   * Constructs a transformation that does nothing. The identity.
   */
  Transformation();

  /**
   * Constructs a scaling transformation
   */
  static Transformation scaling(double x, double y);

  /**
   * Constructs a translation transformation
   */
  static Transformation translation(double x, double y);

  /**
   * Constructs a rotation transformation
   */
  static Transformation rotation(double theta);

  /**
   * Adds translation to the transformation
   */
  Transformation &translate(double x, double y);
  Transformation &translate_x(double x);
  Transformation &translate_y(double y);

  /**
   * Adds scaling to the transformation
   */
  Transformation &scale(double x, double y);
  Transformation &scale_x(double x);
  Transformation &scale_y(double y);

  /**
   * Adds rotation to the transformation
   */
  Transformation &rotate(double theta);

  /**
   * Returns the decomposed transformation
   */
  [[nodiscard]] Decomposed decompose() const;

  /**
   * Returns the raw transformation matrix
   */
  [[nodiscard]] const Eigen::Matrix3d &get() const;

  /**
   * Returns the inverse of the transformation matrix
   */
  [[nodiscard]] Eigen::Matrix3d get_inverse() const;

  /**
   * Returns the A-submatrix as a glm type
   */
  [[nodiscard]] glm::mat2 get_A_glm() const;

  /**
   * Returns the b-subvector as a glm type
   */
  [[nodiscard]] glm::vec2 get_b_glm() const;

  /**
   * Composes two transformations
   */
  friend Transformation operator*(const Transformation &lhs, const Transformation &rhs);

 private:
  explicit Transformation(Eigen::Matrix3d A_prime);
  Eigen::Matrix3d A_prime;
};

}  // namespace draw::transform