//******************************************************************************
//                            INTEL CONFIDENTIAL
//  Copyright(C) 2008-2010 Intel Corporation. All Rights Reserved.
//  The source code contained  or  described herein and all documents related to
//  the source code ("Material") are owned by Intel Corporation or its suppliers
//  or licensors.  Title to the  Material remains with  Intel Corporation or its
//  suppliers and licensors. The Material contains trade secrets and proprietary
//  and  confidential  information of  Intel or its suppliers and licensors. The
//  Material  is  protected  by  worldwide  copyright  and trade secret laws and
//  treaty  provisions. No part of the Material may be used, copied, reproduced,
//  modified, published, uploaded, posted, transmitted, distributed or disclosed
//  in any way without Intel's prior express written permission.
//  No license  under any  patent, copyright, trade secret or other intellectual
//  property right is granted to or conferred upon you by disclosure or delivery
//  of the Materials,  either expressly, by implication, inducement, estoppel or
//  otherwise.  Any  license  under  such  intellectual property  rights must be
//  express and approved by Intel in writing.
//
//******************************************************************************
// Content:
//     Intel(R) Math Kernel Library (MKL) overloaded Boost/uBLAS prod()
//******************************************************************************

#ifndef _MKL_BOOST_UBLAS_MATRIX_PROD_
#define _MKL_BOOST_UBLAS_MATRIX_PROD_

#ifdef NDEBUG

#include <boost/version.hpp>
#if defined (BOOST_VERSION) && \
	  ((BOOST_VERSION == 103401) \
	|| (BOOST_VERSION == 103500) \
	|| (BOOST_VERSION == 103600) \
	|| (BOOST_VERSION == 103700) \
	|| (BOOST_VERSION == 103800) \
	|| (BOOST_VERSION == 103900) \
	|| (BOOST_VERSION == 104000) \
	|| (BOOST_VERSION == 104100))

#include <boost/numeric/ublas/matrix.hpp>

#include "mkl_boost_ublas_gemm.hpp"

namespace boost { namespace numeric { namespace ublas {

    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, m2 )
    prod(const matrix<T,F,A> &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), m2 )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), m2 )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), m2 )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, trans(m2) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), trans(m2) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), trans(m2) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), trans(m2) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, trans(conj(m2)) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), trans(conj(m2)) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), trans(conj(m2)) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), trans(conj(m2)) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, conj(trans(m2)) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), conj(trans(m2)) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), conj(trans(m2)) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), conj(trans(m2)) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }

}}}
#endif  // BOOST_VERSION
#endif  // NDEBUG
#endif  // _MKL_BOOST_UBLAS_MATRIX_PROD_


#pragma once
#include <autoppl/util/var_expr_traits.hpp>
#include <autoppl/variable.hpp>
#include <autoppl/math/math.hpp>
#include <iostream>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <autoppl/program_analysis/UncertainIntervals/IntervalAnalysis.hpp>
#include <autoppl/global_data.hpp>

namespace ppl {
namespace expr {

#if __cplusplus <= 201703L
template <class BinaryOp, class LHSVarExprType, class RHSVarExprType>
#else
template <class BinaryOp, util::var_expr LHSVarExprType, util::var_expr RHSVarExprType>
#endif
struct BinaryOpNode :
    util::VarExpr<BinaryOpNode<BinaryOp, LHSVarExprType, RHSVarExprType>>
{
#if __cplusplus <= 201703L
	static_assert(util::assert_is_var_expr_v<LHSVarExprType>);
	static_assert(util::assert_is_var_expr_v<RHSVarExprType>);
#endif

	using value_t = std::common_type_t<
		typename util::var_expr_traits<LHSVarExprType>::value_t,
		typename util::var_expr_traits<RHSVarExprType>::value_t
			>;

	BinaryOpNode(const LHSVarExprType& lhs, const RHSVarExprType& rhs)
		: lhs_{lhs}, rhs_{rhs}
	{ assert(lhs.size() == rhs.size() || lhs.size() == 1 || rhs.size() == 1); }

    value_t get_value(size_t i = 0) const {
        auto lhs_value = lhs_.get_value(i);
        auto rhs_value = rhs_.get_value(i);
        return BinaryOp::evaluate(lhs_value, rhs_value);
    }

    size_t size() const { return std::max(lhs_.size(), rhs_.size()); }

    /**
     * Returns ad expression of the binary operation.
     */
    template <class VecRefType, class VecADVarType>
    auto get_ad(const VecRefType& keys,
                const VecADVarType& vars,
                size_t idx = 0) const
    {
        return BinaryOp::evaluate(lhs_.get_ad(keys, vars, idx),
                                  rhs_.get_ad(keys, vars, idx));
    }

    std::set<boost::uuids::uuid> getDeps(){
	std::set<boost::uuids::uuid> used_vars;
	std::set<boost::uuids::uuid> lhs_used_vars = lhs_.getDeps(); //runtime polymorphism: whatever class mean_ is should have it's own getDeps() function
	used_vars.insert(lhs_used_vars.begin(),lhs_used_vars.end());
	std::set<boost::uuids::uuid> rhs_used_vars = rhs_.getDeps();
	used_vars.insert(rhs_used_vars.begin(),rhs_used_vars.end());
	return used_vars;
    }

   DeterministicInterval getInterval(){
        return BinaryOp::getInterval(lhs_, rhs_);
   }

private:
	LHSVarExprType lhs_;
	RHSVarExprType rhs_;

};

struct AddOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x + y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return add(x.getInterval(),y.getInterval());
	}

};

struct SubOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x - y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return sub(x.getInterval(),y.getInterval());
	}

};

struct MultOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x * y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return mult(x.getInterval(),y.getInterval());
	}

};

struct DivOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return x / y;
	}

};

struct MaxOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x >= y) ? x : y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return max(x.getInterval(),y.getInterval());
	}

};

struct MinOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x <= y) ? x : y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return min(x.getInterval(),y.getInterval());
	}

};

struct GTEOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x >= y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0.,1.);
	}

};

struct LTEOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x <= y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0.,1.);
	}

};

struct GTOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x > y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0.,1.);
	}

};

struct LTOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x < y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0,1);
	}

};

struct EQOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x == y) ? LHSValueType(1.) : LHSValueType(0.);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return DeterministicInterval(0,1);
	}

};

struct SqrtOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return ppl::math::sqrt(x*y);
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return sqrt_prod(x.getInterval(),y.getInterval());
	}

};

struct BooleanSwitchOp {
	//left value is a boolean switch
	template <class BooleanSwitchType, class RHSValueType>
	static auto evaluate(BooleanSwitchType x, RHSValueType y)
	{
		return x*y;
	}

	template <class BooleanSwitchType, class RHSValueType>
	static auto getInterval(BooleanSwitchType x, RHSValueType y)
	{
		return y.getInterval();
	}

};

struct MixtureMaxOp {

	template <class LHSValueType, class RHSValueType>
	static auto evaluate(LHSValueType x, RHSValueType y)
	{
		return (x >= y) ? x : y;
	}

	template <class LHSValueType, class RHSValueType>
	static auto getInterval(LHSValueType x, RHSValueType y)
	{
		return union_(x.getInterval(),y.getInterval());
	}

};

} // namespace expr
} // namespace ppl


// Copyright (c) 2018-2019 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>
#include <frc/controller/StateSpaceLoop.h>

#include "Constants.hpp"
#include "control/ElevatorCoeffs.hpp"
#include "control/TrapezoidalMotionProfile.hpp"

class ElevatorController {
public:
    // State tolerances in meters and meters/sec respectively.
    static constexpr double kPositionTolerance = 0.05;
    static constexpr double kVelocityTolerance = 2.0;

    ElevatorController();

    ElevatorController(const ElevatorController&) = delete;
    ElevatorController& operator=(const ElevatorController&) = delete;

    void Enable();
    void Disable();

    void SetGoal(double goal);

    /**
     * Sets the references.
     *
     * @param position Position of the carriage in meters.
     * @param velocity Velocity of the carriage in meters per second.
     */
    void SetReferences(units::meter_t position,
                       units::meters_per_second_t velocity);

    bool AtReferences() const;

    /**
     * Sets the current encoder measurement.
     *
     * @param measuredPosition Position of the carriage in meters.
     */
    void SetMeasuredPosition(double measuredPosition);

    /**
     * Returns the control loop calculated voltage.
     */
    double ControllerVoltage() const;

    /**
     * Returns the estimated position.
     */
    double EstimatedPosition() const;

    /**
     * Returns the estimated velocity.
     */
    double EstimatedVelocity() const;

    /**
     * Returns the error between the position reference and the position
     * estimate.
     */
    double PositionError() const;

    /**
     * Returns the error between the velocity reference and the velocity
     * estimate.
     */
    double VelocityError() const;

    /**
     * Returns the current reference set by the profile
     */
    double PositionReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update(void);

    /**
     * Resets any internal state.
     */
    void Reset(void);

private:
    // The current sensor measurement.
    Eigen::Matrix<double, 1, 1> m_Y;
    double m_estimatedPosition = 0.0;
    TrapezoidalMotionProfile::State m_goal;

    TrapezoidalMotionProfile::Constraints constraints{kElevatorMaxV,
                                                      kElevatorMaxA};
    TrapezoidalMotionProfile m_positionProfile{constraints, {0_m, 0_mps}};

    TrapezoidalMotionProfile::State m_profiledReference;

    // The control loop.
    frc::StateSpaceLoop<2, 1, 1> m_loop{MakeElevatorLoop()};

    bool m_atReferences = false;
};


#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

TEST(ProbDistributionsLkjCov, testIdentity) {
  boost::random::mt19937 rng;
  unsigned int K = 4;
  Eigen::MatrixXd Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  double mu = 0;
  Eigen::VectorXd muV = Eigen::VectorXd::Zero(K);
  double sd = 1;
  Eigen::VectorXd sdV = Eigen::VectorXd::Ones(K);
  double eta = stan::math::uniform_rng(0.5, 1.5, rng);
  double f = stan::math::do_lkj_constant(eta, K)
             + K * stan::math::lognormal_lpdf(1, 0, 1);
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, mu, sd, eta));
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, muV, sdV, eta));
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K)
      + K * stan::math::lognormal_lpdf(1, 0, 1);
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, mu, sd, eta));
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, muV, sdV, eta));
}

