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

//-----------------------------------------------------------------------------
/*!\file    cartesian_trajectory_segment.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2021/01/20
 *
 */
//-----------------------------------------------------------------------------

#include <cartesian_trajectory_interpolation/cartesian_trajectory_segment.h>
#include <algorithm>
#include <cmath>
#include "Eigen/src/Core/GlobalFunctions.h"
#include "Eigen/src/Core/Matrix.h"
#include "Eigen/src/Core/util/Constants.h"
#include "Eigen/src/Geometry/Quaternion.h"
#include <Eigen/Dense>

namespace cartesian_ros_control
{
  using Time = CartesianTrajectorySegment::Time;
  using SplineState = CartesianTrajectorySegment::SplineState;

  CartesianTrajectorySegment::CartesianTrajectorySegment(const Time&  start_time,
      const CartesianState& start_state,
      const Time&  end_time,
      const CartesianState& end_state)
    : QuinticSplineSegment(start_time, convert(start_state), end_time, convert(end_state))
  {
  };

  void CartesianTrajectorySegment::sample(const Time& time, CartesianState& state) const
  {
    // Sample from the underlying spline segment.
    SplineState s(7);
    QuinticSplineSegment::sample(time, s);

    if (time < this->startTime() || time > this->endTime())
    {
      state.p = Eigen::Vector3d(s.position[0], s.position[1], s.position[2]);
      state.q = Eigen::Quaterniond(s.position[3], s.position[4], s.position[5], s.position[6]).normalized();

      state.v = Eigen::Vector3d::Zero();
      state.w = Eigen::Vector3d::Zero();
      state.v_dot = Eigen::Vector3d::Zero();
      state.w_dot = Eigen::Vector3d::Zero();
    }
    else
    {
      state = convert(s);
    }
  }

  SplineState convert(const CartesianState& state)
  {
    SplineState spline_state;

    // Note: The pre-multiplication of velocity and acceleration terms with
    // `state.q.inverse()` transforms them into the body-local reference frame.
    // This is required for computing quaternion-based velocities and
    // accelerations below.

    // Convenience method
    auto fill = [](auto& vec, const auto& first, const auto& second)
    {
      vec.push_back(first.x());
      vec.push_back(first.y());
      vec.push_back(first.z());

      vec.push_back(second.w());
      vec.push_back(second.x());
      vec.push_back(second.y());
      vec.push_back(second.z());
    };

    // Spline positions
    fill(spline_state.position, state.p, state.q);

    // Spline velocities
    if(std::isnan(state.v.x()) || std::isnan(state.v.y()) || std::isnan(state.v.z())
          || std::isnan(state.w.x()) || std::isnan(state.w.y()) || std::isnan(state.w.z()))
    {
      return spline_state;  // with uninitialized velocity/acceleration data
    }
    Eigen::Quaterniond q_dot;
    Eigen::Vector3d tmp = state.q.inverse() * state.w;
    Eigen::Quaterniond omega(0, tmp.x(), tmp.y(), tmp.z());
    q_dot.coeffs() = 0.5 * (omega * state.q).coeffs();

    fill(spline_state.velocity, state.q.inverse() * state.v, q_dot);

    // Spline accelerations
    if(std::isnan(state.v_dot.x()) || std::isnan(state.v_dot.y()) || std::isnan(state.v_dot.z())
          || std::isnan(state.w_dot.x()) || std::isnan(state.w_dot.y()) || std::isnan(state.w_dot.z()))
    {
      return spline_state;  // with uninitialized acceleration data
    }
    Eigen::Quaterniond q_ddot;
    tmp = state.q.inverse() * state.w_dot;
    Eigen::Quaterniond omega_dot(0, tmp.x(), tmp.y(), tmp.z());
    q_ddot.coeffs() = 0.5 * (omega_dot * state.q).coeffs() + 0.5 * (omega * q_dot).coeffs();

    fill(spline_state.acceleration, state.q.inverse() * state.v_dot, q_ddot);

    return spline_state;
  };

  CartesianState convert(const SplineState& s)
  {
    CartesianState state;

    // Cartesian positions
    if (s.position.empty())
    {
      return state;  // with positions/velocities/accelerations zero initialized
    }
    state.p = Eigen::Vector3d(s.position[0], s.position[1], s.position[2]);
    state.q = Eigen::Quaterniond(s.position[3], s.position[4], s.position[5], s.position[6]).normalized();

    // Cartesian velocities
    if (s.velocity.empty())
    {
      return state;  // with velocities/accelerations zero initialized
    }
    Eigen::Quaterniond q_dot(s.velocity[3], s.velocity[4], s.velocity[5], s.velocity[6]);

    Eigen::Quaterniond omega;
    omega.coeffs() = 2.0 * (q_dot * state.q.inverse()).coeffs();

    state.v = Eigen::Vector3d(s.velocity[0], s.velocity[1], s.velocity[2]);
    state.w = Eigen::Vector3d(omega.x(), omega.y(), omega.z());

    // Cartesian accelerations
    if (s.acceleration.empty())
    {
      return state;  // with accelerations zero initialized
    }
    Eigen::Quaterniond q_ddot(s.acceleration[3], s.acceleration[4], s.acceleration[5], s.acceleration[6]);

    Eigen::Quaterniond omega_dot;
    omega_dot.coeffs() = 2.0 * (
        (q_ddot * state.q.inverse()).coeffs() -
        ((q_dot * state.q.inverse()) * (q_dot * state.q.inverse())).coeffs()
        );

    state.v_dot = Eigen::Vector3d(s.acceleration[0], s.acceleration[1], s.acceleration[2]);
    state.w_dot = Eigen::Vector3d(omega_dot.x(), omega_dot.y(), omega_dot.z());

    // Re-transform vel and acc to the correct reference frame.
    state.v = state.q * state.v;
    state.w = state.q * state.w;
    state.v_dot = state.q * state.v_dot;
    state.w_dot = state.q * state.w_dot;

    return state;
  }

  std::ostream& operator<<(std::ostream &out, const CartesianTrajectorySegment::SplineState& state)
  {
    out << "pos:\n";
    for (size_t i = 0; i < state.position.size(); ++i)
    {
      out << state.position[i] << '\n';
    }
    out << "vel:\n";
    for (size_t i = 0; i < state.velocity.size(); ++i)
    {
      out << state.velocity[i] << '\n';
    }
    out << "acc:\n";
    for (size_t i = 0; i < state.acceleration.size(); ++i)
    {
      out << state.acceleration[i] << '\n';
    }

    return out;
  }
}


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/Norm/PAlphaDPW08.h"
#include "latbuilder/WeightsDispatcher.h"
#include "latbuilder/Util.h"

#include <boost/math/special_functions/zeta.hpp>

namespace LatBuilder { namespace Norm {

namespace SumHelperPAlphaDPW08{

   template <typename WEIGHTS>
   class SumHelper {
   public:
      Real operator()(
            const WEIGHTS& weights,
            Real normType,
            Real z,
            Real lambda,
            Dimension dimension
            ) const
      { throw std::logic_error("DPW08 norm not implemented for this type of weights"); }
   };

#define DECLARE_PALPHA_DPW08_SUM(weight_type) \
      template <> \
      class SumHelper<weight_type> { \
      public: \
         Real operator()( \
               const weight_type& weights, \
               Real normType, \
               Real z, \
               Real lambda, \
               Dimension dimension \
               ) const; \
      }

   DECLARE_PALPHA_DPW08_SUM(LatticeTester::ProductWeights);
   DECLARE_PALPHA_DPW08_SUM(LatBuilder::CombinedWeights);

#undef DECLARE_PALPHA_DPW08_SUM

   //===========================================================================
   // combined weights
   //===========================================================================

   // Separating sumCombined() from
   // SumHelper<LatBuilder::CombinedWeights>::operator() is a workaround for
   // LLVM/clang++.
   Real sumCombined(
         const CombinedWeights& weights,
         Real normType,
         Real z,
         Real lambda,
         Dimension dimension
         )
   {
      Real val = 0.0;
      for (const auto& w : weights.list())
         val += WeightsDispatcher::dispatch<SumHelper>(*w, normType, z, lambda, dimension);
      return val;
   }

   Real SumHelper<LatBuilder::CombinedWeights>::operator()(
         const CombinedWeights& weights,
         Real normType,
         Real z,
         Real lambda,
         Dimension dimension
         ) const
   {
      return sumCombined(weights, normType, z, lambda, dimension);
   }

   //===========================================================================
   // product weights
   //===========================================================================