TEST(ProbDistributionsLkjCov, testHalf) {
  boost::random::mt19937 rng;
  unsigned int K = 4;
  Eigen::MatrixXd Sigma(K, K);
  Sigma.setConstant(0.5);
  Sigma.diagonal().setOnes();
  double mu = 0;
  Eigen::VectorXd muV = Eigen::VectorXd::Zero(K);
  double sd = 1;
  Eigen::VectorXd sdV = Eigen::VectorXd::Ones(K);
  double eta = stan::math::uniform_rng(0.5, 1.5, rng);
  double f = stan::math::do_lkj_constant(eta, K)
             + K * stan::math::lognormal_lpdf(1, 0, 1)
             + (eta - 1.0) * log(0.3125);
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, mu, sd, eta));
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, muV, sdV, eta));
  eta = 1.0;
  f = stan::math::do_lkj_constant(eta, K)
      + K * stan::math::lognormal_lpdf(1, 0, 1);
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, mu, sd, eta));
  EXPECT_FLOAT_EQ(f, stan::math::lkj_cov_lpdf(Sigma, muV, sdV, eta));
}

TEST(ProbDistributionsLkjCov, ErrorChecks) {
  boost::random::mt19937 rng;
  unsigned int K = 4;
  Eigen::MatrixXd Sigma(K, K);
  Sigma.setZero();
  Sigma.diagonal().setOnes();
  double mu = 0;
  Eigen::VectorXd muV = Eigen::VectorXd::Zero(K);
  double sd = 1;
  Eigen::VectorXd sdV = Eigen::VectorXd::Ones(K);
  double eta = stan::math::uniform_rng(0.5, 1.5, rng);
  EXPECT_NO_THROW(stan::math::lkj_cov_lpdf(Sigma, mu, sd, eta));

  // Error checks for non vectorized version.

  EXPECT_NO_THROW(stan::math::lkj_cov_lpdf(Sigma, -1, sd, eta));
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, stan::math::NOT_A_NUMBER, sd, eta),
      std::domain_error);
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, stan::math::INFTY, sd, eta),
               std::domain_error);
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, stan::math::NEGATIVE_INFTY, sd, eta),
      std::domain_error);

  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, mu, -1, eta), std::domain_error);
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, mu, stan::math::NOT_A_NUMBER, eta),
      std::domain_error);
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, mu, stan::math::INFTY, eta),
               std::domain_error);
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, mu, stan::math::NEGATIVE_INFTY, eta),
      std::domain_error);

  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, mu, sd, -1), std::domain_error);
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, muV, sd, stan::math::NOT_A_NUMBER),
      std::domain_error);
  EXPECT_NO_THROW(stan::math::lkj_cov_lpdf(Sigma, mu, sd, stan::math::INFTY));
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, mu, sd, stan::math::NEGATIVE_INFTY),
      std::domain_error);

  // Vectorized.

  Eigen::VectorXd muV1 = -muV;
  EXPECT_NO_THROW(stan::math::lkj_cov_lpdf(Sigma, muV1, sdV, eta));
  muV1 = stan::math::NOT_A_NUMBER * muV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV1, sdV, eta),
               std::domain_error);
  muV1 = stan::math::INFTY * muV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV1, sdV, eta),
               std::domain_error);
  muV1 = stan::math::NEGATIVE_INFTY * muV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV1, sdV, eta),
               std::domain_error);

  Eigen::VectorXd sdV1 = -sdV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV, sdV1, eta),
               std::domain_error);
  sdV1 = stan::math::NOT_A_NUMBER * sdV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV, sdV1, eta),
               std::domain_error);
  sdV1 = stan::math::INFTY * sdV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV, sdV1, eta),
               std::domain_error);
  sdV1 = stan::math::NEGATIVE_INFTY * sdV;
  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV, sdV1, eta),
               std::domain_error);

  EXPECT_THROW(stan::math::lkj_cov_lpdf(Sigma, muV, sdV, -1),
               std::domain_error);
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, muV, sdV, stan::math::NOT_A_NUMBER),
      std::domain_error);
  EXPECT_NO_THROW(stan::math::lkj_cov_lpdf(Sigma, muV, sdV, stan::math::INFTY));
  EXPECT_THROW(
      stan::math::lkj_cov_lpdf(Sigma, muV, sdV, stan::math::NEGATIVE_INFTY),
      std::domain_error);
}


// g++ cdf.cpp  -lboost_unit_test_framework

#include <cmath>
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/p_square_cumul_dist.hpp>

#include <vector>
#include <utility>

using namespace boost;
using namespace boost::accumulators;
//using namespace unit_test;

typedef iterator_range<std::vector<std::pair<double, double> >::iterator > histogram_type;

std::vector<std::pair<double, int> mapdata(histogram_type& h)
{
  const double mapToBlackPct = 0.2;
  const int otherSteps = 15;
  const double stepSizePct = (1-mapToBlackPct) / otherSteps;
  std::cout << "stepSizePct = " << stepSizePct << std::endl;

  std::vector<std::pair<double, int> mapping;

  for (std::size_t i = 0; i < h.size(); ++i)
    {
      int step = -1;
      if(h[i].second < mapToBlackPct)
	step = 0;
      else
	step = std::min(otherSteps, ceil((h[i].second - mapToBlackPct) / stepSizePct));
      std::cout << "[" << h[i].first << "," << h[i].second << "]"  << " ==> " << step << std::endl;

      // NOTE: Using -1 here so that the searches are easier later
      mapping.push_back(std::make_pair(h[i].first, step-1));
    }

  return mapping;
}

int main(void)
{

  // tolerance in %
  double epsilon = 3;

  typedef accumulator_set<double, stats<tag::p_square_cumulative_distribution> > accumulator_t;

  accumulator_t acc(tag::p_square_cumulative_distribution::num_cells = 100);

  // two random number generators
  boost::lagged_fibonacci607 rng;
  boost::normal_distribution<> mean_sigma(0,1);
  boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);

  for (std::size_t i=0; i<100000; ++i)
    {
      acc(normal());
    }

  histogram_type histogram = p_square_cumulative_distribution(acc);
  //histogram_type histogram;

  for (std::size_t i = 0; i < histogram.size(); ++i)
    {
      std::cout << "[" << histogram[i].first << "," << histogram[i].second << "]" << std::endl;

      /*
      // problem with small results: epsilon is relative (in percent), not absolute!
      if ( histogram[i].second > 0.001 )
        BOOST_CHECK_CLOSE( 0.5 * (1.0 + erf( histogram[i].first / sqrt(2.0) )), histogram[i].second, epsilon );
      */
    }

  std::cout << "*************************" << std::endl;
  std::vector<std::pair<double, int> mapping (mapdata(histogram));

  std::vector<double> vals;
  vals.push_back(-0.75);
  vals.push_back(-0.25);
  vals.push_back(0);
  vals.push_back(0.5);
  vals.push_back(2);
  //vals.push_back(4.2);

  for(std::vector<double>::const_iterator iter; iter!=vals.end(); ++iter)
    {

    }

  return 0;
}


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef RK_HPP
#define RK_HPP

// Runge-Kutta

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
rk(F f, ub::vector<T>& init, T start, T end) {

	ub::vector<T> x, dx1, dx2, dx3, dx4, x1, x2, x3;

	T t = start;
	T dt = end - start;

	x = init;
	dx1 = f(x, t) * dt;
	x1 = x + dx1 / 2.;
	dx2 = f(x1, t + dt/2.) * dt;
	x2 = x + dx2 / 2.;
	dx3 = f(x2, t + dt/2.) * dt;
	x3 = x + dx3;
	dx4 = f(x3, t + dt) * dt;

	init = x + dx1 / 6. + dx2 / 3. + dx3 / 3. + dx4 / 6.;
}

} // namespace kv

#endif // RK_HPP


#define BOOST_TEST_MODULE SplineTest
/*
 * ########################################################################
 * The contents of this file is free and unencumbered software released into the
 * public domain. For more information, please refer to <http://unlicense.org/>
 * ########################################################################
 */

#include <okruz/bspline/interpolation/interpolation.h>
#include <okruz/bspline/support/Support.h>

#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#ifdef OKRUZ_BSPLINE_INTERPOLATION_USE_EIGEN
template <typename T, size_t order>
void testInterpolationEigen(T tol) {
  using Spline = okruz::bspline::Spline<T, order>;
  using Support = okruz::bspline::support::Support<T>;
  using Grid = okruz::bspline::support::Grid<T>;
  using Construction = okruz::bspline::support::Construction;

  const Grid grid(std::vector<T>{-3.0l, -2.5l, -1.5l, -1.0l, 0.0l, 0.5l, 1.5l,
                                 2.5l, 3.5l, 4.0l, 5.0l});
  const Support x(grid, Construction::WHOLE_GRID);
  const std::vector<T> y{-3.0l, -2.5l, -1.5l, -1.0l, 0.0l, -0.5l,
                         -1.5l, -2.5l, -3.5l, -4.0l, 3.0l};
  Spline s =
      okruz::bspline::interpolation::interpolate_using_eigen<T, order>(x, y);
  for (size_t i = 0; i < x.size(); i++) {
    BOOST_CHECK_SMALL(s(x[i]) - y[i], tol);
  }
}

BOOST_AUTO_TEST_CASE(TestInterpolationEigen) {
  testInterpolationEigen<double, 1>(2.0e-14);
  testInterpolationEigen<double, 2>(2.0e-14);
  testInterpolationEigen<double, 3>(2.0e-14);
  testInterpolationEigen<double, 4>(2.0e-14);

  if constexpr (sizeof(long double) != sizeof(double)) {
    testInterpolationEigen<long double, 1>(1.0e-17l);
    testInterpolationEigen<long double, 2>(1.0e-17l);
    testInterpolationEigen<long double, 3>(1.0e-17l);
    testInterpolationEigen<long double, 4>(1.0e-17l);
  }
}
#endif

#ifdef OKRUZ_BSPLINE_INTERPOLATION_USE_ARMADILLO
template <size_t order>
void testInterpolationArmadillo(double tol) {
  using Spline = okruz::bspline::Spline<double, order>;
  using Support = okruz::bspline::support::Support<double>;
  using Grid = okruz::bspline::support::Grid<double>;
  using Construction = okruz::bspline::support::Construction;

  const Grid grid(std::vector<double>{-3.0l, -2.5l, -1.5l, -1.0l, 0.0l, 0.5l,
                                      1.5l, 2.5l, 3.5l, 4.0l, 5.0l});
  const Support x(grid, Construction::WHOLE_GRID);
  const std::vector<double> y{-3.0l, -2.5l, -1.5l, -1.0l, 0.0l, -0.5l,
                              -1.5l, -2.5l, -3.5l, -4.0l, 3.0l};
  Spline s =
      okruz::bspline::interpolation::interpolate_using_armadillo<order>(x, y);
  for (size_t i = 0; i < x.size(); i++) {
    BOOST_CHECK_SMALL(s(x[i]) - y[i], tol);
  }
}

BOOST_AUTO_TEST_CASE(TestInterpolationArmadillo) {
  testInterpolationArmadillo<1>(2.0e-14);
  testInterpolationArmadillo<2>(2.0e-14);
  testInterpolationArmadillo<3>(2.0e-14);
  testInterpolationArmadillo<4>(2.0e-14);
}
#endif


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      140219    E. Brandon        File copied from Newton-Raphson unit test.
 *      150417    D. Dirkx          Made modifications for templated root finding.
 *
 *    References
 *
 *    Notes
 *
 */

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/RootFinders/secantRootFinder.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction1.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction2.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction3.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithLargeRootDifference.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithZeroRoot.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( testsuite_rootfinders )

using namespace tudat;
using namespace root_finders;
using namespace root_finders::termination_conditions;

//! Check if Secant method converges on test function #1 (TestFunction1).
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunction1 )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunction1 > testFunction = boost::make_shared< TestFunction1 >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            boost::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    testFunction->getTrueRootAccuracy( ) ), _1, _2, _3, _4, _5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Secant method converges on test function #2 (TestFunction2).
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunction2 )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunction2 > testFunction = boost::make_shared< TestFunction2 >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            boost::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    testFunction->getTrueRootAccuracy( ) ), _1, _2, _3, _4, _5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Secant method converges on test function #3 (TestFunction3).
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunction3 )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunction3 > testFunction = boost::make_shared< TestFunction3 >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            boost::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    testFunction->getTrueRootAccuracy( ) ), _1, _2, _3, _4, _5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Secant method converges on function with large root difference
//! (testFunctionWithLargeRootDifference).
// Not the best test case. Inheritance from old code.
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunctionWithLargeRootDifference )
{
    // Declare tolerance.
    const double tolerance = 1.0e-10;

    // Declare expected roots.
    const double expectedRootLowCase = 1.00000000793634;
    const double expectedRootHighCase = 7937.3386333591;

    // Create objects containing the test functions. Values were obtained during a limit case
    // gravity assist calculation (while evaluating Cassini-1 trajectory).
    boost::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionLowCase =
            boost::make_shared< TestFunctionWithLargeRootDifference >
            ( 0, -3.24859999867635e18, -3248600.0, 1.5707963267949 );
    boost::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionHighCase =
            boost::make_shared< TestFunctionWithLargeRootDifference >
            ( 0, -3248600.0, -3.24859999867635e18, 1.5707963267949 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction
            = boost::bind( &RootRelativeToleranceTerminationCondition< >::checkTerminationCondition,
                           boost::make_shared< RootRelativeToleranceTerminationCondition< > >(
                               1.0e-10 ), _1, _2, _3, _4, _5 );

    // Test Secant object, per case.
    SecantRootFinder secantLowCase( terminationConditionFunction, 1.0 );
    SecantRootFinder secantHighCase( terminationConditionFunction, 1.0 );

    // Let the Secant method search for the root.
    const double rootLowCase = secantLowCase.execute( testFunctionLowCase, 1.0 + 8.0e-9 );
    const double rootHighCase = secantHighCase.execute( testFunctionHighCase, 10000.0 );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( rootLowCase, expectedRootLowCase, tolerance );
    BOOST_CHECK_CLOSE_FRACTION( rootHighCase, expectedRootHighCase, tolerance );
}

//! Check if Secant method converges on function with zero root (testFunctionWithZeroRoot).
// Not the best test case. Inheritance from old code. Not really relevant anymore. The basic
// idea is that Newton-Raphson should work for both a function that becomes zero, as well as for a
// function that does not become zero. A better case should be written.
BOOST_AUTO_TEST_CASE( test_secantRootFinder_testFunctionWithZeroRoot )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunctionWithZeroRoot > testFunction =
            boost::make_shared< TestFunctionWithZeroRoot >( 0 );

    // The termination condition.
    SecantRootFinder::TerminationFunction terminationConditionFunction =
            boost::bind(
                &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                    1.0e-150 ), _1, _2, _3, _4, _5 );

    // Test Secant object. Use the default value of the first initial guess.
    SecantRootFinder secantRootFinder( terminationConditionFunction );

    // Let the Secant method search for the root.
    const double root = secantRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_SMALL( root, 1.0e-100 );
    BOOST_CHECK_SMALL( testFunction->evaluate( root ), 1.0e-200 );
}

BOOST_AUTO_TEST_SUITE_END( ) // testsuite_rootfinders

} // namespace unit_tests
} // namespace tudat


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

#include <iostream>
#include <fstream>
#include <iomanip>

#include "main.h"
#include <Eigen/LevenbergMarquardt>
using namespace std;
using namespace Eigen;

template<typename Scalar>
struct DenseLM : DenseFunctor<Scalar>
{
  typedef DenseFunctor<Scalar> Base;
  typedef typename Base::JacobianType JacobianType;
  typedef Matrix<Scalar,Dynamic,1> VectorType;

  DenseLM(int n, int m) : DenseFunctor<Scalar>(n,m)
  { }

  VectorType model(const VectorType& uv, VectorType& x)
  {
    VectorType y; // Should change to use expression template
    int m = Base::values();
    int n = Base::inputs();
    eigen_assert(uv.size()%2 == 0);
    eigen_assert(uv.size() == n);
    eigen_assert(x.size() == m);
    y.setZero(m);
    int half = n/2;
    VectorBlock<const VectorType> u(uv, 0, half);
    VectorBlock<const VectorType> v(uv, half, half);
    for (int j = 0; j < m; j++)
    {
      for (int i = 0; i < half; i++)
        y(j) += u(i)*std::exp(-(x(j)-i)*(x(j)-i)/(v(i)*v(i)));
    }
    return y;

  }
  void initPoints(VectorType& uv_ref, VectorType& x)
  {
    m_x = x;
    m_y = this->model(uv_ref, x);
  }

  int operator()(const VectorType& uv, VectorType& fvec)
  {

    int m = Base::values();
    int n = Base::inputs();
    eigen_assert(uv.size()%2 == 0);
    eigen_assert(uv.size() == n);
    eigen_assert(fvec.size() == m);
    int half = n/2;
    VectorBlock<const VectorType> u(uv, 0, half);
    VectorBlock<const VectorType> v(uv, half, half);
    for (int j = 0; j < m; j++)
    {
      fvec(j) = m_y(j);
      for (int i = 0; i < half; i++)
      {
        fvec(j) -= u(i) *std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i)));
      }
    }

    return 0;
  }
  int df(const VectorType& uv, JacobianType& fjac)
  {
    int m = Base::values();
    int n = Base::inputs();
    eigen_assert(n == uv.size());
    eigen_assert(fjac.rows() == m);
    eigen_assert(fjac.cols() == n);
    int half = n/2;
    VectorBlock<const VectorType> u(uv, 0, half);
    VectorBlock<const VectorType> v(uv, half, half);
    for (int j = 0; j < m; j++)
    {
      for (int i = 0; i < half; i++)
      {
        fjac.coeffRef(j,i) = -std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i)));
        fjac.coeffRef(j,i+half) = -2.*u(i)*(m_x(j)-i)*(m_x(j)-i)/(std::pow(v(i),3)) * std::exp(-(m_x(j)-i)*(m_x(j)-i)/(v(i)*v(i)));
      }
    }
    return 0;
  }
  VectorType m_x, m_y; //Data Points
};

template<typename FunctorType, typename VectorType>
int test_minimizeLM(FunctorType& functor, VectorType& uv)
{
  LevenbergMarquardt<FunctorType> lm(functor);
  LevenbergMarquardtSpace::Status info;

  info = lm.minimize(uv);

  VERIFY_IS_EQUAL(info, 1);
  //FIXME Check other parameters
  return info;
}

template<typename FunctorType, typename VectorType>
int test_lmder(FunctorType& functor, VectorType& uv)
{
  typedef typename VectorType::Scalar Scalar;
  LevenbergMarquardtSpace::Status info;
  LevenbergMarquardt<FunctorType> lm(functor);
  info = lm.lmder1(uv);

  VERIFY_IS_EQUAL(info, 1);
  //FIXME Check other parameters
  return info;
}

template<typename FunctorType, typename VectorType>
int test_minimizeSteps(FunctorType& functor, VectorType& uv)
{
  LevenbergMarquardtSpace::Status info;
  LevenbergMarquardt<FunctorType> lm(functor);
  info = lm.minimizeInit(uv);
  if (info==LevenbergMarquardtSpace::ImproperInputParameters)
      return info;
  do
  {
    info = lm.minimizeOneStep(uv);
  } while (info==LevenbergMarquardtSpace::Running);

  VERIFY_IS_EQUAL(info, 1);
  //FIXME Check other parameters
  return info;
}