   Real SumHelper<LatticeTester::ProductWeights>::operator()(
         const LatticeTester::ProductWeights& weights,
         Real normType,
         Real z,
         Real lambda,
         Dimension dimension
         ) const
   {
      Real val = 1.0;
      for (Dimension coord = 0; coord < dimension; coord++) {
         Real weight = weights.getWeightForCoordinate(coord);
         if (weight)
            // weights are assumed to already be to the power normType; map
            // them to power 2
            val *= 1.0 + z * pow(weight, lambda * 2 / normType);
      }
      val -= 1.0;
      return val;
   }
}

PAlphaDPW08::PAlphaDPW08(unsigned int alpha, const LatticeTester::Weights& weights, Real normType):
   NormAlphaBase<PAlphaDPW08>(alpha, normType),
   m_weights(weights)
{}

template <LatticeType LR, EmbeddingType L>
Real PAlphaDPW08::value(
      Real lambda,
      const SizeParam<LR, L>& sizeParam,
      Dimension dimension,
      Real norm
      ) const
{
   norm = 1.0 / (norm * sizeParam.numPoints());
   const auto kappa = primeFactors(sizeParam.numPoints()).size();
   // 2^(kappa+1), where kappa is the number of distinct prime factors of n
   const auto k = intPow(2, kappa + 1);
   Real z = static_cast<Real>(k * boost::math::zeta<Real>(this->alpha() * lambda));
   Real val = WeightsDispatcher::dispatch<SumHelperPAlphaDPW08::SumHelper>(
         m_weights,
         this->normType(),
         z,
         lambda,
         dimension
         );

   return pow(norm * val, 1.0 / lambda);
}

template Real PAlphaDPW08::value<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>(Real, const SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>&, Dimension, Real) const;
template Real PAlphaDPW08::value<LatticeType::ORDINARY, EmbeddingType::MULTILEVEL>(Real, const SizeParam<LatticeType::ORDINARY, EmbeddingType::MULTILEVEL>&, Dimension, Real) const;

template Real PAlphaDPW08::value<LatticeType::POLYNOMIAL, EmbeddingType::UNILEVEL>(Real, const SizeParam<LatticeType::POLYNOMIAL, EmbeddingType::UNILEVEL>&, Dimension, Real) const;
template Real PAlphaDPW08::value<LatticeType::POLYNOMIAL, EmbeddingType::MULTILEVEL>(Real, const SizeParam<LatticeType::POLYNOMIAL, EmbeddingType::MULTILEVEL>&, Dimension, Real) const;

}}


#include <vector>

#include <armadillo>

#include <plink/plink_file.hpp>

class lars_variables
{
public:
    virtual arma::vec get_centered_phenotype( ) const = 0;
    virtual size_t get_num_samples( ) const = 0;
    virtual size_t get_num_variables( ) const = 0;
    virtual std::string get_name(size_t index) const = 0;
    virtual void calculate_cor(const arma::vec &residual, arma::vec &c) const = 0;
    virtual arma::vec eig_prod(const arma::vec &u) const = 0;
    virtual arma::mat get_active(const arma::uvec &active) const = 0;
};

class null_lars : public lars_variables
{
    public:
        null_lars(const arma::mat &X, const arma::vec &y)
            : m_X( X ),
              m_y( y )
        {
        }

        arma::vec get_centered_phenotype() const
        {
            return m_y - arma::mean( m_y );
        }

        size_t get_num_samples() const
        {
            return m_y.n_elem;
        }

        size_t get_num_variables() const
        {
            return m_X.n_cols;
        }

        std::string get_name(size_t index) const
        {
            return "";
        }

        void calculate_cor(const arma::vec &residual, arma::vec &c) const
        {
            for(int i = 0; i < m_X.n_cols; i++)
            {
                c[ i ] = arma::dot( m_X.col( i ), residual );
            }
        }

        arma::vec eig_prod(const arma::vec &u) const
        {
            return m_X.t( ) * u;
        }

        arma::mat get_active(const arma::uvec &active) const
        {
            return m_X.cols( active );
        }

    private:
        arma::mat m_X;
        arma::vec m_y;
};

/**
 * This class is responsible for both genetic and environmental data, along
 * with computing certain properties of this data that relates to the LARS
 * algorithm. Primarily, to avoid passing and operating on each data type
 * separately within the algorithm.
 */
class gene_environment : public lars_variables
{
public:
    /**
     * Constructor.
     *
     * @param genotypes A matrix of genotypes.
     * @param cov A matrix of covariates.
     * @param phenotype A vector of phenotypes.
     * @param cov_names Names of the covariates.
     * @param only_main Exclude gene-environment.
     */
    gene_environment(genotype_matrix_ptr genotypes, const arma::mat &cov, const arma::vec &phenotype, const std::vector<std::string> &cov_names, bool only_main);
    /**
     * Imputes the missing genotypes, covariates and phenotypes.
     */
    void impute_missing();

    /**
     * Centers a phenotype and returns it.
     *
     * @return Returns a centered phenotype.
     */
    arma::vec get_centered_phenotype() const;

    /**
     * Returns the number of samples.
     *
     * @return the number of samples.
     */
    size_t get_num_samples() const;

    /**
     * Returns the number of variables.
     *
     * @return the number of variables.
     */
    size_t get_num_variables() const;

    /**
     * Returns the name of the variable with the given index.
     *
     * @param index Index of the variable whoose name to return.
     *
     * @return The name of the variable with the given index.
     */
    std::string get_name(size_t index) const;

    /**
     * Returns the complete list of variable names.
     *
     * @return List of variables.
     */
    std::vector<std::string> get_names() const;

    /**
     * Returns the number of minor alleles for the given index, and
     * 0 if index is out of bounds.
     *
     * @param index Index to the genotype.
     *
     * @return The number of minor alleles for the given index, and
     * 0 if the index is out of bounds.
     */
    unsigned int compute_num_minor(size_t index) const;

    /**
     * Calculates the correlation between each variable and
     * the given vector of residuals. The results are stored
     * in the supplied vector c.
     *
     * @param residual A vector of residuals.
     * @param c A vector of at least the size of the number of variables,
     *          to store the computed correlations in.
     */
    void calculate_cor(const arma::vec &residual, arma::vec &c) const;

    /**
     * Computes the vector a from the LARS paper from the
     * vector u.
     *
     * @param u The u-vector from the LARS paper.
     *
     * @return The a-vector from the LARS paper.
     */
    arma::vec eig_prod(const arma::vec &u) const;

    /**
     * Returns a matrix of standardized variables according to the
     * given vector of indicides of active variables.
     *
     * @param active  Indicies of active variables.
     *
     * @return Matrix of standardized active variables.
     */
    arma::mat get_active(const arma::uvec &active) const;

    /**
     * Returns a matrix of raw variables according to the
     * given vector of indicides of active variables.
     *
     * @param active  Indicies of active variables.
     *
     * @return Matrix of standardized active variables.
     */
    arma::mat get_active_raw(const arma::uvec &active) const;

private:
    /**
     * Assigns missing genotypes with genotype mean.
     */
    void fill_missing_genotypes();

    /**
     * Assigns missing phenotypes with phenotype mean.
     */
    void fill_missing_phenotypes();

    /**
     * Assigns missing covariates with covariate mean.
     */
    void fill_missing_cov();

    /**
     * Computes the mean and standard deviation of each variable
     * and stores it in the m_mean and m_sd vectors. This is performed
     * in this way because it is too expensive to store the genotypes
     * as floats.
     */
    void compute_mean_sd();

private:
    /**
     * Matrix of genotypes.
     */
    genotype_matrix_ptr m_genotypes;

    /**
     * Matrix of covariates.
     */
    arma::mat m_cov;

    /**
     * Vector of phenotypes.
     */
    arma::vec m_phenotype;

    /**
     * Vector of mean values for each variable.
     */
    arma::vec m_mean;

    /**
     * Vector of standard deviation for each variable.
     */
    arma::vec m_sd;

    /**
     * Vector variable names.
     */
    std::vector<std::string> m_names;

    /**
     * Only consider main effects.
     */
    bool m_only_main;
};


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class Plane {
    PS::F64vec nvec;    // Normal vector of the projection surface
    PS::F64    cnst;    // Distance of the projection surface from the origin
    PS::F64vec base;    // The nearest point on the projection surface from the origin
    PS::F64vec axis[2]; // Newly defined vectors of the projection surface
    PS::F64vec pdsp;    // Vector of pallaral displacement on the projection surface
    PS::F64    wdth;    // Width of the projection surface

    void normalize() {
        PS::F64 vabs = sqrt(this->nvec * this->nvec);
        this->nvec = this->nvec / vabs;
        this->cnst = this->cnst / vabs;
    }

    void defineAxis() {
        if(this->nvec[0] == 0. && this->nvec[1] == 0. && this->nvec[2] == 1.) {
            this->axis[0] = PS::F64vec(1., 0., 0.);
            this->axis[1] = PS::F64vec(0., 1., 0.);
        } else if (this->nvec[0] == 0. && this->nvec[1] == 1. && this->nvec[2] == 0.) {
            this->axis[0] = PS::F64vec(1., 0., 0.);
            this->axis[1] = PS::F64vec(0., 0., 1.);
        } else if (this->nvec[0] == 1. && this->nvec[1] == 0. && this->nvec[2] == 0.) {
            this->axis[0] = PS::F64vec(0., 1., 0.);
            this->axis[1] = PS::F64vec(0., 0., 1.);
        } else {
            assert(NULL);
        }
    }

public:

    Plane() {
        this->nvec    = 0.;
        this->cnst    = 0.;
        this->base    = 0.;
        this->axis[0] = 0.;
        this->axis[1] = 0.;
        this->pdsp    = 0.;
        this->wdth    = 0.;
    }

    PS::F64 readData(FILE *fp) {
        fscanf(fp, "%lf%lf%lf%lf",
               &this->nvec[0], &this->nvec[1], &this->nvec[2], &this->cnst);
        fscanf(fp, "%lf%lf", &this->pdsp[0], &this->pdsp[1]);
        fscanf(fp, "%lf", &this->wdth);
        this->normalize();
        this->base = this->cnst * this->nvec;
        this->defineAxis();
    }

    PS::F64 writeData(FILE *fp) {
        if(PS::Comm::getRank() == 0) {
            fprintf(fp, "nvec: %+e %+e %+e\n",
                    this->nvec[0], this->nvec[1], this->nvec[2]);
            fprintf(fp, "cnst: %+e\n", this->cnst);
            fprintf(fp, "base: %+e %+e %+e\n",
                    this->base[0], this->base[1], this->base[2]);
            fprintf(fp, "axis[0]: %+e %+e %+e\n",
                    this->axis[0][0], this->axis[0][1], this->axis[0][2]);
            fprintf(fp, "axis[1]: %+e %+e %+e\n",
                    this->axis[1][0], this->axis[1][1], this->axis[1][2]);
            fprintf(fp, "pdsp: %+e %+e %+e\n",
                    this->pdsp[0], this->pdsp[1], this->pdsp[2]);
            fprintf(fp, "wdth: %+e\n", this->wdth);
        }
    }

    inline PS::F64 calcDistance(PS::F64vec pos) {
        return fabs(this->nvec * (pos - this->base));
    }

    inline PS::F64vec calcCoordinateOnPlane(PS::F64vec pos) {
        PS::F64 px = this->axis[0] * (pos - this->base) + pdsp[0];
        PS::F64 py = this->axis[1] * (pos - this->base) + pdsp[1];
        return PS::F64vec(px, py, 0.);
    }

    PS::F64 getWidth() {
        return this->wdth;
    }

};

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

};