template<typename T>
void test_denseLM_T()
{
  typedef Matrix<T,Dynamic,1> VectorType;

  int inputs = 10;
  int values = 1000;
  DenseLM<T> dense_gaussian(inputs, values);
  VectorType uv(inputs),uv_ref(inputs);
  VectorType x(values);

  // Generate the reference solution
  uv_ref << -2, 1, 4 ,8, 6, 1.8, 1.2, 1.1, 1.9 , 3;

  //Generate the reference data points
  x.setRandom();
  x = 10*x;
  x.array() += 10;
  dense_gaussian.initPoints(uv_ref, x);

  // Generate the initial parameters
  VectorBlock<VectorType> u(uv, 0, inputs/2);
  VectorBlock<VectorType> v(uv, inputs/2, inputs/2);

  // Solve the optimization problem

  //Solve in one go
  u.setOnes(); v.setOnes();
  test_minimizeLM(dense_gaussian, uv);

  //Solve until the machine precision
  u.setOnes(); v.setOnes();
  test_lmder(dense_gaussian, uv);

  // Solve step by step
  v.setOnes(); u.setOnes();
  test_minimizeSteps(dense_gaussian, uv);

}

void test_denseLM()
{
  CALL_SUBTEST_2(test_denseLM_T<double>());

  // CALL_SUBTEST_2(test_sparseLM_T<std::complex<double>());
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/linear_position_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <algorithm>
#include <ceres/rotation.h>
#include <glog/logging.h>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "spectra/include/SymEigsShiftSolver.h"

#include "theia/math/graph/triplet_extractor.h"
#include "theia/math/matrix/spectra_linear_operator.h"
#include "theia/sfm/find_common_tracks_in_views.h"
#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {

std::vector<ViewIdTriplet> GetLargetConnectedTripletGraph(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs) {
  static const int kLargestCCIndex = 0;

  // Get a list of all edges in the view graph.
  std::unordered_set<ViewIdPair> view_id_pairs;
  view_id_pairs.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    view_id_pairs.insert(view_pair.first);
  }

  // Extract connected triplets.
  TripletExtractor<ViewId> extractor;
  std::vector<std::vector<ViewIdTriplet> > triplets;
  CHECK(extractor.ExtractTriplets(view_id_pairs, &triplets));
  CHECK_GT(triplets.size(), 0);
  return triplets[kLargestCCIndex];
}

// Adds the constraint from the triplet to the symmetric matrix. Our standard
// constraint matrix A is a 3M x 3N matrix with M triplet constraints and N
// cameras. We seek to construct A^t * A directly. For each triplet constraint
// in our matrix A (i.e. a 3-row block), we can compute the corresponding
// entries in A^t * A with the following summation:
//
//   A^t * A += Row(i)^t * Row(i)
//
// for each triplet constraint i.
void AddTripletConstraintToSymmetricMatrix(
    const std::vector<Matrix3d>& constraints,
    const std::vector<int>& view_indices,
    std::unordered_map<std::pair<int, int>, double>* sparse_matrix_entries) {
  // Construct Row(i)^t * Row(i). If we denote the row as a block matrix:
  //
  //   Row(i) = [A | B | C]
  //
  // then we have:
  //
  //   Row(i)^t * Row(i) = [A | B | C]^t * [A | B | C]
  //                     = [ A^t * A  |  A^t * B  |  A^t * C]
  //                       [ B^t * A  |  B^t * B  |  B^t * C]
  //                       [ C^t * A  |  C^t * B  |  C^t * C]
  //
  // Since A^t * A is symmetric, we only store the upper triangular portion.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      // Skip any block entries that correspond to the lower triangular portion
      // of the matrix.
      if (view_indices[i] > view_indices[j]) {
        continue;
      }

      // Compute the A^t * B, etc. matrix.
      const Eigen::Matrix3d symmetric_constraint =
          constraints[i].transpose() * constraints[j];

      // Add to the 3x3 block corresponding to (i, j)
      for (int r = 0; r < 3; r++) {
        for (int c = 0; c < 3; c++) {
          const std::pair<int, int> row_col(view_indices[i] + r,
                                            view_indices[j] + c);
          (*sparse_matrix_entries)[row_col] += symmetric_constraint(r, c);
        }
      }
    }
  }
}

inline Matrix3d AngleAxisToRotationMatrix(const Vector3d angle_axis) {
  const double angle = angle_axis.norm();
  const Eigen::AngleAxisd rotation_aa(angle, angle_axis / angle);
  return rotation_aa.toRotationMatrix();
}

// Returns true if the vector R1 * (c2 - c1) is in the same direction as t_12.
bool VectorsAreSameDirection(const Vector3d& position1,
                             const Vector3d& position2,
                             const Vector3d& rotation1,
                             const Vector3d& relative_position12) {
  const Vector3d global_relative_position =
      (position2 - position1).normalized();
  Vector3d rotated_relative_position;
  ceres::AngleAxisRotatePoint(rotation1.data(),
                              global_relative_position.data(),
                              rotated_relative_position.data());
  return rotated_relative_position.dot(relative_position12) > 0;
}

}  // namespace

LinearPositionEstimator::LinearPositionEstimator(
    const Options& options, const Reconstruction& reconstruction)
    : options_(options), reconstruction_(reconstruction) {
  CHECK_GT(options.num_threads, 0);
}

bool LinearPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();
  view_pairs_ = &view_pairs;
  orientations_ = &orientations;

  // Extract triplets from the view pairs. As of now, we only consider the
  // largest connected triplet in the viewing graph.
  VLOG(2) << "Extracting triplets from the viewing graph.";
  triplets_ = GetLargetConnectedTripletGraph(view_pairs);

  VLOG(2) << "Determining baseline ratios within each triplet...";
  // Baselines where (x, y, z) corresponds to the baseline of the first,
  // second, and third view pair in the triplet.
  std::unique_ptr<ThreadPool> pool(new ThreadPool(options_.num_threads));
  baselines_.resize(triplets_.size());
  for (size_t i = 0; i < triplets_.size(); i++) {
    AddTripletConstraint(triplets_[i]);
    pool->Add(&LinearPositionEstimator::ComputeBaselineRatioForTriplet,
              this,
              triplets_[i],
              &baselines_[i]);
  }
  pool.reset(nullptr);

  VLOG(2) << "Building the constraint matrix...";
  // Create the linear system based on triplet constraints.
  Eigen::SparseMatrix<double> constraint_matrix;
  CreateLinearSystem(&constraint_matrix);

  // Solve for positions by examining the smallest eigenvalues. Since we have
  // set one position constant at the origin, we only need to solve for the
  // eigenvector corresponding to the smallest eigenvalue. This can be done
  // efficiently with inverse power iterations.
  VLOG(2) << "Solving for positions from the sparse eigenvalue problem...";
  SparseSymShiftSolveLLT op(constraint_matrix);
  Spectra::
      SymEigsShiftSolver<double, Spectra::LARGEST_MAGN, SparseSymShiftSolveLLT>
          eigs(&op, 1, 6, 0.0);
  eigs.init();
  eigs.compute();

  // Compute with power iterations.
  const Eigen::VectorXd solution = eigs.eigenvectors().col(0);

  // Add the solutions to the output. Set the position with an index of -1 to
  // be at the origin.
  for (const auto& view_index : linear_system_index_) {
    if (view_index.second < 0) {
      (*positions)[view_index.first].setZero();
    } else {
      (*positions)[view_index.first] =
          solution.segment<3>(view_index.second * 3);
    }
  }

  // Flip the sign of the positions if necessary.
  FlipSignOfPositionsIfNecessary(positions);

  return true;
}

// An alternative interface is to instead add triplets one by one to linear
// estimator. This allows for adding redundant observations of triplets, which
// may be useful if there are multiple estimates of the data.
void LinearPositionEstimator::AddTripletConstraint(
    const ViewIdTriplet& view_triplet) {
  num_triplets_for_view_[std::get<0>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<1>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<2>(view_triplet)] += 1;

  // Determine the order of the views in the linear system. We subtract 1 from
  // the linear system index so that the first position added to the system
  // will be set constant (index of -1 is intentionally not evaluated later).
  InsertIfNotPresent(&linear_system_index_,
                     std::get<0>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_,
                     std::get<1>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_,
                     std::get<2>(view_triplet),
                     linear_system_index_.size() - 1);
}

void LinearPositionEstimator::ComputeBaselineRatioForTriplet(
    const ViewIdTriplet& triplet, Vector3d* baseline) {
  baseline->setZero();

  const View& view1 = *reconstruction_.View(std::get<0>(triplet));
  const View& view2 = *reconstruction_.View(std::get<1>(triplet));
  const View& view3 = *reconstruction_.View(std::get<2>(triplet));

  // Find common tracks.
  const std::vector<ViewId> triplet_view_ids = {
      std::get<0>(triplet), std::get<1>(triplet), std::get<2>(triplet)};
  const std::vector<TrackId>& common_tracks =
      FindCommonTracksInViews(reconstruction_, triplet_view_ids);

  // Normalize all features.
  std::vector<Feature> feature1, feature2, feature3;
  feature1.reserve(common_tracks.size());
  feature2.reserve(common_tracks.size());
  feature3.reserve(common_tracks.size());
  for (const TrackId track_id : common_tracks) {
    feature1.emplace_back(GetNormalizedFeature(view1, track_id).point_);
    feature2.emplace_back(GetNormalizedFeature(view2, track_id).point_);
    feature3.emplace_back(GetNormalizedFeature(view3, track_id).point_);
  }

  // Get the baseline ratios.
  ViewTriplet view_triplet;
  view_triplet.view_ids[0] = std::get<0>(triplet);
  view_triplet.view_ids[1] = std::get<1>(triplet);
  view_triplet.view_ids[2] = std::get<2>(triplet);
  view_triplet.info_one_two = FindOrDieNoPrint(
      *view_pairs_,
      ViewIdPair(view_triplet.view_ids[0], view_triplet.view_ids[1]));
  view_triplet.info_one_three = FindOrDieNoPrint(
      *view_pairs_,
      ViewIdPair(view_triplet.view_ids[0], view_triplet.view_ids[2]));
  view_triplet.info_two_three = FindOrDieNoPrint(
      *view_pairs_,
      ViewIdPair(view_triplet.view_ids[1], view_triplet.view_ids[2]));

  ComputeTripletBaselineRatios(
      view_triplet, feature1, feature2, feature3, baseline);
}

// Sets up the linear system with the constraints that each triplet adds.
void LinearPositionEstimator::CreateLinearSystem(
    Eigen::SparseMatrix<double>* constraint_matrix) {
  const int num_views = num_triplets_for_view_.size();

  std::unordered_map<std::pair<int, int>, double> sparse_matrix_entries;
  sparse_matrix_entries.reserve(27 * num_triplets_for_view_.size());
  for (int i = 0; i < triplets_.size(); i++) {
    const ViewId& view_id1 = std::get<0>(triplets_[i]);
    const ViewId& view_id2 = std::get<1>(triplets_[i]);
    const ViewId& view_id3 = std::get<2>(triplets_[i]);
    AddTripletConstraintToSparseMatrix(
        view_id1, view_id2, view_id3, baselines_[i], &sparse_matrix_entries);
  }

  // Set the sparse matrix from the container of the accumulated entries.
  std::vector<Eigen::Triplet<double> > triplet_list;
  triplet_list.reserve(sparse_matrix_entries.size());
  for (const auto& sparse_matrix_entry : sparse_matrix_entries) {
    // Skip this entry if the indices are invalid. This only occurs when we
    // encounter a constraint with the constant camera (which has a view index
    // of -1).
    if (sparse_matrix_entry.first.first < 0 ||
        sparse_matrix_entry.first.second < 0) {
      continue;
    }
    triplet_list.emplace_back(sparse_matrix_entry.first.first,
                              sparse_matrix_entry.first.second,
                              sparse_matrix_entry.second);
  }

  // We construct the constraint matrix A^t * A directly, which is an
  // N - 1 x N - 1 matrix where N is the number of cameras (and 3 entries per
  // camera, corresponding to the camera position entries).
  constraint_matrix->resize((num_views - 1) * 3, (num_views - 1) * 3);
  constraint_matrix->setFromTriplets(triplet_list.begin(), triplet_list.end());
}

void LinearPositionEstimator::ComputeRotatedRelativeTranslationRotations(
    const ViewId view_id0,
    const ViewId view_id1,
    const ViewId view_id2,
    Eigen::Matrix3d* r012,
    Eigen::Matrix3d* r201,
    Eigen::Matrix3d* r120) {
  // Relative camera positions.
  const Eigen::Vector3d& orientation0_aa =
      FindOrDieNoPrint(*orientations_, view_id0);
  const Eigen::Vector3d& orientation1_aa =
      FindOrDieNoPrint(*orientations_, view_id1);
  const Matrix3d orientation0 = AngleAxisToRotationMatrix(orientation0_aa);
  const Matrix3d orientation1 = AngleAxisToRotationMatrix(orientation1_aa);
  const Vector3d t01 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id1)).position_2;
  const Vector3d t02 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id2)).position_2;
  const Vector3d t12 =
      -orientation1.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id1, view_id2)).position_2;

  // Rotations between the translation vectors.
  *r012 = Eigen::Quaterniond::FromTwoVectors(t12, -t01).toRotationMatrix();
  *r201 = Eigen::Quaterniond::FromTwoVectors(t01, t02).toRotationMatrix();
  *r120 = Eigen::Quaterniond::FromTwoVectors(-t02, -t12).toRotationMatrix();
}

// Adds a triplet constraint to the linear system. The weight of the constraint
// (w), the global orientations, baseline (ratios), and view triplet information
// are needed to form the constraint.
void LinearPositionEstimator::AddTripletConstraintToSparseMatrix(
    const ViewId view_id0,
    const ViewId view_id1,
    const ViewId view_id2,
    const Eigen::Vector3d& baselines,
    std::unordered_map<std::pair<int, int>, double>* sparse_matrix_entries) {
  // Weight each term by the inverse of the # of triplet that the nodes
  // participate in.
  const double w =
      1.0 / std::sqrt(std::min({num_triplets_for_view_[view_id0],
                                num_triplets_for_view_[view_id1],
                                num_triplets_for_view_[view_id2]}));

  // Get the index of each camera in the sparse matrix.
  const std::vector<int> view_indices = {
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id0)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id1)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id2))};

  // Compute the rotations between relative translations.
  Eigen::Matrix3d r012, r201, r120;
  ComputeRotatedRelativeTranslationRotations(
      view_id0, view_id1, view_id2, &r012, &r201, &r120);

  // Baselines ratios.
  const double s_012 = baselines[0] / baselines[2];
  const double s_201 = baselines[1] / baselines[0];
  const double s_120 = baselines[2] / baselines[1];

  // Assume that t01 is perfect and solve for c2.
  std::vector<Eigen::Matrix3d> constraints(3);
  constraints[0] =
      (-s_201 * r201 + r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  constraints[1] =
      (s_201 * r201 - r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  constraints[2] = -2.0 * w * Matrix3d::Identity();
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);

  // Assume t02 is perfect and solve for c1.
  constraints[0] =
      (-r201.transpose() / s_201 + s_120 * r120 + Matrix3d::Identity()) * w;
  constraints[1] = -2.0 * w * Matrix3d::Identity();
  constraints[2] =
      (r201.transpose() / s_201 - s_120 * r120 + Matrix3d::Identity()) * w;
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);

  // Assume t12 is perfect and solve for c0.
  constraints[0] = -2.0 * w * Matrix3d::Identity();
  constraints[1] =
      (-s_012 * r012 + r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  constraints[2] =
      (s_012 * r012 - r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);
}

Feature LinearPositionEstimator::GetNormalizedFeature(const View& view,
                                                      const TrackId track_id) {
  Feature feature = *view.GetFeature(track_id);
  const Camera& camera = view.Camera();
  Eigen::Vector3d ray = camera.PixelToNormalizedCoordinates(feature.point_);
  Feature normalized_Feature(ray.hnormalized());
  // todo normalized covariance?
  return normalized_Feature;
}

void LinearPositionEstimator::FlipSignOfPositionsIfNecessary(
    std::unordered_map<ViewId, Vector3d>* positions) {
  // If this value is below zero, then we should flip the sign.
  int correct_sign_votes = 0;
  for (const auto& view_pair : *view_pairs_) {
    // Only count the votes for edges where both positions were successfully
    // estimated.
    const Vector3d* position1 = FindOrNull(*positions, view_pair.first.first);
    const Vector3d* position2 = FindOrNull(*positions, view_pair.first.second);
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Check the relative translation of views 1 and 2 in the triplet.
    if (VectorsAreSameDirection(
            *position1,
            *position2,
            FindOrDieNoPrint(*orientations_, view_pair.first.first),
            view_pair.second.position_2)) {
      correct_sign_votes += 1;
    } else {
      correct_sign_votes -= 1;
    }
  }

  // If the sign of the votes is below zero, we must flip the sign of all
  // position estimates.
  if (correct_sign_votes < 0) {
    const int num_correct_votes =
        (view_pairs_->size() + correct_sign_votes) / 2;
    VLOG(2) << "Sign of the positions was incorrect: " << num_correct_votes
            << " of " << view_pairs_->size()
            << " relative translations had the correct sign. "
               "Flipping the sign of the camera positions.";
    for (auto& position : *positions) {
      position.second *= -1.0;
    }
  }
}

std::unordered_map<ViewId, Eigen::Vector3d>
LinearPositionEstimator::EstimatePositionsWrapper(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientation) {
  std::unordered_map<ViewId, Eigen::Vector3d> positions;
  EstimatePositions(view_pairs, orientation, &positions);
  return positions;
}

}  // namespace theia


#include <array>
#include <vector>
#include "plane3d.hpp"
#include "mesh_components.hpp"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

#ifndef FOUND_PATH_HPP
#define FOUND_PATH_HPP

class FoundPath{
    unsigned long int planeId;
    Vector3d pt0;
    Vector3d pt1;
    double lowerBound;
    double upperBound;
  public:
    FoundPath(unsigned long int _planeId, Vector3d _pt0, Vector3d _pt1, double _lowerBound, double _upperBound);
    unsigned long int PlaneId();
    double UpperBound();
    double LowerBound();
    array<Vector3d, 2> Points();
};

#endif


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/
#ifndef __DEVELOPMENT_DEBUG_MODE__
#define __FEATURE_UTILS_NO_DEBUG__
#endif
#include "feature_utils.h"
#include "patch_score.h"

#include "cameras/PinholeCamera.hpp"
#include "cameras/RadialTangentialDistortion.hpp"
#include "cameras/RadialTangentialDistortion8.hpp"

#include <boost/lexical_cast.hpp>
#ifndef __FEATURE_UTILS_NO_DEBUG__
#include <opencv2/highgui.hpp>
#endif

// define this MACROS to enable profing and verifying align2D_NEON
#undef _ALIGN2D_TIMING_AND_VERIFYING
#ifdef _ALIGN2D_TIMING_AND_VERIFYING
#include <vio/timing/ProfilingTimer.hpp>
using vio::timing::MicrosecondStopwatch;
MicrosecondStopwatch timer_align2d("align2d", 0);
MicrosecondStopwatch timer_align2d_neon("align2d neon", 0);
float align2d_total_time = 0.f;
float align2d_total_time_neon = 0.f;
#endif