template <class Tsph>
void calcDensityCenter(Tsph  & sph,
                       PS::F64vec & xdens_g,
                       PS::F64vec & vdens_g,
                       PS::F64vec & adens_g) {
    PS::F64    tdens = 0.;
    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        if(sph[i].istar != 0) {
            continue;
        }
        tdens += sph[i].dens;
        xdens += sph[i].dens * sph[i].pos;
        vdens += sph[i].dens * sph[i].vel;
        adens += sph[i].dens * sph[i].acc;
    }

    PS::F64    tdens_g = PS::Comm::getSum(tdens);
    xdens_g = PS::Comm::getSum(xdens);
    vdens_g = PS::Comm::getSum(vdens);
    adens_g = PS::Comm::getSum(adens);

    xdens_g = xdens_g / tdens_g;
    vdens_g = vdens_g / tdens_g;
    adens_g = adens_g / tdens_g;

    return;
}

template <class Tsph>
void projectOnPlane(char * ofile,
                    Plane & plane,
                    Tsph  & sph) {
    const  PS::S64 nmax = 256;
    static PS::S64 ptcl[nmax][nmax];
    static PS::F64 dens[nmax][nmax];
    static PS::F64 temp[nmax][nmax];
    static PS::F64 omgz[nmax][nmax];
    static PS::F64 entr[nmax][nmax];
    static PS::F64 istr[nmax][nmax];

    PS::F64 wdth  = plane.getWidth();
    PS::F64 dx    = wdth / (PS::F64)nmax;
    PS::F64 dxinv = 1. / dx;

    for(PS::S64 i = 0; i < nmax; i++) {
        for(PS::S64 j = 0; j < nmax; j++) {
            ptcl[i][j] = 0;
            dens[i][j] = 0.;
            temp[i][j] = 0.;
            omgz[i][j] = 0.;
            entr[i][j] = 0.;
            istr[i][j] = 0.;
        }
    }

    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;
    calcDensityCenter(sph, xdens, vdens, adens);

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos -= xdens;
        sph[i].vel -= vdens;
        sph[i].acc -= adens;
        PS::F64vec omg_i  = sph[i].pos ^ sph[i].vel;
        PS::F64    omgz_i = (1. / (sph[i].pos[0] * sph[i].pos[0] + sph[i].pos[1] * sph[i].pos[1])) * omg_i[2];
        if(plane.calcDistance(sph[i].pos) > sph[i].ksr) {
            continue;
        }
        PS::F64vec pvec  = plane.calcCoordinateOnPlane(sph[i].pos);
        PS::S64    ix    = (PS::S64)((pvec[0] + 0.5 * wdth) * dxinv);
        PS::S64    iy    = (PS::S64)((pvec[1] + 0.5 * wdth) * dxinv);
        if(ix < 0 || nmax <= ix) {
            continue;
        }
        if(iy < 0 || nmax <= iy) {
            continue;
        }
        ptcl[ix][iy] += 1;
        dens[ix][iy] += sph[i].dens;
        temp[ix][iy] += sph[i].temp;
        omgz[ix][iy] += omgz_i;
        entr[ix][iy] += sph[i].entr;
        istr[ix][iy] += (sph[i].istar + 1);
    }

    static PS::S64 ptcl_g[nmax][nmax];
    static PS::F64 dens_g[nmax][nmax];
    static PS::F64 temp_g[nmax][nmax];
    static PS::F64 omgz_g[nmax][nmax];
    static PS::F64 entr_g[nmax][nmax];
    static PS::F64 istr_g[nmax][nmax];

    PS::S64 ierr = 0;
    ierr = MPI_Allreduce(ptcl, ptcl_g, nmax*nmax, MPI_LONG_LONG, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(dens, dens_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(temp, temp_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(omgz, omgz_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(entr, entr_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
    ierr = MPI_Allreduce(istr, istr_g, nmax*nmax, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);

    for(PS::S64 i = 0; i < nmax; i++) {
        for(PS::S64 j = 0; j < nmax; j++) {
            PS::F64 pinv = ((ptcl_g[i][j] != 0) ? (1. / ptcl_g[i][j]) : 0.);
            dens_g[i][j] *= pinv;
            temp_g[i][j] *= pinv;
            omgz_g[i][j] *= pinv;
            entr_g[i][j] *= pinv;
            istr_g[i][j] *= pinv;
        }
    }

    if(PS::Comm::getRank() == 0) {
        FILE * fp = fopen(ofile, "w");
        for(PS::S64 i = 0; i < nmax; i++) {
            for(PS::S64 j = 0; j < nmax; j++) {
                PS::F64 px = dx * (PS::F64)i - 0.5 * wdth;
                PS::F64 pz = dx * (PS::F64)j - 0.5 * wdth;
                fprintf(fp, "%+e %+e %+e %+e %+e %+e %+e\n",
                        px, pz,
                        dens_g[i][j], temp_g[i][j], omgz_g[i][j],
                        entr_g[i][j], istr_g[i][j]);
            }
        }
        fclose(fp);
    }

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::S64 ibgn, iend, dsnp;
    Plane plane;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lld%lld%lld", &ibgn, &iend, &dsnp);
    plane.readData(fp);
    fclose(fp);
    plane.writeData(stdout);

    for(PS::S64 itime = ibgn; itime <= iend; itime += dsnp) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            if(PS::Comm::getRank() == 0) {
                fprintf(stderr, "Not found %s\n", tfile);
            }
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        sprintf(ofile, "%s/anim_%04d.dat", odir, itime);
        projectOnPlane(ofile, plane, sph);
    }

    PS::Finalize();

    return 0;
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../../example_func_xy_jacobian.h"
#include "../../constraints_jacobian.h"

#define RENDER_PSI 0
#define RENDER_OBJECTIVE_FUNC 1
#define RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT 2

int render_type = RENDER_PSI;

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -20.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

double x_result = 5;
double y_result = 5;

double psi_trg = 0.99;

std::vector<std::pair<double, double>> path_result;
double contraint_a = 1;
double contraint_x_trg = 0;
double contraint_y_trg = 0;
double weight_constraint = 1;
int constraint_type = 0;
double angle_heading = 0;
double update_scale = 1.0;
double w_longitudal_motion = 0.01;

void obs_eq_constraint(double &delta, double a, double x, double x_trg, int type)
{
	if(type == 0){
		observation_equation_constraint(delta, a, x, x_trg);
	}else{
		observation_equation_sq_constraint(delta, a, x, x_trg);
	}
}
void obs_eq_constraint_jacobian(Eigen::Matrix<double, 1, 1> &j, double a, double x, double x_trg, int type)
{
	if(type == 0){
		observation_equation_constraint_jacobian(j, a, x, x_trg);
	}else{
		observation_equation_sq_constraint_jacobian(j, a, x, x_trg);
	}
}

int main(int argc, char *argv[]){
	path_result.emplace_back(x_result, y_result);

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("simple_optimization_problem_func_xy_with_constraints");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glLineWidth(1);

	/*glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(100.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 100.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 100.0f);
	glEnd();*/

	float scale = 10;
	switch(render_type){
		case RENDER_PSI:{
			glColor3f(1,0,0);

			float step = 0.1;
			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double psi1;
					example_func_xy(psi1, x, y);

					double psi2;
					example_func_xy(psi2, x+step, y);

					double psi3;
					example_func_xy(psi3, x+step, y+step);

					double psi4;
					example_func_xy(psi4, x, y+step);

					glColor3f(psi1,1-psi1,0);
					glBegin(GL_LINE_STRIP);
						glVertex3f(x,y,psi1*scale);
						glVertex3f(x+step,y,psi2*scale);
						glVertex3f(x+step,y+step,psi3*scale);
						glVertex3f(x,y+step,psi4*scale);
						glVertex3f(x,y,psi1*scale);
					glEnd();
				}
			}

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double psi;
				example_func_xy(psi,x_result,y_result);
				glVertex3f(x_result, y_result, psi*scale);
			glEnd();
			glPointSize(1);

			//heading
			glLineWidth(3);
			Eigen::Affine2d m_rot = Eigen::Affine2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			glColor3f(0.1,0.5,0.6);
			glBegin(GL_LINES);
				Eigen::Vector2d v(1,0);
				Eigen::Vector2d vt = m_rot * v;
				example_func_xy(psi,x_result,y_result);
				glVertex3f(x_result, y_result, psi*scale);
				glVertex3f(x_result + vt.x(), y_result + vt.y(), psi*scale);
			glEnd();
			glLineWidth(1);

			glLineWidth(2);
			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(const auto &p:path_result){
				double psi;
				example_func_xy(psi,p.first,p.second);
				glVertex3f(p.first,p.second, psi*scale);
			}
			glEnd();
			glLineWidth(1);

			glColor3f(0,0,0);
			glBegin(GL_LINES);
			for(float x = -10; x <= 10; x+=1){
				glVertex3f(-10, x, psi_trg*scale);
				glVertex3f(10, x, psi_trg*scale);

				glVertex3f(x, -10, psi_trg*scale);
				glVertex3f(x, 10, psi_trg*scale);
			}
			glEnd();
		break;
		}
		case RENDER_OBJECTIVE_FUNC:{

			glColor3f(1,0,0);
			float step = 0.1;
			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double psi;
					example_func_xy(psi, x, y);
					glColor3f(psi,1-psi,0);

					glBegin(GL_LINE_STRIP);
					    double psi1;
						observation_equation_example_func_xy(psi1, x, y, psi_trg);

						double psi2;
						observation_equation_example_func_xy(psi2, x+step, y, psi_trg);

						double psi3;
						observation_equation_example_func_xy(psi3, x+step, y+step, psi_trg);

						double psi4;
						observation_equation_example_func_xy(psi4, x, y+step, psi_trg);

						glVertex3f(x,y,           psi1*psi1*scale);
						glVertex3f(x+step,y,      psi2*psi2*scale);
						glVertex3f(x+step,y+step, psi3*psi3*scale);
						glVertex3f(x,y+step,      psi4*psi4*scale);
						glVertex3f(x,y,           psi1*psi1*scale);
					glEnd();
				}
			}

			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double cx_delta1;
					obs_eq_constraint(cx_delta1, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta1;
					obs_eq_constraint(cy_delta1, contraint_a, y, contraint_y_trg, constraint_type);

					double cx_delta2;
					obs_eq_constraint(cx_delta2, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta2;
					obs_eq_constraint(cy_delta2, contraint_a, y, contraint_y_trg, constraint_type);

					double cx_delta3;
					obs_eq_constraint(cx_delta3, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta3;
					obs_eq_constraint(cy_delta3, contraint_a, y+step, contraint_y_trg, constraint_type);

					double cx_delta4;
					obs_eq_constraint(cx_delta4, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta4;
					obs_eq_constraint(cy_delta4, contraint_a, y+step, contraint_y_trg, constraint_type);

					double psi;
					example_func_xy(psi, x, y);
					glColor3f(psi,1-psi,0);
					glBegin(GL_LINE_STRIP);
						glVertex3f(x,y,           cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
						glVertex3f(x+step,y,      cx_delta2*cx_delta2 + cy_delta2 * cy_delta2);
						glVertex3f(x+step,y+step, cx_delta3*cx_delta3 + cy_delta3 * cy_delta3);
						glVertex3f(x,y+step,      cx_delta4*cx_delta4 + cy_delta4 * cy_delta4);
						glVertex3f(x,y,           cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
					glEnd();
				}
			}

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double psi;
				observation_equation_example_func_xy(psi, x_result, y_result, psi_trg);
				glVertex3f(x_result, y_result, psi*psi*scale);
			glEnd();
			glPointSize(1);

			//heading
			glLineWidth(3);
			Eigen::Affine2d m_rot = Eigen::Affine2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			glColor3f(0.1,0.5,0.6);
			glBegin(GL_LINES);
				Eigen::Vector2d v(1,0);
				Eigen::Vector2d vt = m_rot * v;
				observation_equation_example_func_xy(psi, x_result, y_result, psi_trg);
				glVertex3f(x_result, y_result, psi*psi*scale);
				glVertex3f(x_result + vt.x(), y_result + vt.y(), psi*psi*scale);
			glEnd();
			glLineWidth(1);

			glLineWidth(2);
			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(const auto &p:path_result){
				double psi;
				observation_equation_example_func_xy(psi, p.first, p.second, psi_trg);
				glVertex3f(p.first, p.second, psi*psi*scale);
			}
			glEnd();
			glLineWidth(1);
			break;
		}
		case RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT:{
			glColor3f(1,0,0);
			float step = 0.1;
			for(double x = -10; x <=10; x+=step){
				for(double y = -10; y <=10; y+=step){
					double delta1;
					observation_equation_example_func_xy(delta1, x, y, psi_trg);

					double cx_delta1;
					obs_eq_constraint(cx_delta1, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta1;
					obs_eq_constraint(cy_delta1, contraint_a, y, contraint_y_trg, constraint_type);

					double delta2;
					observation_equation_example_func_xy(delta2, x+step, y, psi_trg);

					double cx_delta2;
					obs_eq_constraint(cx_delta2, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta2;
					obs_eq_constraint(cy_delta2, contraint_a, y, contraint_y_trg, constraint_type);

					double delta3;
					observation_equation_example_func_xy(delta3, x+step, y+step, psi_trg);

					double cx_delta3;
					obs_eq_constraint(cx_delta3, contraint_a, x+step, contraint_x_trg, constraint_type);

					double cy_delta3;
					obs_eq_constraint(cy_delta3, contraint_a, y+step, contraint_y_trg, constraint_type);

					double delta4;
					observation_equation_example_func_xy(delta4, x, y+step, psi_trg);

					double cx_delta4;
					obs_eq_constraint(cx_delta4, contraint_a, x, contraint_x_trg, constraint_type);

					double cy_delta4;
					obs_eq_constraint(cy_delta4, contraint_a, y+step, contraint_y_trg, constraint_type);

					double psi;
					example_func_xy(psi, x, y);
					glColor3f(psi,1-psi,0);
					glBegin(GL_LINE_STRIP);
						glVertex3f(x,y,           delta1*delta1 + cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
						glVertex3f(x+step,y,      delta2*delta2 + cx_delta2*cx_delta2 + cy_delta2 * cy_delta2);
						glVertex3f(x+step,y+step, delta3*delta3 + cx_delta3*cx_delta3 + cy_delta3 * cy_delta3);
						glVertex3f(x,y+step,      delta4*delta4 + cx_delta4*cx_delta4 + cy_delta4 * cy_delta4);
						glVertex3f(x,y,           delta1*delta1 + cx_delta1*cx_delta1 + cy_delta1 * cy_delta1);
					glEnd();
				}
			}

			glPointSize(20);
			glColor3f(0,0,1);
			glBegin(GL_POINTS);
				double delta;
				observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

				double cx_delta;
				obs_eq_constraint(cx_delta, contraint_a, x_result, contraint_x_trg, constraint_type);

				double cy_delta;
				obs_eq_constraint(cy_delta, contraint_a, y_result, contraint_y_trg, constraint_type);

				glVertex3f(x_result, y_result, delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
			glEnd();
			glPointSize(1);

			//heading
			glLineWidth(3);
			Eigen::Affine2d m_rot = Eigen::Affine2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			glColor3f(0.1,0.5,0.6);
			glBegin(GL_LINES);
				observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);
				obs_eq_constraint(cx_delta, contraint_a, x_result, contraint_x_trg, constraint_type);
				obs_eq_constraint(cy_delta, contraint_a, y_result, contraint_y_trg, constraint_type);

				Eigen::Vector2d v(1,0);
				Eigen::Vector2d vt = m_rot * v;

				glVertex3f(x_result, y_result, delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
				glVertex3f(x_result+vt.x(), y_result+vt.y(), delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
			glEnd();
			glLineWidth(1);

			glLineWidth(2);
			glColor3f(0,0,1);
			glBegin(GL_LINE_STRIP);
			for(const auto &p:path_result){
				observation_equation_example_func_xy(delta, p.first, p.second, psi_trg);
				obs_eq_constraint(cx_delta, contraint_a, p.first, contraint_x_trg, constraint_type);
				obs_eq_constraint(cy_delta, contraint_a, p.second, contraint_y_trg, constraint_type);

				glVertex3f(p.first, p.second, delta*delta + cx_delta * cx_delta + cy_delta * cy_delta);
			}
			glEnd();
			glLineWidth(1);
			break;
		}
	}
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'o':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			ic = 0;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  0.000000001);
			tripletListB.emplace_back(ir, 0,  0);

			ic = 1;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  0.000000001);
			tripletListB.emplace_back(ir, 0,  0);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'c':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			ic = 0;
			ir = tripletListB.size();
			double c_delta;
			Eigen::Matrix<double, 1, 1> c_jacobian;
			obs_eq_constraint(c_delta, contraint_a, x_result, contraint_x_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, x_result, contraint_x_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			ic = 1;
			ir = tripletListB.size();
			obs_eq_constraint(c_delta, contraint_a, y_result, contraint_y_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, y_result, contraint_y_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'z':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			//anizotropic motion weights
			Eigen::Matrix2d m_rot = Eigen::Matrix2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			Eigen::Matrix2d m_w = Eigen::Matrix2d::Identity();
			m_w(0,0) = w_longitudal_motion;
			m_w(1,1) = 1;

			Eigen::Matrix2d m_c = (m_rot*m_w)*m_rot.transpose();

			ic = 0;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  m_c(0,0));
			tripletListP.emplace_back(ir, ir+1,  m_c(0,1));
			tripletListB.emplace_back(ir, 0,  0);

			ic = 1;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir-1,  m_c(1,0));
			tripletListP.emplace_back(ir, ir,  m_c(1,1));
			tripletListB.emplace_back(ir, 0,  0);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'x':{
			x_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;
			y_result += ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 0.0001;

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			double delta;
			observation_equation_example_func_xy(delta, x_result, y_result, psi_trg);

			Eigen::Matrix<double, 1, 2> jacobian;
			observation_equation_example_func_xy_jacobian(jacobian, x_result, y_result);

			if(jacobian(0,0) == 0.0 || jacobian(0,1) == 0.0 || delta == 0){
				return;
			}

			int ir = 0;
			int ic = 0;

			tripletListA.emplace_back(ir + 0, ic + 0, -jacobian(0,0));
			tripletListA.emplace_back(ir + 0, ic + 1, -jacobian(0,1));
			tripletListP.emplace_back(ir + 0, ir + 0,  1.0);
			tripletListB.emplace_back(ir + 0, 0,  delta);

			//anizotropic motion weights
			Eigen::Matrix2d m_rot = Eigen::Matrix2d::Identity();

			double angle_rad = angle_heading * M_PI/180.0;
			double sh = sin(angle_rad);
			double ch = cos(angle_rad);

			m_rot(0,0) = ch;
			m_rot(1,0) = sh;

			m_rot(0,1) = -sh;
			m_rot(1,1) =  ch;

			Eigen::Matrix2d m_w = Eigen::Matrix2d::Identity();
			m_w(0,0) = w_longitudal_motion;
			m_w(1,1) = 1;

			Eigen::Matrix2d m_c = (m_rot*m_w)*m_rot.transpose();

			ic = 0;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir,  m_c(0,0));
			tripletListP.emplace_back(ir, ir+1,  m_c(0,1));
			tripletListB.emplace_back(ir, 0,  0);

			ic = 1;
			ir = tripletListB.size();
			tripletListA.emplace_back(ir, ic,  1);
			tripletListP.emplace_back(ir, ir-1,  m_c(1,0));
			tripletListP.emplace_back(ir, ir,  m_c(1,1));
			tripletListB.emplace_back(ir, 0,  0);

			ic = 0;
			ir = tripletListB.size();
			double c_delta;
			Eigen::Matrix<double, 1, 1> c_jacobian;
			obs_eq_constraint(c_delta, contraint_a, x_result, contraint_x_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, x_result, contraint_x_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			ic = 1;
			ir = tripletListB.size();
			obs_eq_constraint(c_delta, contraint_a, y_result, contraint_y_trg, constraint_type);
			obs_eq_constraint_jacobian(c_jacobian, contraint_a, y_result, contraint_y_trg, constraint_type);
			tripletListA.emplace_back(ir, ic,  -c_jacobian(0,0));
			tripletListP.emplace_back(ir, ir,  weight_constraint);
			tripletListB.emplace_back(ir, 0,  c_delta);

			int number_of_columns = 2;
			Eigen::SparseMatrix<double> matA(tripletListB.size(), number_of_columns);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(number_of_columns, number_of_columns);
			Eigen::SparseMatrix<double> AtPB(number_of_columns, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == number_of_columns){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				x_result += h_x[0] * update_scale;
				y_result += h_x[1] * update_scale;
				path_result.emplace_back(x_result, y_result);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			x_result = ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 10;
			y_result = ((rand()%1000000)/1000000.0f - 0.5) * 2.0 * 10;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case '-':{
			contraint_a -= 0.01;
			if(contraint_a < 0)contraint_a= 0.01;
			break;
		}
		case '=':{
			contraint_a += 0.01;
			break;
		}
		case 'd':{
			contraint_x_trg -= 0.1;
			break;
		}
		case 'i':{
			contraint_x_trg += 0.1;
			break;
		}
		case 'f':{
			contraint_y_trg -= 0.1;
			break;
		}
		case 'g':{
			contraint_y_trg += 0.1;
			break;
		}
		case '1':{
			render_type = RENDER_PSI;
			break;
		}
		case '2':{
			render_type = RENDER_OBJECTIVE_FUNC;
			break;
		}
		case '3':{
			render_type = RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT;
			break;
		}
		case 'y':{
			psi_trg += 0.01;
			std::cout << "psi_trg: " << psi_trg << std::endl;
			break;
		}
		case 't':{
			psi_trg -= 0.01;
			std::cout << "psi_trg: " << psi_trg << std::endl;
			break;
		}
		case 'q':{
			x_result-=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case 'w':{
			x_result+=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case 'a':{
			y_result-=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case 's':{
			y_result+=0.1;
			path_result.clear();
			path_result.emplace_back(x_result, y_result);
			break;
		}
		case '4':{
			constraint_type = 0;
			break;
		}
		case '5':{
			constraint_type = 1;
			break;
		}
		case '6':{
			weight_constraint /=10;
			std::cout << "weight_constraint: " << weight_constraint << std::endl;
			break;
		}
		case '7':{
			weight_constraint *=10;
			std::cout << "weight_constraint: " << weight_constraint << std::endl;
			break;
		}
		case '8':{
			angle_heading += 1;
			break;
		}
		case '9':{
			angle_heading -= 1;
			break;
		}
		case '[':{
			update_scale -= 0.01;
			if(update_scale <= 0) update_scale = 0.01;
			std::cout << "update_scale: " << update_scale << std::endl;
			break;
		}
		case ']':{
			update_scale += 0.01;
			std::cout << "update_scale: " << update_scale << std::endl;
			break;
		}
		case 'k':{
			w_longitudal_motion *= 10;
			std::cout << "w_longitudal_motion: " << w_longitudal_motion << std::endl;
			break;
		}
		case 'l':{
			w_longitudal_motion /= 10;
			std::cout << "w_longitudal_motion: " << w_longitudal_motion << std::endl;
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "o: optimize" << std::endl;
	std::cout << "c: optimize with constaint" << std::endl;
	std::cout << "z: optimize with anizotropic motion constaint" << std::endl;
	std::cout << "x: optimize with constaint and anizotropic motion constaint" << std::endl;
	std::cout << "-: contraint_a -= 0.01" << std::endl;
	std::cout << "=: contraint_a += 0.01" << std::endl;
	std::cout << "d: contraint_x_trg -= 0.1" << std::endl;
	std::cout << "i: contraint_x_trg += 0.1" << std::endl;
	std::cout << "f: contraint_y_trg -= 0.1" << std::endl;
	std::cout << "g: contraint_y_trg += 0.1" << std::endl;
	std::cout << "1: RENDER_PSI" << std::endl;
	std::cout << "2: RENDER_OBJECTIVE_FUNC" << std::endl;
	std::cout << "3: RENDER_OBJECTIVE_FUNC_WITH_CONTRAINT" << std::endl;
	std::cout << "y: psi_trg += 0.01" << std::endl;
	std::cout << "t: psi_trg -= 0.01" << std::endl;
	std::cout << "q: x_result-=0.1" << std::endl;
	std::cout << "w: x_result+=0.1" << std::endl;
	std::cout << "a: y_result-=0.1" << std::endl;
	std::cout << "s: y_result+=0.1" << std::endl;
	std::cout << "4: constraint_type linear" << std::endl;
	std::cout << "5: constraint_type squared" << std::endl;
	std::cout << "6: weight_constraint /=10" << std::endl;
	std::cout << "7: weight_constraint *=10" << std::endl;
	std::cout << "8: angle_heading += 1" << std::endl;
	std::cout << "9: angle_heading -= 1" << std::endl;
	std::cout << "[: pdate_scale -= 0.01" << std::endl;
	std::cout << "]: pdate_scale += 0.01" << std::endl;
	std::cout << "k: w_longitudal_motion *= 10" << std::endl;
	std::cout << "l: w_longitudal_motion /= 10" << std::endl;
	std::cout << "r: random initial guess" << std::endl;
}


#include <iostream>
#include <iomanip>
#include "PSMoveController.h"
#include "ServerLog.h"
#include "PSEyeVideoCapture.h"
#include "opencv2/opencv.hpp"
#include <algorithm>
#include <Eigen/Dense>

#ifndef HAVE_M_PI
#ifndef M_PI
#define M_PI    3.14159265358979323846264338327950288   /* pi */
#endif
#endif

const int HUE_RANGE = 25;
const int SAT_RANGE = 50;
const int VAL_RANGE = 32;

typedef cv::Vec< unsigned char, 3 > cvBGR;
typedef cv::Vec< unsigned char, 3 > cvRGB;
typedef cv::Vec< unsigned char, 3 > cvHSV;

cvHSV
bgr2hsv(cvBGR bgr_in)
{
    static cv::Mat img_bgr(1, 1, CV_8UC3, bgr_in);
    static cv::Mat img_hsv(1, 1, CV_8UC3);
    cv::cvtColor(img_bgr, img_hsv, CV_BGR2HSV);
    return img_hsv.at<cvHSV>(0, 0);
}

void hsv_range(cvHSV hsv_in, cvHSV& min_out, cvHSV& max_out)
{
    min_out.val[0] = MAX(hsv_in.val[0] - HUE_RANGE, 0);  //0-180
    min_out.val[1] = MAX(hsv_in.val[1] - SAT_RANGE, 0);  //0-255
    min_out.val[2] = MAX(hsv_in.val[2] - VAL_RANGE, 0);  //0-255

    max_out.val[0] = MIN(hsv_in.val[0] + HUE_RANGE, 180);
    max_out.val[1] = MIN(hsv_in.val[1] + SAT_RANGE, 255);
    max_out.val[2] = MIN(hsv_in.val[2] + VAL_RANGE, 255);
}

void onMouse(int evt, int x, int y, int flags, void* param) {
    if(evt == CV_EVENT_LBUTTONDOWN) {
        cv::Point *ptPtr = (cv::Point*)param;
        *ptPtr = cv::Point(x, y);
    }
}

int main()
{
    log_init("info");

    if (hid_init() == -1)
    {
        std::cerr << "Failed to initialize hidapi" << std::endl;
        return -1;
    }

    PSMoveController psmove;
    PSEyeVideoCapture cap(0); // open the default camera
    cv::namedWindow("bgr video");
    cv::namedWindow("hsv video");
    cv::namedWindow("result");

    cv::Point clickPoint;
    cv::setMouseCallback("hsv video", onMouse, (void*)&clickPoint);

	std::cout << "Opening PSMoveController..." << std::endl;
	if (psmove.open() && cap.isOpened())
	{
        const PSMoveControllerState *psmstate= nullptr;

        cap.set(cv::CAP_PROP_EXPOSURE, 16);
        cap.set(cv::CAP_PROP_GAIN, 64);

        psmove.poll();
        psmstate= static_cast<const PSMoveControllerState *>(psmove.getState());

		unsigned char r = 255;
		unsigned char g = 0;
		unsigned char b = 255;
        psmove.setLED(r, g, b);
        psmove.setRumbleIntensity(0);

        cv::Mat bgrFrame;
        cv::Mat hsvFrame;
        cv::Mat intGsFrame;
        cv::Mat gsFrame;
        cvHSV led_min, led_max;
        cvHSV psmoveHSVColour = bgr2hsv(cvBGR(b, g, r));

        // HACK FOR TESTING
        psmoveHSVColour = cvHSV(140, 25, 255);

        hsv_range(psmoveHSVColour, led_min, led_max);

		while (psmove.getIsBluetooth() && psmstate->Move != CommonControllerState::Button_DOWN)
		{
            psmove.poll();
            psmstate= static_cast<const PSMoveControllerState *>(psmove.getState());

            psmove.setLED(r, g, b); // TODO: Rate-limit LED update

            cap >> bgrFrame; // get a new frame from camera

            if (!bgrFrame.empty())
            {
                cv::cvtColor(bgrFrame, hsvFrame, CV_BGR2HSV);       // Convert frame to HSV colour space
                cv::inRange(hsvFrame, led_min, led_max, gsFrame);  // Filter based on led HSV range

//                cv::GaussianBlur(intGsFrame, gsFrame, cv::Size(3, 3), 0);
//                cv::blur(intGsFrame, gsFrame, cv::Size(3, 3));  // Fastest
//                cv::bilateralFilter(intGsFrame, gsFrame, 3, 6.0, 1.5);  // Preserves edges

                imshow("result", gsFrame);

//                if (clickPoint.x > 0)
//                {
//                    cvHSV hsv_bulb = hsvFrame.at<cvHSV>(clickPoint);
//                    std::cout << clickPoint << ";" << hsv_bulb << std::endl;
//                }

                std::vector<std::vector<cv::Point> > contours;
                cv::findContours(gsFrame, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

                cv::Scalar contCol = cv::Scalar( 255, 0, 0 );
                for (int i=0; i<contours.size(); ++i) {
                    cv::drawContours(bgrFrame, contours, i, contCol);
                }

                std::vector<cv::Point> biggest_contour;
                if (contours.size() > 0)
                {
                    double contArea = 0;
                    double newArea = 0;
                    for (auto it = contours.begin(); it != contours.end(); ++it) {
                        newArea = cv::contourArea(*it);
                        if (newArea > contArea)
                        {
                            contArea = newArea;
                            biggest_contour = *it;
                        }
                    }
                }

                //TODO: If our contour is suddenly much smaller than last frame,
                // but is next to an almost-as-big contour, then maybe these
                // 2 contours should be joined.
                // (i.e. if a finger is blocking the middle of the bulb)

                if (biggest_contour.size() > 6)
                {
                    // Remove any points in contour on edge of camera/ROI
                    std::vector<cv::Point>::iterator it = biggest_contour.begin();
                    while (it != biggest_contour.end()) {
                        if (it->x == 320 || it->x == -320 || it->y == 240 || it->y == -240) {
                            it = biggest_contour.erase(it);
                        }
                        else {
                            ++it;
                        }
                    }

                    // Get the convex hull
                    std::vector<cv::Point> hull;
                    cv::convexHull(biggest_contour, hull);

//                    // If our hull has lots of points, select the N consecutive
//                    // points that are furthest, because furthest are least
//                    // likely to be occluded (assuming player is near middle of FOV).
//                    int N = 10;
//                    if (hull.size() > N)
//                    {
//                        std::deque<float> dists;
//                        float max_dist = 0;
//                        float sum_dists = 0;
//                        int best_i = 0;
//                        for (int i=0; i < (hull.size()-N); i++) {
//                            float new_dist = hull[i].x*hull[i].x + hull[i].y*hull[i].y;
//                            sum_dists += new_dist;
//                            dists.push_back(new_dist);
//                            if (i >= N)
//                            {
//                                sum_dists -= dists.front();
//                                dists.pop_front();
//                                if (sum_dists > max_dist)
//                                {
//                                    best_i = i;
//                                    max_dist = sum_dists;
//                                }
//                            }
//                        }
//                        hull = std::vector<cv::Point>(&hull[best_i], &hull[best_i+N]);
//                    }

                    {
                        std::vector< std::vector<cv::Point> > contours;

                        contours.push_back(hull);
                        cv::drawContours(bgrFrame, contours, 0, cv::Scalar(255, 0, 255));
                    }

                    // Fit ellipse to reduced hull

                    // Subtract midpoint from each point.
                    std::for_each(hull.begin(), hull.end(), [](cv::Point& p) { p.x -= 320; p.y = 240 - p.y;});

                    if (hull.size() > 5)
                    {
                        // http://autotrace.sourceforge.net/WSCG98.pdf
                        std::vector<float> conic_params(6);

                        Eigen::MatrixXf D1(hull.size(), 3);
                        Eigen::MatrixXf D2(hull.size(), 3);
                        for (int ix=0; ix < hull.size(); ++ix) {
                            D1.row(ix)[0] = hull[ix].x * hull[ix].x;
                            D1.row(ix)[1] = hull[ix].x * hull[ix].y;
                            D1.row(ix)[2] = hull[ix].y * hull[ix].y;
                            D2.row(ix)[0] = hull[ix].x;
                            D2.row(ix)[1] = hull[ix].y;
                            D2.row(ix)[2] = 1;
                        }
                        // std::cout << "\n\n\n\n" << hull << "\n\n\n\n" << std::endl;
                        Eigen::Matrix3f S1 = D1.transpose() * D1;
                        Eigen::Matrix3f S2 = D1.transpose() * D2;
                        Eigen::Matrix3f S3 = D2.transpose() * D2;
                        Eigen::Matrix3f T = -S3.colPivHouseholderQr().solve(S2.transpose());
//                        Eigen::Matrix3f T = -S3.inverse() * S2.transpose();
                        Eigen::Matrix3f M = S2*T + S1;
                        Eigen::Matrix3f Mout;
                        Mout.block<1, 3>(0, 0) = M.block<1, 3>(2, 0) / 2;
                        Mout.block<1, 3>(1, 0) = -M.block<1, 3>(1, 0);
                        Mout.block<1, 3>(2, 0) = M.block<1, 3>(0, 0) / 2;
                        Eigen::EigenSolver<Eigen::Matrix3f> eigsolv(Mout);
                        for (int row_ix=0; row_ix<3; ++row_ix) {
                            Eigen::Vector3f evec = eigsolv.eigenvectors().col(row_ix).real();
                            //cond = 4 * evec(1, :) .* evec(3, :) - evec(2, :).^2; % evaluate a'Ca
                            if ((4.0 * evec[0] * evec[2]) - (evec[1] * evec[1]) > 0)
                            {
                                conic_params[0] = evec[0];
                                conic_params[1] = evec[1];
                                conic_params[2] = evec[2];
                                Eigen::Vector3f Tevec = T*evec;
                                conic_params[3] = Tevec[0];
                                conic_params[4] = Tevec[1];
                                conic_params[5] = Tevec[2];
                            }
                        }

                        cv::RotatedRect cvFitEllipse= cv::fitEllipse(hull);
                        cv::ellipse(
                            bgrFrame,
                            cv::Point(cvFitEllipse.center.x + 320, 240 - cvFitEllipse.center.y),
                            cv::Size(cvFitEllipse.size.width/2, cvFitEllipse.size.height/2),
                            cvFitEllipse.angle,
                            0, 360,
                            cv::Scalar(0, 255, 255));

                        // Convert to parametric params
                        float A = conic_params[0];
                        float B = conic_params[1] / 2;
                        float C = conic_params[2];
                        float D = conic_params[3] / 2;
                        float F = conic_params[4] / 2;
                        float G = conic_params[5];
                        float BB = B*B;
                        float AC = A*C;
                        float FF = F*F;
                        float off_d = BB - AC;
                        float h = (C*D - B*F) / off_d;
                        float k = (A*F - B*D) / off_d;
                        float semi_n = A*FF + C*D*D + G*BB - 2*B*D*F - AC*G;
                        float dAC = A-C;
                        float dACsq = dAC*dAC;
                        float semi_d_2 = sqrtf(dACsq + 4*BB);
                        float semi_d_3 = -A - C;
                        float a_sqrd = (2 * semi_n) / (off_d * (semi_d_3 + semi_d_2));
                        float b_sqrd = (2 * semi_n) / (off_d * (semi_d_3 - semi_d_2));

                        if (a_sqrd > FLT_EPSILON && b_sqrd > FLT_EPSILON)
                        {
                            // b and tau are only needed for drawing an ellipse.
                            float a = sqrt(a_sqrd);
                            float b = sqrt(b_sqrd);
                            double tau = atan2(2 * B, dAC) / 2;  //acot((A-C)/(2*B))/2;
                            if (A > C)
                            {
                                tau += M_PI/2;
                            }
//                            if (tau < 0)
//                            {
//                                float oldb = b;
//                                b = a;
//                                a = oldb;
//                                tau += M_PI;
//                            }
                            cv::ellipse(bgrFrame, cv::Point(h+320, 240-k), cv::Size(a, b), tau, 0, 360, cv::Scalar(0, 255, 0));

                            //Get sphere coordinates from parametric
                            float F_PX = 554.2563;
                            float R = 2.25;
                            float L_px = sqrt(h*h + k*k);
                            float m = L_px / F_PX;
                            float fl = F_PX / L_px;
                            float j = (L_px + a) / F_PX;
                            float l = (j - m) / (1 + j*m);
                            float D_cm = R * sqrt(1 + l*l) / l;
                            float z = D_cm * fl / sqrt(1 + fl*fl);
                            float L_cm = z * m;
                            float x = L_cm * h / L_px;
                            float y = L_cm * k / L_px;

                            std::cout << "X: " << x << " Y: " << y << " Z: " << z << std::endl;
                        }
                    }

                }

                imshow("bgr video", bgrFrame);
                imshow("hsv video", hsvFrame);
            }

			int myw = 4;
			std::cout << '\r' <<
				"# " << std::setw(myw) << std::left << psmstate->RawSequence <<
				" A(1): " <<
				std::setw(myw) << std::right << psmstate->RawAccel[0][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[0][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[0][2] <<
				"; A(2): " <<
                std::setw(myw) << std::right << psmstate->RawAccel[1][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[1][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawAccel[1][2] <<
				"; G(1): " <<
                std::setw(myw) << std::right << psmstate->RawGyro[0][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[0][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[0][2] <<
				"; G(2): " <<
                std::setw(myw) << std::right << psmstate->RawGyro[1][0] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[1][1] << "," <<
                std::setw(myw) << std::right << psmstate->RawGyro[1][2] <<
				"; M: " <<
                std::setw(myw) << std::right << psmstate->RawMag[0] << "," <<
                std::setw(myw) << std::right << psmstate->RawMag[1] << "," <<
                std::setw(myw) << std::right << psmstate->RawMag[2] <<
				std::flush;

            cv::waitKey(16);
		}
        std::cout << std::endl;

        psmove.close();
	}

    // Tear-down hid api
    hid_exit();

	log_dispose();

    return 0;
}


#include "find_doors.h"

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/version.hpp>

#include <cstring>
#include <cstdlib>
#include <cmath>
#include <cstdint>

#include <algorithm>
#include <string>
#include <iostream>
#include <limits>
#include <set>
#include <map>
#include <list>
#include <utility>
#include <array>
#include <iterator>

using namespace cslibs_vectormaps;

namespace {

// some utility boost::geometry is lacking
point_t add(const point_t& p1, const point_t& p2)
{
    point_t p = p1;
    boost::geometry::add_point(p, p2);
    return p;
}

point_t subtract(const point_t& p1, const point_t& p2)
{
    point_t p = p1;
    boost::geometry::subtract_point(p, p2);
    return p;
}

point_t multiply(const point_t& p, double v)
{
    point_t p2 = p;
    boost::geometry::multiply_value(p2, v);
    return p2;
}

point_t divide(const point_t& p, double v)
{
    point_t p2 = p;
    boost::geometry::divide_value(p2, v);
    return p2;
}

bool equal(const point_t& a, const point_t& b)
{
    return a.x() == b.x() && a.y() == b.y();
}

}

FindDoors::FindDoors(const FindDoorsParameter& parameter) : parameter_(parameter)
{

}

std::vector<segment_t> FindDoors::round_segments(const std::vector<segment_t>& segments, double precision)
{
    double p = precision;
    if (p != 0.) {
        std::vector<segment_t> rounded_segments = segments;
        for (segment_t& toround : rounded_segments) {
            toround.first.x(std::round(toround.first.x() * p) / p);
            toround.first.y(std::round(toround.first.y() * p) / p);
            toround.second.x(std::round(toround.second.x() * p) / p);
            toround.second.y(std::round(toround.second.y() * p) / p);
        }
        return rounded_segments;
    } else {
        return segments;
    }
}

std::vector<segment_t> FindDoors::clean_segments(const std::vector<segment_t>& rounded_segments)
{
    // make a new vector that only contains successive segments, no overlapping
    // segments. we can only replace exact vertical or exact horizontal
    // overlappings at the moment.
    std::vector<segment_t> non_overlapping_segments;

    // sort original segments into std::vectors (vertical, horizontal, other)
    std::vector<segment_t> vertical, horizontal;
    for (const segment_t& segment : rounded_segments) {
        if (segment.first.x() == segment.second.x()) {
            segment_t s = {
                {segment.first.x(), std::min(segment.first.y(), segment.second.y())},
                {segment.second.x(), std::max(segment.first.y(), segment.second.y())}
            };
            vertical.push_back(s);
        } else if (segment.first.y() == segment.second.y()) {
            segment_t s = {
                {std::min(segment.first.x(), segment.second.x()), segment.first.y()},
                {std::max(segment.first.x(), segment.second.x()), segment.second.y()}
            };
            horizontal.push_back(s);
        } else {
            non_overlapping_segments.push_back(segment);
        }
    }
    // sort the std::vectors
    std::sort(vertical.begin(), vertical.end(), [](const segment_t& s1, const segment_t& s2) {
        return s1.first.x() < s2.first.x()
               || s1.first.x() == s2.first.x()
                  && (s1.first.y() < s2.first.y()/*
                      || s1.first.y() == s2.first.y()
                         && s1.second.y() < s2.second.y()*/);
    });
    std::sort(horizontal.begin(), horizontal.end(), [](const segment_t& s1, const segment_t& s2) {
        return s1.first.y() < s2.first.y()
               || s1.first.y() == s2.first.y()
                  && (s1.first.x() < s2.first.x()/*
                      || s1.first.x() == s2.first.x()
                         && s1.second.x() < s2.second.x()*/);
    });
    // finally, detect overlapping segments and replace those by single segments
    std::size_t overlaps = 0;
    for (std::size_t i = 0, n = vertical.size(), j; i < n; i = j) {
        double miny = vertical[i].first.y();
        double maxy = vertical[i].second.y();
        for (j = i + 1; j < n && vertical[i].first.x() == vertical[j].first.x(); j++) {
            if (vertical[j].first.y() < maxy) {
                if (vertical[j].second.y() > maxy)
                    maxy = vertical[j].second.y();
                overlaps++;
            } else {
                break;
            }
        }
        segment_t s = {{vertical[i].first.x(), miny}, {vertical[i].first.x(), maxy}};
        non_overlapping_segments.push_back(s);
    }
    for (std::size_t i = 0, n = horizontal.size(), j; i < n; i = j) {
        double minx = horizontal[i].first.x();
        double maxx = horizontal[i].second.x();
        for (j = i + 1; j < n && horizontal[i].first.y() == horizontal[j].first.y(); j++) {
            if (horizontal[j].first.x() < maxx) {
                if (horizontal[j].second.x() > maxx)
                    maxx = horizontal[j].second.x();
                overlaps++;
            } else {
                break;
            }
        }
        segment_t s = {{minx, horizontal[i].first.y()}, {maxx, horizontal[i].first.y()}};
        non_overlapping_segments.push_back(s);
    }
    std::cout << "Merged " << overlaps << " overlapping segments\n";

    // TODO: workaround for older Boost using bounding boxes
#if BOOST_VERSION < 105600
    std::cerr << "Boost version >= 1.56.0 is required for creating the segment graph! The results are probably unusable.\n";
    return non_overlapping_segments;
#else
    // replace intersecting line segments by multiple new segments that end in a common intersection point
    // use a spatial index to ease finding those intersecting line segments
    namespace bgi = boost::geometry::index;
    bgi::rtree<segment_t, bgi::rstar<16>> rtree(non_overlapping_segments);
    std::vector<segment_t> clean_segments;
    for (const segment_t& query_segment : non_overlapping_segments) {
        std::vector<point_t> intersections;
        for (auto qit = rtree.qbegin(bgi::intersects(query_segment)), end = rtree.qend(); qit != end; ++qit) {
            const segment_t& result_segment = *qit;
            boost::geometry::intersection(result_segment, query_segment, intersections);
        }
        std::set<point_t, point_compare> newpoints(intersections.begin(), intersections.end());
        for (auto pit1 = newpoints.begin(), pit2 = std::next(pit1), end = newpoints.end(); pit2 != end; ++pit1, ++pit2) {
            clean_segments.push_back({*pit1, *pit2});
        }
    }
    std::cout << "Created " << (clean_segments.size() - non_overlapping_segments.size()) << " new segments because of intersections\n";

    return clean_segments;
#endif
}

std::vector<FindDoors::door_t> FindDoors::find_doors(const std::vector<segment_t>& cleaned_segments)
{
    const FindDoorsParameter& params = parameter_;

    // find doors
    double door_depth_min = params.door_depth_min;
    double door_depth_max = params.door_depth_max;
    double door_width_min = params.door_width_min;
    double door_width_max = params.door_width_max;

    auto check_length = [](const point_t& v, double min, double max) {
        // checks the length of a vector
        double d = v.x() * v.x() + v.y() * v.y();
        return d >= min * min && d <= max * max;
    };
    auto check_angle = [&params](const point_t& v1, const point_t& v2) {
        double angle = std::atan2(v2.y(), v2.x()) - std::atan2(v1.y(), v1.x());
        if (angle < 0)
            angle += boost::math::double_constants::two_pi;
        return angle >= boost::math::double_constants::half_pi - params.door_angle_diff_max
            && angle <= boost::math::double_constants::half_pi + params.door_angle_diff_max;
    };
    /*auto check_door_side = [&door_depth_min, &door_depth_max, &check_length, &check_angle](const point_t& v1, const point_t& v2, const point_t& v3) {
        // Checks if v1, v2, v3 could form the side of a door like this:
        //     v1
        //  +<----+
        //        |v2
        //        v
        //  +<----+
        //     v3
        // This could be the left side of a door. The length of v2 is
        // checked. angle(v1, v2) and angle(v3, v2) are checked to be
        // approximately 90 deg. 270 deg does not count! This is so that
        // v2's direction is consistent and so that no duplicates occur
        // when considering edges in both directions.
        // The angles are not checked anymore right now.
        return check_length(v2, door_depth_min, door_depth_max);
            //&& check_angle(v1, v2) && check_angle(v3, v2);
    };
    auto check_node = [&check_door_side](const node_t& firstnode, const edge_t& e1, const edge_t& e2) {
        // checks if edge e2 from firstnode to secondnode could form
        // the side of a door as explained in check_door_side() above
        const node_t* secondnode = e2.target->node;
        if (secondnode->edges.size() == 2) {
            const edge_t& e3 = secondnode->edges[
                secondnode->edges[0].target->node == &firstnode ? 1 : 0
            ];
            auto edge_to_vector = [](const edge_t& e) {
                return subtract(e.target->point, e.start->point);
            };
            if (check_door_side(edge_to_vector(e1), edge_to_vector(e2), edge_to_vector(e3)))
                return true;
        }
        return false;
    };*/
    std::vector<segment_t> side_candidates;
    /*for (const node_t& firstnode : graph.nodes) {
        // find all nodes that connect exactly two edges. those can be corner points of doors.
        if (firstnode.edges.size() != 2)
            continue;
        // if one of the connected nodes also has exactly two edges, the
        // three edges might form one of the two walls on either side of
        // the door
        if (check_node(firstnode, firstnode.edges[1], firstnode.edges[0]))
            side_candidates.emplace_back(firstnode.edges[0].start->point, firstnode.edges[0].target->point);
        if (check_node(firstnode, firstnode.edges[0], firstnode.edges[1]))
            side_candidates.emplace_back(firstnode.edges[1].start->point, firstnode.edges[1].target->point);
    }*/
    // here, segments become door side candidates if their length fits.
    // just forget the madness above.
    for (const segment_t& segment : cleaned_segments) {
        if (check_length(subtract(segment.second, segment.first), door_depth_min, door_depth_max)) {
            side_candidates.push_back(segment);
            side_candidates.emplace_back(segment.second, segment.first);
        }
    }
    std::cout << "Found " << side_candidates.size() << " door side candidates\n";

    std::size_t ncandidates = side_candidates.size();
    std::vector<std::size_t> side_candidate_partners(ncandidates, static_cast<std::size_t>(-1));
    std::vector<double> side_candidate_distances(ncandidates, std::numeric_limits<double>::max());
    for (std::size_t i1 = 0; i1 < ncandidates; i1++) {
        // we search for the other side of the door frame by drawing
        // a ray r from the middle of the edge like this:
        //     v1
        //  +<----+          +---->+
        //        |  v       ^
        //      v2|----->----|--- r
        //        v          |w2
        //  +<----+          +---->+
        //     v3
        // If r cuts another possible door frame side nearly
        // perpendicularly and that intersection point is not too far
        // and not too close from v2, we found a door.
        const point_t& v21 = side_candidates[i1].first;
        const point_t& v22 = side_candidates[i1].second;
        // r = middle + t * v
        point_t middle = multiply(add(v21, v22), .5);
        point_t v2 = subtract(v22, v21);
        point_t v = {-v2.y(), v2.x()}; // 90 deg turn
        double v_length = std::sqrt(v.x() * v.x() + v.y() * v.y());

        // check all other candidates
        for (std::size_t i2 = 0; i2 < ncandidates; i2++) {
            if (i2 == i1)
                continue;
            // line intersection adapted from https://stackoverflow.com/a/565282/1007605
            auto cross = [](const point_t& v, const point_t& w) {
                return v.x() * w.y() - v.y() * w.x();
            };
            const point_t& w21 = side_candidates[i2].first;
            const point_t& w22 = side_candidates[i2].second;
            point_t w2 = subtract(w22, w21);
            double det = cross(v, w2);
            // check if vectors are neither parallel nor collinear
            if (det == 0.)
                continue;
            // check that second side is cut by ray
            double u = cross(subtract(w21, middle), v) / det;
            if (u < 0. || u > 1.)
                continue;
            // check distance to second side
            double d = cross(subtract(w21, middle), w2) * v_length / det;
            if (d < door_width_min || d > door_width_max)
                continue;
            // check that sides are approximately parallel
            if (!check_angle(v, w2))
                continue;
            // we found a match! if there are multiple matches we only keep the closest
            if (d < side_candidate_distances[i1]) {
                side_candidate_distances[i1] = d;
                side_candidate_partners[i1] = i2;
            }
            if (d < side_candidate_distances[i2]) {
                side_candidate_distances[i2] = d;
                side_candidate_partners[i2] = i1;
            }
        }
    }

    // match side candidate pairs
    std::vector<door_t> door_candidates;
    std::vector<double> door_candidate_distances;
    for (std::size_t i1 = 0; i1 < ncandidates; i1++) {
        std::size_t i2 = side_candidate_partners[i1];
        if (i2 != static_cast<std::size_t>(-1) && i1 < i2
        && side_candidate_partners[i2] == i1) {
            const point_t& v21 = side_candidates[i1].first;
            const point_t& v22 = side_candidates[i1].second;
            const point_t& w21 = side_candidates[i2].first;
            const point_t& w22 = side_candidates[i2].second;
            door_t door_candidate = {segment_t{v21, v22}, segment_t{w21, w22}};
            door_candidates.push_back(door_candidate);
            door_candidate_distances.push_back(side_candidate_distances[i1]);
        }
    }
    std::cout << "Found " << door_candidates.size() << " door candidates\n";

#if BOOST_VERSION < 105600
    std::cerr << "Boost version >= 1.56.0 is required for checking space between door frames, so you might get more false positives\n";
    return door_candidates;
#else
    // check if area between door frame sides is unobstructed
    std::vector<door_t> doors;
    namespace bgi = boost::geometry::index;
    bgi::rtree<segment_t, bgi::rstar<16>> rtree(cleaned_segments);
    for (std::size_t i = 0; i < door_candidates.size(); i++) {
        const door_t& door_candidate = door_candidates[i];
        double d1 = boost::geometry::distance(door_candidate[0].first, door_candidate[0].second);
        double d2 = boost::geometry::distance(door_candidate[1].first, door_candidate[1].second);

        // aligned at the middle of the longer door frame side, create a
        // rectangle between the door sides. it looks like this:
        //      door_width
        //      +---------+
        //
        //        +-----+             +
        //  ----+ |     | +----       |
        // frame| |rect | |frame      | door_width * unobstructed_area_depth
        //  ----+ |     | +----       |
        //        +-----+             +
        //
        //        +-----+
        // door_width * unobstructed_area_width
        const segment_t& longer_side = door_candidate[d1 > d2 ? 0 : 1];
        double longer_side_length = d1 > d2 ? d1 : d2;
        const point_t& v21 = longer_side.first;
        const point_t& v22 = longer_side.second;
        // r = middle + t * v
        point_t middle = multiply(add(v21, v22), .5);
        point_t v2 = subtract(v22, v21);
        point_t v = {-v2.y(), v2.x()}; // 90 deg turn
        double factor = door_candidate_distances[i] / longer_side_length;
        // t2 and t are as long as the door is wide, t2 points in direction of
        // the first door side, t points perpendicularly to the other door side
        point_t t2 = multiply(v2, factor);
        point_t t = multiply(v, factor);
        double rectangle_length = params.unobstructed_area_depth;
        double width_padding = .5 - params.unobstructed_area_width * .5;
        point_t p1 = add(add(middle, multiply(t,      width_padding)), multiply(t2, -.5 * rectangle_length));
        point_t p2 = add(add(middle, multiply(t, 1. - width_padding)), multiply(t2, -.5 * rectangle_length));
        point_t p3 = add(add(middle, multiply(t, 1. - width_padding)), multiply(t2,  .5 * rectangle_length));
        point_t p4 = add(add(middle, multiply(t,      width_padding)), multiply(t2,  .5 * rectangle_length));

        // if this rectangle is not obstructed, we found an actual door
        ring_t unobstructed_area;
        unobstructed_area.insert(unobstructed_area.end(), {p1, p2, p3, p4, p1});
        std::vector<segment_t> result;
        bgi::query(rtree, bgi::intersects(unobstructed_area), std::back_inserter(result));
        if (result.size() == 0) {
            door_t d = {
                segment_t{door_candidate[0].second, door_candidate[1].first},
                segment_t{door_candidate[1].second, door_candidate[0].first}
            };
            doors.push_back(d);
        }
    }

    // near the door frames, the door might still intersect the map. prune door sides appropriately
    for (door_t& door : doors) {
        for (segment_t& side : door) {
            std::vector<segment_t> intersections;
            bgi::query(rtree, bgi::intersects(side), std::back_inserter(intersections));
            for (const segment_t& intersection : intersections) {
                // work around buggy boost::geometry::intersection rarely giving incorrect result
                if (equal(intersection.first, side.first) || equal(intersection.first, side.second)
                || equal(intersection.second, side.first) || equal(intersection.second, side.second))
                    continue;
                std::vector<point_t> intersection_points;
                boost::geometry::intersection(side, intersection, intersection_points);
                for (const point_t& intersection_point : intersection_points) {
                    segment_t newside1 = {side.first, intersection_point};
                    segment_t newside2 = {intersection_point, side.second};
                    side = boost::geometry::length(newside1) > boost::geometry::length(newside2)
                         ? newside1 : newside2;
                }
            }
        }
    }
    std::cout << "Found " << doors.size() << " doors!\n";
    return doors;
#endif
}