namespace XP {

using Eigen::Vector2i;
using Eigen::Vector2f;
using Eigen::Vector3f;
using Eigen::Matrix2f;
using Eigen::Matrix3f;
using Eigen::Matrix4f;

// The triangulated point at the current frame coordinate can be represented as:
// R_cur_ref * f_ref * d_ref + t_cur_ref =  f_cur * d_cur,
// where d_ref and d_cur are scales, respetively
// We can organize the equation into a linear system to solve d in the form of
// Ax = b:
// [R_cur_ref * f_ref  f_cur] * [d_ref; - d_cur] = - t_cur_ref
//   A = [R_cur_ref * f_ref f_cur],  3 x 2
//   x = [d_ref; -d_cur],  2 x 1
//   b = - t_cur_ref,  3 x 1
// x = (AtA)^-1 * At * b
//
// The returned depth is based on the reference frame
bool depthFromTriangulation(const Matrix3f& R_cur_ref,
                            const Vector3f& t_cur_ref, const Vector3f& f_ref,
                            const Vector3f& f_cur, float* depth) {
  Eigen::Matrix<float, 3, 2> A;
  A << R_cur_ref * f_ref, f_cur;
  const Matrix2f AtA = A.transpose() * A;
  if (AtA.determinant() < 1e-6) {
    // TODO(mingyu): figure the right threshold for float
    *depth = 1000;  // a very far point
    return false;
  }
  const Vector2f depth2 = -AtA.inverse() * A.transpose() * t_cur_ref;
  *depth = fabs(depth2[0]);
  return true;
}

void DirectMatcher::createPatchFromPatchWithBorder() {
  uint8_t* ref_patch_ptr = patch_;
  for (int y = 1; y < patch_size_ + 1; ++y, ref_patch_ptr += patch_size_) {
    uint8_t* ref_patch_border_ptr =
        patch_with_border_ + y * (patch_size_ + 2) + 1;
    memcpy(ref_patch_ptr, ref_patch_border_ptr, patch_size_);
    /*
    for (int x = 0; x < patch_size_; ++x) {
      ref_patch_ptr[x] = ref_patch_border_ptr[x];
    }
    */
  }
}

bool DirectMatcher::findMatchDirect(
    const vio::cameras::CameraBase& cam_ref,
    const vio::cameras::CameraBase& cam_cur, const Vector2f& px_ref,
    const Vector3f& f_ref, const Matrix3f& R_cur_ref, const Vector3f& t_cur_ref,
    const int level_ref, const float depth_ref,
    const std::vector<cv::Mat>& pyrs_ref, const std::vector<cv::Mat>& pyrs_cur,
    const bool edgelet_feature, Vector2f* px_cur) {
  CHECK_NEAR(f_ref[2], 1.f, 1e-6);
  CHECK_EQ(pyrs_ref.size(), pyrs_cur.size());

  // TODO(mingyu): check return boolean of getWarpMatrixAffine
  // warp affine
  warp::getWarpMatrixAffine(cam_ref, cam_cur, px_ref, f_ref, depth_ref,
                            R_cur_ref, t_cur_ref, level_ref, &A_cur_ref_);
  const int max_level = pyrs_ref.size() - 1;
  const int search_level = warp::getBestSearchLevel(A_cur_ref_, max_level);

  // TODO(mingyu): check return boolean of warpAffine
  warp::warpAffine(A_cur_ref_, pyrs_ref[level_ref], px_ref, level_ref,
                   search_level, halfpatch_size_ + 1, patch_with_border_);
  createPatchFromPatchWithBorder();

  // px_cur should be set
  Vector2f px_scaled = *px_cur / (1 << search_level);

  bool success = false;
  if (edgelet_feature) {
    // TODO(mingyu): currently not used until we further refine the feature type
    //               with gradient direction.
    //               Fast features do contain edgelet features.
    /*
    Vector2f dir_cur(A_cur_ref_ * ref_ftr_->grad);
    dir_cur.normalize();
    success = align::align1D(pyrs_cur[search_level],
                             dir_cur,
                             patch_with_border_,
                             patch_,
                             options_.max_iter,
                             &px_scaled,
                             &h_inv_);
    */
    LOG(ERROR) << "findMatchDirect for edgelet feature is NOT rimplemented yet";
    success = false;
  } else {
#ifndef _ALIGN2D_TIMING_AND_VERIFYING
#ifndef __ARM_NEON__
    success = align::align2D(pyrs_cur[search_level], patch_with_border_, patch_,
                             options_.max_iter, &px_scaled);
#else
    success = align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                  patch_, options_.max_iter, &px_scaled);
#endif  // __ARM_NEON__
#else
// timing and verifying code.
#ifdef __ARM_NEON__
    Vector2f px_scaled_neon = px_scaled;
#endif  // __ARM_NEON__
    timer_align2d.start();
    success = align::align2D(pyrs_cur[search_level], patch_with_border_, patch_,
                             options_.max_iter, &px_scaled);
    timer_align2d.stop();
    align2d_total_time = timer_align2d.stop();

#ifdef __ARM_NEON__
    timer_align2d_neon.start();
    bool success_neon =
        align::align2D_NEON(pyrs_cur[search_level], patch_with_border_, patch_,
                            options_.max_iter, &px_scaled_neon);
    timer_align2d_neon.stop();
    align2d_total_time_neon = timer_align2d_neon.elapse_ms();
    CHECK_EQ(success, success_neon);
    if (success_neon) {
      CHECK_NEAR(px_scaled[0], px_scaled_neon[0], 0.05);
      CHECK_NEAR(px_scaled[1], px_scaled_neon[1], 0.05);
      std::cout << "[NEON : NON-NEON --->"
                << "[" << align2d_total_time_neon << " : " << align2d_total_time
                << "]" << std::endl;
    }
#endif  // __ARM_NEON__
#endif  // _ALIGN2D_TIMING_AND_VERIFYING
  }
  *px_cur = px_scaled * (1 << search_level);
  return success;
}

bool DirectMatcher::findEpipolarMatchDirect(
    const vio::cameras::CameraBase& cam_ref,
    const vio::cameras::CameraBase& cam_cur, const Vector2f& px_ref,
    const Vector3f& f_ref, const Matrix3f& R_cur_ref, const Vector3f& t_cur_ref,
    const int level_ref, const float d_estimate, const float d_min,
    const float d_max, const std::vector<cv::Mat>& pyrs_ref,
    const std::vector<cv::Mat>& pyrs_cur, const cv::Mat_<uchar>& mask_cur,
    const bool edgelet_feature, Vector2f* px_cur, float* depth, int* level_cur,
    cv::Mat* dbg_cur) {
  CHECK_NEAR(f_ref[2], 1.f, 1e-6);
  CHECK_EQ(pyrs_ref.size(), pyrs_cur.size());

  // Compute start and end of epipolar line in old_kf for match search, on unit
  // plane!
  // i.e., A & B are the first two elements of unit rays.
  // We will search from far to near
  Vector3f ray_A, ray_B;
  Vector2f px_A, px_B;
  ray_B = R_cur_ref * (f_ref * d_max) + t_cur_ref;  // far
  ray_B /= ray_B(2);
  if (vio::cameras::CameraBase::ProjectionStatus::Successful !=
      cam_cur.project(ray_B, &px_B)) {
#ifndef __FEATURE_UTILS_NO_DEBUG__
    VLOG(2) << "ray_A (far) cannot be reprojected in cam_cur";
#endif
    return false;
  }

  bool invalid_ray_A = true;
  for (float d = d_min; d < d_estimate; d *= 10) {
    ray_A = R_cur_ref * (f_ref * d) + t_cur_ref;  // near
    ray_A /= ray_A(2);
    if (vio::cameras::CameraBase::ProjectionStatus::Successful ==
        cam_cur.project(ray_A, &px_A)) {
      invalid_ray_A = false;
      break;
    }
  }
  if (invalid_ray_A) {
#ifndef __FEATURE_UTILS_NO_DEBUG__
    VLOG(2) << "ray_B (near) cannot be reprojected in cam_cur "
            << " d_min = " << d_min << " d_estimate = " << d_estimate;
#endif
    return false;
  }

  // Compute warp affine matrix
  if (!warp::getWarpMatrixAffine(cam_ref, cam_cur, px_ref, f_ref, d_estimate,
                                 R_cur_ref, t_cur_ref, level_ref,
                                 &A_cur_ref_)) {
    LOG(WARNING) << "warp::getWarpMatrixAffine fails";
    return false;
  }
#ifndef __FEATURE_UTILS_NO_DEBUG__
  VLOG(2) << "A_cur_ref_ =\n" << A_cur_ref_;
#endif

  const int max_level = pyrs_ref.size() - 1;
  const int search_level = warp::getBestSearchLevel(A_cur_ref_, max_level);
  epi_dir_ = ray_A.head<2>() - ray_B.head<2>();  // far to near, B to A
  epi_length_ = (px_A - px_B).norm() / (1 << search_level);

  // feature pre-selection
  if (edgelet_feature) {
    /*
    const Vector2f grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
    const float cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
    if (cosangle < options_.epi_search_edgelet_max_angle) {
      return false;
    }
    */
    LOG(ERROR)
        << "findEpipolarMatchDirect for edgelet feature is NOT implemented yet";
    return false;
  }

  if (!warp::warpAffine(A_cur_ref_, pyrs_ref[level_ref], px_ref, level_ref,
                        search_level, halfpatch_size_ + 1,
                        patch_with_border_)) {
    return false;
  }
  createPatchFromPatchWithBorder();
#ifndef __FEATURE_UTILS_NO_DEBUG__
  VLOG(2) << " search_level = " << search_level
          << " epi_length_ = " << epi_length_;
#endif
  if (epi_length_ < options_.max_epi_length_optim) {
    // The epipolar search line is short enough (< 2 pixels)
    // to perform direct alignment
    *px_cur = (px_A + px_B) * 0.5f;
    Vector2f px_scaled(*px_cur / (1 << search_level));
    bool success;
    if (options_.align_1d) {
      Vector2f direction = (px_A - px_B).normalized();
      success =
          align::align1D(pyrs_cur[search_level], direction, patch_with_border_,
                         patch_, options_.max_iter, &px_scaled, &h_inv_);
    } else {
#ifndef _ALIGN2D_TIMING_AND_VERIFYING
#ifndef __ARM_NEON__
      success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                               patch_, options_.max_iter, &px_scaled);
#else
      success = align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                    patch_, options_.max_iter, &px_scaled);
#endif  // __ARM_NEON__
#else
// verifying and timing code
#ifdef __ARM_NEON__
      Vector2f px_scaled_neon = px_scaled;
#endif  // __ARM_NEON__
      timer_align2d.start();
      success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                               patch_, options_.max_iter, &px_scaled);
      timer_align2d.stop();
      align2d_total_time = timer_align2d.elapse_ms();
#ifdef __ARM_NEON__
      timer_align2d_neon.start();
      bool success_neon =
          align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                              patch_, options_.max_iter, &px_scaled_neon);
      timer_align2d_neon.stop();
      align2d_total_time_neon = timer_align2d_neon.elapse_ms();
      CHECK_EQ(success, success_neon);
      if (success_neon) {
        CHECK_NEAR(px_scaled[0], px_scaled_neon[0], 0.05);
        CHECK_NEAR(px_scaled[1], px_scaled_neon[1], 0.05);
        std::cout << "[NEON : NON-NEON --->"
                  << "[" << align2d_total_time_neon << " : "
                  << align2d_total_time << "]" << std::endl;
      }
#endif  // __ARM_NEON__
#endif  // _ALIGN2D_TIMING_AND_VERIFYING
    }

    if (success) {
      *px_cur = px_scaled * (1 << search_level);
      Vector3f f_cur;
      if (cam_cur.backProject(*px_cur, &f_cur)) {
        CHECK_NEAR(f_cur[2], 1.f, 1e-6);
        if (!depthFromTriangulation(R_cur_ref, t_cur_ref, f_ref, f_cur,
                                    depth)) {
          LOG(WARNING) << "depthFromTriangulation fails, set depth to d_max";
          *depth = d_max;
        }
      }
    }

    if (dbg_cur != nullptr) {
      if (success) {
        // green: subpix alignment is good
        cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                   cv::Scalar(0, 255, 0));
      } else {
        // red: subpix alignment fails
        cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                   cv::Scalar(0, 0, 255));
      }
    }
    return success;
  }

  // Determine the steps to Search along the epipolar line
  // [NOTE] The epipolar line can be curvy, so we slightly increase it
  //        to roughly have one step per pixel (heuristically).
  size_t n_steps = epi_length_ / 0.7;
  Vector3f step;
  step << epi_dir_ / n_steps, 0;
  if (n_steps > options_.max_epi_search_steps) {
    LOG(ERROR) << "Skip epipolar search: evaluations = " << n_steps
               << "epi length (px) = " << epi_length_;
    return false;
  }

  // Search along the epipolar line (on unit plane) with patch matching
  // for matching, precompute sum and sum2 of warped reference patch
  // [heuristic] The ssd from patch mean difference can be up to 50% of the
  // resulting ssd.
  //  ssd = zmssd + N * (a_bar - b_bar)^2
  typedef patch_score::ZMSSD<halfpatch_size_> PatchScore;
  PatchScore patch_score(patch_);
  int zmssd_best = PatchScore::threshold();
  int ssd_corr = PatchScore::threshold() * 2;
  Vector3f ray_best;
  Vector3f ray = ray_B;
  Eigen::Vector2i last_checked_pxi(0, 0);
  const int search_img_rows = pyrs_cur[search_level].rows;
  const int search_img_cols = pyrs_cur[search_level].cols;
  ++n_steps;
  for (size_t i = 0; i < n_steps; ++i, ray += step) {
    Vector2f px;
    if (vio::cameras::CameraBase::ProjectionStatus::Successful !=
        cam_cur.project(ray, &px)) {
      // We have already checked the valid projection of starting and ending
      // rays.  However,
      // under very rare circumstance, cam_cur.project may still fail:
      // close to zero denominator for radial tangential 8 distortion:
      // 1 + k4 * r^2 + k5 * r^4 + k6 * r^6 < 1e-6
      continue;
    }
    Vector2i pxi(px[0] / (1 << search_level) + 0.5,
                 px[1] / (1 << search_level) + 0.5);  // round to closest int
    if (pxi == last_checked_pxi) {
      continue;
    }
    last_checked_pxi = pxi;

    // check if the patch is full within the new frame
    if (pxi[0] >= halfpatch_size_ &&
        pxi[0] < search_img_cols - halfpatch_size_ &&
        pxi[1] >= halfpatch_size_ &&
        pxi[1] < search_img_rows - halfpatch_size_ &&
        mask_cur(pxi(1), pxi(0)) > 0) {
      // TODO(mingyu): Interpolation instead?
      uint8_t* cur_patch_ptr = pyrs_cur[search_level].data +
                               (pxi[1] - halfpatch_size_) * search_img_cols +
                               (pxi[0] - halfpatch_size_);
      int ssd, zmssd;
      patch_score.computeScore(cur_patch_ptr, search_img_cols, &zmssd, &ssd);
      if (zmssd < zmssd_best) {
        // We store the best zmssd and its corresponding ssd score.  Usually,
        // zmssd and ssd have good correlation if the *matching* is reasonable.
        zmssd_best = zmssd;
        ssd_corr = ssd;
        ray_best = ray;
      }
#ifndef __FEATURE_UTILS_NO_DEBUG__
      VLOG(2) << "search pxi=[" << pxi[0] << ", " << pxi[1]
              << "] zmssd = " << zmssd << " ssd = " << ssd;
#endif
      if (dbg_cur != nullptr) {
        dbg_cur->at<cv::Vec3b>(pxi[1], pxi[0]) = cv::Vec3b(255, 0, 0);
      }
    } else {
      // The patch contains out of bound pixels
      continue;
    }
  }

  VLOG(2) << "zmssd_best = " << zmssd_best << " ssd_corr = " << ssd_corr
          << " zmssd / ssd = " << static_cast<float>(zmssd_best) / ssd_corr;
  if (zmssd_best < PatchScore::threshold()) {
    cam_cur.project(ray_best, px_cur);
    if (options_.subpix_refinement) {
      Vector2f px_scaled(*px_cur / (1 << search_level));
      bool success;
      if (options_.align_1d) {
        Vector2f direction = (px_A - px_B).normalized();
        success = align::align1D(pyrs_cur[search_level], direction,
                                 patch_with_border_, patch_, options_.max_iter,
                                 &px_scaled, &h_inv_);
      } else {
#ifndef _ALIGN2D_TIMING_AND_VERIFYING
#ifndef __ARM_NEON__
        success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                                 patch_, options_.max_iter, &px_scaled);
#else
        success =
            align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                patch_, options_.max_iter, &px_scaled);
#endif  // __ARM_NEON__
#else
#ifdef __ARM_NEON__
        Vector2f px_scaled_neon = px_scaled;
#endif  // __ARM_NEON__
        timer_align2d.start();
        success = align::align2D(pyrs_cur[search_level], patch_with_border_,
                                 patch_, options_.max_iter, &px_scaled);
        timer_align2d.stop();
        align2d_total_time = timer_align2d.elapse_ms();
#ifdef __ARM_NEON__
        timer_align2d_neon.start();
        bool success_neon =
            align::align2D_NEON(pyrs_cur[search_level], patch_with_border_,
                                patch_, options_.max_iter, &px_scaled_neon);
        timer_align2d_neon.stop();
        align2d_total_time_neon = timer_align2d_neon.elapse_ms();
        CHECK_EQ(success, success_neon);
        if (success_neon) {
          CHECK_NEAR(px_scaled[0], px_scaled_neon[0], 0.05);
          CHECK_NEAR(px_scaled[1], px_scaled_neon[1], 0.05);
          std::cout << "[NEON : NON-NEON --->"
                    << "[" << align2d_total_time_neon << " : "
                    << align2d_total_time << "]" << std::endl;
        }
#endif  // __ARM_NEON__
#endif  // _ALIGN2D_TIMING_AND_VERIFYING
      }

      if (success) {
        *px_cur = px_scaled * (1 << search_level);
        Vector3f f_cur;
        if (cam_cur.backProject(*px_cur, &f_cur)) {
          CHECK_NEAR(f_cur[2], 1.f, 1e-6);
          if (!depthFromTriangulation(R_cur_ref, t_cur_ref, f_ref, f_cur,
                                      depth)) {
            LOG(WARNING) << "depthFromTriangulation fails, set depth to d_max";
            *depth = d_max;
          }
        }
      }

      if (dbg_cur != nullptr) {
        if (success) {
          // green: subpix alignment is good
          cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                     cv::Scalar(0, 255, 0));
        } else {
          // red: subpix alignment fails
          cv::circle(*dbg_cur, cv::Point2f((*px_cur)(0), (*px_cur)(1)), 2,
                     cv::Scalar(0, 0, 255));
        }
      }
      return success;
    } else {
      // No subpix refinement
      CHECK_NEAR(ray_best[2], 1.f, 1e-6);
      if (!depthFromTriangulation(R_cur_ref, t_cur_ref, f_ref, ray_best,
                                  depth)) {
        LOG(WARNING) << "depthFromTriangulation fails, set depth to d_max";
        *depth = d_max;
      }
      return true;
    }
  }

// No patch qualifiess a match
#ifndef __FEATURE_UTILS_NO_DEBUG__
  VLOG(1) << "No matching patch found for this feature";
#endif
  return false;
}

// Prepare all the shared variabls for the whole tracking pipeline
ImgFeaturePropagator::ImgFeaturePropagator(
    const Eigen::Matrix3f& cur_camK, const Eigen::Matrix3f& ref_camK,
    const cv::Mat_<float>& cur_cv_dist_coeff,
    const cv::Mat_<float>& ref_cv_dist_coeff, const cv::Mat_<uchar>& cur_mask,
    int feat_det_pyramid_level, float min_feature_distance_over_baseline_ratio,
    float max_feature_distance_over_baseline_ratio)
    : mask_cur_(cur_mask),
      min_feature_distance_over_baseline_ratio_(
          min_feature_distance_over_baseline_ratio),
      max_feature_distance_over_baseline_ratio_(
          max_feature_distance_over_baseline_ratio),
      feat_det_pyramid_level_(feat_det_pyramid_level) {
  CHECK_GT(mask_cur_.rows, 0);
  CHECK_GT(mask_cur_.cols, 0);
  if (cur_cv_dist_coeff.rows == 8) {
    cam_cur_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion8>(
        mask_cur_.cols, mask_cur_.rows, cur_camK(0, 0),  // focalLength[0],
        cur_camK(1, 1),                                  // focalLength[1],
        cur_camK(0, 2),                                  // principalPoint[0],
        cur_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion8(
            cur_cv_dist_coeff(0), cur_cv_dist_coeff(1), cur_cv_dist_coeff(2),
            cur_cv_dist_coeff(3), cur_cv_dist_coeff(4), cur_cv_dist_coeff(5),
            cur_cv_dist_coeff(6), cur_cv_dist_coeff(7))));
  } else if (cur_cv_dist_coeff.rows == 4) {
    cam_cur_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion>(
        mask_cur_.cols, mask_cur_.rows, cur_camK(0, 0),  // focalLength[0],
        cur_camK(1, 1),                                  // focalLength[1],
        cur_camK(0, 2),                                  // principalPoint[0],
        cur_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion(
            cur_cv_dist_coeff(0), cur_cv_dist_coeff(1), cur_cv_dist_coeff(2),
            cur_cv_dist_coeff(3))));
  } else {
    LOG(FATAL) << "Dist model unsupported for cam_cur_";
  }
  if (ref_cv_dist_coeff.rows == 8) {
    cam_ref_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion8>(
        mask_cur_.cols, mask_cur_.rows, ref_camK(0, 0),  // focalLength[0],
        ref_camK(1, 1),                                  // focalLength[1],
        ref_camK(0, 2),                                  // principalPoint[0],
        ref_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion8(
            ref_cv_dist_coeff(0), ref_cv_dist_coeff(1), ref_cv_dist_coeff(2),
            ref_cv_dist_coeff(3), ref_cv_dist_coeff(4), ref_cv_dist_coeff(5),
            ref_cv_dist_coeff(6), ref_cv_dist_coeff(7))));
  } else if (ref_cv_dist_coeff.rows == 4) {
    cam_ref_.reset(new vio::cameras::PinholeCamera<
                   vio::cameras::RadialTangentialDistortion>(
        mask_cur_.cols, mask_cur_.rows, ref_camK(0, 0),  // focalLength[0],
        ref_camK(1, 1),                                  // focalLength[1],
        ref_camK(0, 2),                                  // principalPoint[0],
        ref_camK(1, 2),                                  // principalPoint[1],
        vio::cameras::RadialTangentialDistortion(
            ref_cv_dist_coeff(0), ref_cv_dist_coeff(1), ref_cv_dist_coeff(2),
            ref_cv_dist_coeff(3))));
  } else {
    LOG(FATAL) << "Dist model unsupported for cam_ref_";
  }
}

bool ImgFeaturePropagator::PropagateFeatures(
    const cv::Mat& cur_img,
    const cv::Mat& ref_img,  // TODO(mingyu): store image pyramids
    const std::vector<cv::KeyPoint>& ref_keypoints, const Matrix4f& T_ref_cur,
    std::vector<cv::KeyPoint>* cur_keypoints, cv::Mat* cur_orb_features,
    const bool draw_debug) {
  cur_keypoints->clear();
  cur_keypoints->reserve(ref_keypoints.size());

  R_cur_ref_ = T_ref_cur.topLeftCorner<3, 3>().transpose();
  t_cur_ref_ = -R_cur_ref_ * T_ref_cur.topRightCorner<3, 1>();

  // TODO(mingyu): Make shared variables of DirectMatcher into member variables
  //               instead of passing as input arguments

  // Heuristically determine the d_max, d_min, and d_estimate based on the
  // baseline,
  // d_min = baseline x 3
  // d_max = baseline x 3000
  // inv_d_estimate is the average of inv_d_min and inv_d_max
  const float baseline = t_cur_ref_.norm();
  const float d_min = baseline * min_feature_distance_over_baseline_ratio_;
  const float d_max = baseline * max_feature_distance_over_baseline_ratio_;
  const float d_estimate = 2.f / (1.f / d_min + 1.f / d_max);

  // TODO(mingyu): feed the backend results back for d_estimate if available
  // TODO(mingyu): feed the pyramids in directly
  std::vector<cv::Mat> pyrs_cur(feat_det_pyramid_level_);
  std::vector<cv::Mat> pyrs_ref(feat_det_pyramid_level_);
  pyrs_cur[0] = cur_img;
  pyrs_ref[0] = ref_img;
  for (int pyr_lv = 1; pyr_lv < feat_det_pyramid_level_; ++pyr_lv) {
    pyrs_cur[pyr_lv] = fast_pyra_down(pyrs_cur[pyr_lv - 1]);
    pyrs_ref[pyr_lv] = fast_pyra_down(pyrs_ref[pyr_lv - 1]);
  }

  for (const cv::KeyPoint& ref_kp : ref_keypoints) {
    int level_cur = 0;
    float depth = -1.f;
    Vector2f px_cur, px_ref(ref_kp.pt.x, ref_kp.pt.y);
    Vector3f f_ref;
    if (!cam_ref_->backProject(px_ref, &f_ref)) {
      continue;
    }

    // Visualization (re-draw for every keypoint)
    if (draw_debug) {
      dbg_img_.create(mask_cur_.rows * 2, mask_cur_.cols, CV_8UC3);
      dbg_ref_ = dbg_img_(cv::Rect(0, 0, mask_cur_.cols, mask_cur_.rows));
      dbg_cur_ =
          dbg_img_(cv::Rect(0, mask_cur_.rows, mask_cur_.cols, mask_cur_.rows));
      dbg_cur_ptr_ = &dbg_cur_;
      cv::cvtColor(ref_img, dbg_ref_, CV_GRAY2RGB);
      cv::cvtColor(cur_img, dbg_cur_, CV_GRAY2RGB);
      cv::circle(dbg_ref_, ref_kp.pt, 2, cv::Scalar(0, 255, 0));
      cv::putText(dbg_ref_, boost::lexical_cast<std::string>(ref_kp.class_id),
                  ref_kp.pt, cv::FONT_HERSHEY_SIMPLEX, 0.5,
                  cv::Scalar(0, 255, 0), 1);
      for (int i = 0; i < mask_cur_.rows; ++i) {
        for (int j = 0; j < mask_cur_.cols; ++j) {
          if (mask_cur_(i, j) == 0x00) {
            dbg_cur_.at<cv::Vec3b>(i, j)[0] = 0;
            dbg_cur_.at<cv::Vec3b>(i, j)[1] = 0;
          }
        }
      }
    }
#ifndef __FEATURE_UTILS_NO_DEBUG__
    VLOG(1) << "findEpipolarMatchDirect for of_id = " << ref_kp.class_id;
#endif
    if (!direct_matcher_.findEpipolarMatchDirect(
            *cam_ref_, *cam_cur_, px_ref, f_ref, R_cur_ref_, t_cur_ref_,
            ref_kp.octave,  // double check here
            d_estimate, d_min, d_max, pyrs_ref, pyrs_cur, mask_cur_,
            false, /*edgelet_feature, not supported yet*/
            &px_cur, &depth, &level_cur, dbg_cur_ptr_)) {
#ifndef __FEATURE_UTILS_NO_DEBUG__
      if (draw_debug) {
        cv::imwrite("/tmp/per_feat_dbg/det_" +
                        boost::lexical_cast<std::string>(ref_kp.class_id) +
                        ".png",
                    dbg_img_);
      }
      VLOG(1) << "findEpipolarMatchDirect fails for of_id: " << ref_kp.class_id;
#endif
      continue;
    }
#ifndef __FEATURE_UTILS_NO_DEBUG__
    if (draw_debug) {
      cv::imwrite("/tmp/per_feat_dbg/det_" +
                      boost::lexical_cast<std::string>(ref_kp.class_id) +
                      ".png",
                  dbg_img_);
      VLOG(1) << "findEpipolarMatchDirect succeeds for of_id: "
              << ref_kp.class_id;
    }
#endif

    // check boundary condition (set to 20 pixels for computing ORB features)
    // [NOTE] Returned px_cur should be within mask_cur_ already
    const int orb_desc_margin = 20;
    if (px_cur(0) < orb_desc_margin ||
        px_cur(0) > mask_cur_.cols - orb_desc_margin ||
        px_cur(1) < orb_desc_margin ||
        px_cur(1) > mask_cur_.rows - orb_desc_margin) {
      continue;
    }

    cv::KeyPoint cur_kp = ref_kp;
    cur_kp.octave = level_cur;
    cur_kp.pt.x = px_cur(0);
    cur_kp.pt.y = px_cur(1);
    cur_keypoints->push_back(cur_kp);
  }

  if (cur_orb_features != nullptr && cur_keypoints->size() > 0) {
    cur_orb_features->create(cur_keypoints->size(), 32, CV_8U);
#ifdef __ARM_NEON__
    ORBextractor::computeDescriptorsN512(cur_img, *cur_keypoints,
                                         cur_orb_features);
#else
    ORBextractor::computeDescriptors(cur_img, *cur_keypoints, cur_orb_features);
#endif
  }
  return true;
}

}  // namespace XP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_NEXTPOW2_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_NEXTPOW2_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief nextpow2 generic tag

     Represents the nextpow2 function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct nextpow2_ : ext::elementwise_<nextpow2_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<nextpow2_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_nextpow2_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::nextpow2_, Site> dispatching_nextpow2_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::nextpow2_, Site>();
   }
   template<class... Args>
   struct impl_nextpow2_;
  }
  /*!
    Returns the least n such that abs(x) is less or equal to \f$2^n\f$

    @par Semantic:

    @code
    T r = nextpow2(a0);
    @endcode

    is similar to:

    @code
    T n = ceil(log2(abs(x)x));
    @endcode

    @param a0

    @return a value of same type as the input
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::nextpow2_, nextpow2, 1)
} }

#endif