/*=============================================================================
    Spirit v1.6.0
    Copyright (c) 2002-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    Permission to copy, use, modify, sell and distribute this software is
    granted provided this copyright notice appears in all copies. This
    software is provided "as is" without express or implied warranty, and
    with no claim as to its suitability for any purpose.
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A Roman Numerals Parser (demonstrating the symbol table)
//
//  [ JDG 8/22/2002 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/core.hpp>
#include <boost/spirit/symbols/symbols.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;

///////////////////////////////////////////////////////////////////////////////
//
//  Parse roman hundreds (100..900) numerals using the symbol table.
//  Notice that the data associated with each slot is passed
//  to attached semantic actions.
//
///////////////////////////////////////////////////////////////////////////////
struct hundreds : symbols<unsigned>
{
    hundreds()
    {
        add
            ("C"    , 100)
            ("CC"   , 200)
            ("CCC"  , 300)
            ("CD"   , 400)
            ("D"    , 500)
            ("DC"   , 600)
            ("DCC"  , 700)
            ("DCCC" , 800)
            ("CM"   , 900)
        ;
    }

} hundreds_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Parse roman tens (10..90) numerals using the symbol table.
//
///////////////////////////////////////////////////////////////////////////////
struct tens : symbols<unsigned>
{
    tens()
    {
        add
            ("X"    , 10)
            ("XX"   , 20)
            ("XXX"  , 30)
            ("XL"   , 40)
            ("L"    , 50)
            ("LX"   , 60)
            ("LXX"  , 70)
            ("LXXX" , 80)
            ("XC"   , 90)
        ;
    }

} tens_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Parse roman ones (1..9) numerals using the symbol table.
//
///////////////////////////////////////////////////////////////////////////////
struct ones : symbols<unsigned>
{
    ones()
    {
        add
            ("I"    , 1)
            ("II"   , 2)
            ("III"  , 3)
            ("IV"   , 4)
            ("V"    , 5)
            ("VI"   , 6)
            ("VII"  , 7)
            ("VIII" , 8)
            ("IX"   , 9)
        ;
    }

} ones_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Semantic actions
//
///////////////////////////////////////////////////////////////////////////////
struct add_1000
{
    add_1000(unsigned& r_) : r(r_) {}
    void operator()(char) const { r += 1000; }
    unsigned& r;
};

struct add_roman
{
    add_roman(unsigned& r_) : r(r_) {}
    void operator()(unsigned n) const { r += n; }
    unsigned& r;
};

///////////////////////////////////////////////////////////////////////////////
//
//  roman (numerals) grammar
//
///////////////////////////////////////////////////////////////////////////////
struct roman : public grammar<roman>
{
    template <typename ScannerT>
    struct definition
    {
        definition(roman const& self)
        {
            first
                =   +ch_p('M')  [add_1000(self.r)]
                ||  hundreds_p  [add_roman(self.r)]
                ||  tens_p      [add_roman(self.r)]
                ||  ones_p      [add_roman(self.r)];

            //  Note the use of the || operator. The expression
            //  a || b reads match a or b and in sequence. Try
            //  defining the roman numerals grammar in YACC or
            //  PCCTS. Spirit rules! :-)
        }

        rule<ScannerT> first;
        rule<ScannerT> const&
        start() const { return first; }
    };

    roman(unsigned& r_) : r(r_) {}
    unsigned& r;
};

///////////////////////////////////////////////////////////////////////////////
//
//  Main driver code
//
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tRoman Numerals Parser\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type a Roman Numeral ...or [q or Q] to quit\n\n";

    //  Start grammar definition

    string str;
    while (getline(cin, str))
    {
        if (str[0] == 'q' || str[0] == 'Q')
            break;

        unsigned n = 0;
        roman roman_p(n);
        if (parse(str.c_str(), roman_p).full)
        {
            cout << "parsing succeeded\n";
            cout << "result = " << n << "\n\n";
        }
        else
        {
            cout << "parsing failed\n\n";
        }
    }

    cout << "Bye... :-) \n\n";

    return 0;
}


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

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

#ifndef THREE_POINT_UTIL_HPP_
#define THREE_POINT_UTIL_HPP_

#include "ThreePoint.hpp"

#include <Eigen/Geometry>

#include <random>

namespace ThreePoint
{
template <typename FloatType, typename RNG>
void getRandomRotation(RNG &rng, FloatType *R, FloatType *q = NULL)
{
	// construct random rotation
	std::uniform_real_distribution<FloatType> urd(0, static_cast<FloatType>(2 * M_PI));
	FloatType rx = urd(rng);
	FloatType ry = urd(rng);
	FloatType rz = urd(rng);
	Eigen::Quaternion<FloatType> mq;
	mq = Eigen::AngleAxis<FloatType>(rz, Eigen::Matrix<FloatType, 3, 1>::UnitZ()) *
		Eigen::AngleAxis<FloatType>(ry, Eigen::Matrix<FloatType, 3, 1>::UnitY()) *
		Eigen::AngleAxis<FloatType>(rx, Eigen::Matrix<FloatType, 3, 1>::UnitX());

	Eigen::Map<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> > mR(R);
	mR = mq.toRotationMatrix();

	if (q != NULL)
	{
		q[0] = mq.x();
		q[1] = mq.y();
		q[2] = mq.z();
		q[3] = mq.w();
	}
}

template <typename FloatType, typename RNG>
void getRandomTranslation(RNG &rng, FloatType *t)
{
	// cosntruct random translation
	std::uniform_real_distribution<FloatType> urd(static_cast<FloatType>(-0.5), static_cast<FloatType>(0.5));
	t[0] = 5 + urd(rng);
	t[1] = 5 + urd(rng);
	t[2] = 5 + urd(rng);
}

template <typename FloatType, typename RNG>
inline void getRandomScale(RNG &rng, FloatType &s)
{
	std::uniform_real_distribution<FloatType> urd(static_cast<FloatType>(-0.05), static_cast<FloatType>(0.05));
	s = 1 + urd(rng);
}

template <typename FloatType, typename RNG>
void getRandomPoint(int n, RNG &rng, FloatType *X)
{
	// construct random point on unit shpere
	std::uniform_real_distribution<FloatType> urd(static_cast<FloatType>(-0.5), static_cast<FloatType>(0.5));
	for (int i = 0; i < 3 * n; i++)
	{
		X[i] = urd(rng);
	}
	Eigen::Map<Eigen::Matrix<FloatType, 3, Eigen::Dynamic> > mX(X, 3, n);
	mX.colwise().normalize();
}

template <typename FloatType>
void getRandomCorrespondences(int n, FloatType *R, FloatType *t, FloatType &s, std::vector<FloatType> &pts1, std::vector<FloatType> &pts2)
{
	std::random_device rd;
	std::mt19937 rng(rd());

	// construct random rotation
	getRandomRotation(rng, R);

	// construct random translation
	getRandomTranslation(rng, t);

	// random scale
	getRandomScale(rng, s);

	// construct random point on unit shpere
	pts1.resize(3 * n);
	getRandomPoint(n, rng, &(pts1[0]));

	// transform
	pts2.resize(3 * n);
	ThreePoint<FloatType>::transformPoints(n, s, R, t, &(pts1[0]), &(pts2[0]));
}

}

#endif // THREE_POINT_UTIL_HPP_


/**
   \file lbfgs_method.hpp
   \brief large bfgs method, used to perform large scale optimization
   \author Junhua Gu
 */

#ifndef LBFGS_METHOD
#define LBFGS_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cstdlib>
#include <core/opt_traits.hpp>
#include "../linmin/linmin.hpp"
#include <math/num_diff.hpp>
#include <cassert>
#include <cmath>
#include <ctime>
#include <vector>
#include <algorithm>
#include "lbfgs.h"
#include "lbfgs.cpp"
/*
 *
*/
#include <iostream>
using std::cerr;
using std::endl;

namespace opt_utilities
{

  template<typename rT,typename pT>
  lbfgsfloatval_t lbfgs_adapter(
				void *instance,
				const lbfgsfloatval_t *x,
				lbfgsfloatval_t *g,
				const int n,
				const lbfgsfloatval_t step
				)
  {
    pT px;
    resize(px,n);
    for(int i=0;i<n;++i)
      {
	set_element(px,i,x[i]);
      }

    lbfgsfloatval_t result=((func_obj<rT,pT>*)instance)->eval(px);
    pT grad(gradient(*static_cast<func_obj<rT,pT>*>(instance),px));
    for(int i=0;i<n;++i)
      {
	g[i]=get_element(grad,i);
      }
    return result;
  }

  template <typename rT,typename pT>
  class lbfgs_method
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;

    //typedef blitz::Array<rT,2> array2d_type;

  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    rT threshold;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

    const char* do_get_type_name()const
    {
      return "large scale bfgs";
    }
  public:
    lbfgs_method()
      :threshold(1e-4)
    {}

    virtual ~lbfgs_method()
    {
    };

    lbfgs_method(const lbfgs_method<rT,pT>& rhs)
      :p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       threshold(rhs.threshold)
    {
    }

    lbfgs_method<rT,pT>& operator=(const lbfgs_method<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      opt_eq(start_point,rhs.start_point);
      opt_eq(end_point,rhs.end_point);
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new lbfgs_method<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      start_point.resize(get_size(p));
      opt_eq(start_point,p);

    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      lbfgs_parameter_t param;
      lbfgs_parameter_init(&param);
      param.ftol=threshold;
      std::vector<lbfgsfloatval_t> buffer(get_size(start_point));
      for(int i=0;i<buffer.size();++i)
	{
	  buffer[i]=get_element(start_point,i);
	}
      lbfgsfloatval_t fx;
      lbfgs(get_size(start_point),&buffer[0],&fx,
	    lbfgs_adapter<rT,pT>,0,p_fo,&param);
      for(int i=0;i<buffer.size();++i)
	{
	  set_element(start_point,i,buffer[i]);
	}
      return start_point;
    }
  };

}

#endif
//EOF


#include "observable.hpp"

#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>

#undef NDEBUG
#include <Eigen/Dense>
#include <array>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>

#include "state.hpp"
#include "utility.hpp"

void HermitianQuantumOperator::add_operator(const PauliOperator* mpt) {
    if (std::abs(mpt->get_coef().imag()) > 0) {
        std::cerr << "Error: HermitianQuantumOperator::add_operator(const "
                     "PauliOperator* mpt): PauliOperator must be Hermitian."
                  << std::endl;
        return;
    }
    GeneralQuantumOperator::add_operator(mpt);
}

void HermitianQuantumOperator::add_operator(
    CPPCTYPE coef, std::string pauli_string) {
    if (std::abs(coef.imag()) > 0) {
        std::cerr << "Error: HermitianQuantumOperator::add_operator(const "
                     "PauliOperator* mpt): PauliOperator must be Hermitian."
                  << std::endl;
        return;
    }
    GeneralQuantumOperator::add_operator(coef, pauli_string);
}

CPPCTYPE HermitianQuantumOperator::get_expectation_value(
    const QuantumStateBase* state) const {
    return GeneralQuantumOperator::get_expectation_value(state).real();
}

CPPCTYPE
HermitianQuantumOperator::solve_ground_state_eigenvalue_by_lanczos_method(
    QuantumStateBase* init_state, const UINT iter_count,
    const CPPCTYPE mu) const {
    if (this->get_term_count() == 0) {
        std::cerr << "Error: "
                     "HermitianQuantumOperator::solve_ground_state_eigenvalue_"
                     "by_lanczos_method("
                     "QuantumStateBase * state, const UINT iter_count, const "
                     "CPPCTYPE mu): At least one PauliOperator is required.";
        return 0;
    }

    // Implemented based on
    // https://files.transtutors.com/cdn/uploadassignments/472339_1_-numerical-linear-aljebra.pdf
    CPPCTYPE mu_;
    if (mu == 0.0) {
        // mu is not changed from default value.
        mu_ = this->calculate_default_mu();
    } else {
        mu_ = mu;
    }

    const auto qubit_count = this->get_qubit_count();
    QuantumState tmp_state(qubit_count);
    QuantumState mu_timed_state(qubit_count);
    // work_states: [q_{i-1}, q_i, q_{i+1}]
    // q_0, q_1, q_2,... span Krylov subspace.
    init_state->normalize(init_state->get_squared_norm());
    std::array<QuantumState, 3> work_states = {QuantumState(qubit_count),
        QuantumState(qubit_count), QuantumState(qubit_count)};
    work_states.at(1).load(init_state);

    Eigen::VectorXd alpha_v(iter_count);
    Eigen::VectorXd beta_v(iter_count - 1);
    for (UINT i = 0; i < iter_count; i++) {
        // v = (A - μI) * q_i
        mu_timed_state.load(&work_states.at(1));
        mu_timed_state.multiply_coef(-mu_);
        this->apply_to_state(&tmp_state, work_states.at(1), &work_states.at(2));
        work_states.at(2).add_state(&mu_timed_state);

        // α_i = q_i^T * v
        alpha_v(i) =
            state::inner_product(&work_states.at(1), &work_states.at(2)).real();
        // In the last iteration, no need to calculate β.
        if (i == iter_count - 1) {
            break;
        }
        // v -= α_i * q_i
        tmp_state.load(&work_states.at(1));
        tmp_state.multiply_coef(-alpha_v(i));
        work_states.at(2).add_state(&tmp_state);
        if (i != 0) {
            // v -= β_{i-1} * q_{i-1}
            tmp_state.load(&work_states.at(0));
            tmp_state.multiply_coef(-beta_v(i - 1));
            work_states.at(2).add_state(&tmp_state);
        }

        // β_i = ||v||
        beta_v(i) = std::sqrt(work_states.at(2).get_squared_norm());
        // q_{i+1} = v / β_i
        work_states.at(2).multiply_coef(1 / beta_v(i));
        work_states.at(0).load(&work_states.at(1));
        work_states.at(1).load(&work_states.at(2));
    }

    // Compute eigenvalue of a symmetric matrix T whose diagonal elements are
    // `alpha_v` and subdiagonal elements are `beta_v`.
    Eigen::SelfAdjointEigenSolver<ComplexMatrix> solver;
    solver.computeFromTridiagonal(alpha_v, beta_v);
    const auto eigenvalues = solver.eigenvalues();
    // Find ground state eigenvalue.
    UINT minimum_eigenvalue_index = 0;
    auto minimum_eigenvalue = eigenvalues(0);
    for (UINT i = 0; i < eigenvalues.size(); i++) {
        if (eigenvalues(i) < minimum_eigenvalue) {
            minimum_eigenvalue_index = i;
            minimum_eigenvalue = eigenvalues(i);
        }
    }

    auto eigenvectors = solver.eigenvectors();
    auto eigenvector_in_krylov = eigenvectors.col(minimum_eigenvalue_index);
    // Store ground state eigenvector to `init_state`.
    // If λ is an eigenvalue of T and q is the eigenvector, Tq = λq.
    // And let V be a matrix whose column vectors span Krylov subspace.
    // Then, T = V^* AV where A is this observable.
    // Tq = λq, VV^* AVq = Vλq, A(Vq) = λ(Vq).
    // So, an eigenvector of A for λ is Vq.
    // q_0 = init_state
    work_states.at(1).load(init_state);
    init_state->multiply_coef(0.0);
    assert(eigenvector_in_krylov.size() == iter_count);
    for (UINT i = 0; i < iter_count; i++) {
        // q += v_i * q_i, where q is eigenvector to compute
        tmp_state.load(&work_states.at(1));
        tmp_state.multiply_coef(eigenvector_in_krylov(i));
        init_state->add_state(&tmp_state);

        // v = (A - μI) * q_i
        mu_timed_state.load(&work_states.at(1));
        mu_timed_state.multiply_coef(-mu_);
        this->apply_to_state(&tmp_state, work_states.at(1), &work_states.at(2));
        work_states.at(2).add_state(&mu_timed_state);
        if (i == iter_count - 1) {
            break;
        }

        // v -= α_i * q_i
        tmp_state.load(&work_states.at(1));
        tmp_state.multiply_coef(-alpha_v(i));
        work_states.at(2).add_state(&tmp_state);
        if (i != 0) {
            // v -= β_{i-1} * q_{i-1}
            tmp_state.load(&work_states.at(0));
            tmp_state.multiply_coef(-beta_v(i - 1));
            work_states.at(2).add_state(&tmp_state);
        }

        // q_{i+1} = v / β_i
        work_states.at(2).multiply_coef(1 / beta_v[i]);
        work_states.at(0).load(&work_states.at(1));
        work_states.at(1).load(&work_states.at(2));
    }

    return minimum_eigenvalue + mu_;
}

std::string HermitianQuantumOperator::to_string() const {
    std::stringstream os;
    auto term_count = this->get_term_count();
    for (UINT index = 0; index < term_count; index++) {
        os << this->get_term(index)->get_coef().real() << " ";
        os << this->get_term(index)->get_pauli_string();
        if (index != term_count - 1) {
            os << " + ";
        }
    }
    return os.str();
}

namespace observable {
HermitianQuantumOperator* create_observable_from_openfermion_file(
    std::string file_path) {
    UINT qubit_count = 0;
    std::vector<CPPCTYPE> coefs;
    std::vector<std::string> ops;

    std::ifstream ifs;
    ifs.open(file_path);

    if (!ifs) {
        std::cerr << "ERROR: Cannot open file" << std::endl;
        return NULL;
    }

    // loading lines and check qubit_count
    std::string str_buf;
    std::vector<std::string> index_list;

    std::string line;
    while (getline(ifs, line)) {
        std::tuple<double, double, std::string> parsed_items =
            parse_openfermion_line(line);
        const auto coef_real = std::get<0>(parsed_items);
        const auto coef_imag = std::get<1>(parsed_items);
        str_buf = std::get<2>(parsed_items);

        CPPCTYPE coef(coef_real, coef_imag);
        coefs.push_back(coef);
        ops.push_back(str_buf);
        index_list = split(str_buf, "IXYZ ");

        for (UINT i = 0; i < index_list.size(); ++i) {
            UINT n = std::stoi(index_list[i]) + 1;
            if (qubit_count < n) qubit_count = n;
        }
    }
    if (!ifs.eof()) {
        std::cerr << "ERROR: Invalid format" << std::endl;
        return NULL;
    }
    ifs.close();

    HermitianQuantumOperator* observable =
        new HermitianQuantumOperator(qubit_count);

    for (UINT i = 0; i < ops.size(); ++i) {
        observable->add_operator(new PauliOperator(ops[i].c_str(), coefs[i]));
    }

    return observable;
}

HermitianQuantumOperator* create_observable_from_openfermion_text(
    const std::string& text) {
    UINT qubit_count = 0;
    std::vector<CPPCTYPE> coefs;
    std::vector<std::string> ops;

    std::vector<std::string> lines;
    std::string str_buf;
    std::vector<std::string> index_list;

    lines = split(text, "\n");
    for (std::string line : lines) {
        std::tuple<double, double, std::string> parsed_items =
            parse_openfermion_line(line);
        const auto coef_real = std::get<0>(parsed_items);
        const auto coef_imag = std::get<1>(parsed_items);
        str_buf = std::get<2>(parsed_items);

        CPPCTYPE coef(coef_real, coef_imag);
        coefs.push_back(coef);
        ops.push_back(str_buf);
        index_list = split(str_buf, "IXYZ ");

        for (UINT i = 0; i < index_list.size(); ++i) {
            UINT n = std::stoi(index_list[i]) + 1;
            if (qubit_count < n) qubit_count = n;
        }
    }
    HermitianQuantumOperator* hermitian_quantum_operator =
        new HermitianQuantumOperator(qubit_count);

    for (UINT i = 0; i < ops.size(); ++i) {
        hermitian_quantum_operator->add_operator(
            new PauliOperator(ops[i].c_str(), coefs[i]));
    }

    return hermitian_quantum_operator;
}

std::pair<HermitianQuantumOperator*, HermitianQuantumOperator*>
create_split_observable(std::string file_path) {
    UINT qubit_count = 0;
    std::vector<CPPCTYPE> coefs;
    std::vector<std::string> ops;

    std::ifstream ifs;
    ifs.open(file_path);

    if (!ifs) {
        std::cerr << "ERROR: Cannot open file" << std::endl;
        return std::make_pair(
            (HermitianQuantumOperator*)NULL, (HermitianQuantumOperator*)NULL);
    }

    // loading lines and check qubit_count
    std::string str_buf;
    std::vector<std::string> index_list;

    std::string line;
    while (getline(ifs, line)) {
        std::tuple<double, double, std::string> parsed_items =
            parse_openfermion_line(line);
        const auto coef_real = std::get<0>(parsed_items);
        const auto coef_imag = std::get<1>(parsed_items);
        str_buf = std::get<2>(parsed_items);

        CPPCTYPE coef(coef_real, coef_imag);
        coefs.push_back(coef);
        ops.push_back(str_buf);
        index_list = split(str_buf, "IXYZ ");

        for (UINT i = 0; i < index_list.size(); ++i) {
            UINT n = std::stoi(index_list[i]) + 1;
            if (qubit_count < n) qubit_count = n;
        }
    }
    if (!ifs.eof()) {
        std::cerr << "ERROR: Invalid format" << std::endl;
        return std::make_pair(
            (HermitianQuantumOperator*)NULL, (HermitianQuantumOperator*)NULL);
    }
    ifs.close();

    HermitianQuantumOperator* observable_diag =
        new HermitianQuantumOperator(qubit_count);
    HermitianQuantumOperator* observable_non_diag =
        new HermitianQuantumOperator(qubit_count);

    for (UINT i = 0; i < ops.size(); ++i) {
        if (ops[i].find("X") != std::string::npos ||
            ops[i].find("Y") != std::string::npos) {
            observable_non_diag->add_operator(
                new PauliOperator(ops[i].c_str(), coefs[i]));
        } else {
            observable_diag->add_operator(
                new PauliOperator(ops[i].c_str(), coefs[i]));
        }
    }

    return std::make_pair(observable_diag, observable_non_diag);
}
}  // namespace observable


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson, John B. Mains
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__NLP_HPP_
#define SMOOTH__FEEDBACK__NLP_HPP_

/**
 * @file
 * @brief Nonlinear program definition.
 */

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <smooth/diff.hpp>

#include <limits>

#include "collocation.hpp"

namespace smooth::feedback {

/**
 * @brief Nonlinear Programming Problem
 * \f[
 *  \begin{cases}
 *   \min_{x}    & f(x)                    \\
 *   \text{s.t.} & x_l \leq x \leq x_u     \\
 *               & g_l \leq g(x) \leq g_u
 *  \end{cases}
 * \f]
 * for \f$ f : \mathbb{R}^n \rightarrow \mathbb{R} \f$ and
 * \f$ g : \mathbb{R}^n \rightarrow \mathbb{R}^m \f$.
 */
struct NLP
{
  /// @brief Number of variables
  std::size_t n;

  /// @brief Number of constraints
  std::size_t m;

  /// @brief Objective function (R^n -> R)
  std::function<double(Eigen::VectorXd)> f;

  /// @brief Variable bounds (R^n)
  Eigen::VectorXd xl, xu;

  /// @brief Constraint function (R^n -> R^m)
  std::function<Eigen::VectorXd(Eigen::VectorXd)> g;

  /// @brief Constaint bounds (R^m)
  Eigen::VectorXd gl, gu;

  /// @brief Jacobian of objective function (R^n -> R^{n x n})
  std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd)> df_dx;

  /// @brief Jacobian of constraint function (R^n -> R^{m x n})
  std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd)> dg_dx;

  /// @brief Hessian of objective function (R^n -> R^{n x n}) [optional]
  std::optional<std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd, Eigen::VectorXd)>>
    d2f_dx2 = std::nullopt;

  /**
   * @brief Projected Hessian of constraint function (R^m, R^n -> R^{n x n}) [optional]
   *
   * Should return the derivative
   * \f[
   *  H_g(\lambda, x) = \nabla^2_x \lambda^T g(x), \quad \lambda \in \mathbb{R}^m, x \in
   * \mathbb{R}^n \f]
   */
  std::optional<std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd, Eigen::VectorXd)>>
    d2g_dx2 = std::nullopt;
};

struct NLPSolution
{
  /// @brief Solver status
  enum class Status {
    Optimal,
    PrimalInfeasible,
    DualInfeasible,
    MaxIterations,
    MaxTime,
    Unknown,
  } status;

  /// @brief Number of iterations
  std::size_t iter{0};

  /// @brief Variable values
  Eigen::VectorXd x;

  /// @brief Inequality multipliers
  Eigen::VectorXd zl, zu;

  /// @brief Constraint multipliers
  Eigen::VectorXd lambda;

  /// @brief Objective
  double objective{0};
};

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__NLP_HPP_


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <helper_functions.h>
#include <nvidia/helper_cuda.h>

//#include <mmf/defines.h>

using namespace Eigen;
using std::min;
using std::max;
using std::cout;
using std::endl;

template<typename T>
class OptSO3ApproxCpu
{
  public:
  OptSO3ApproxCpu(T t_max = 5.0, T dt = 0.05)
    : t_max_(t_max), dt_(dt), t_(0)
  {
    R_ = Matrix<T,3,3>::Identity();
  };

  virtual ~OptSO3ApproxCpu()
  { };

//  virtual T conjugateGradient(Matrix<T,3,3>& R, uint32_t maxIter=100);
  virtual T conjugateGradient(Matrix<T,3,3>& R,
      const Matrix<T,Dynamic,Dynamic>& qKarch,
      const Matrix<T,Dynamic,1>& Ns,
      uint32_t maxIter=100);

  /* return a skew symmetric matrix from A */
  Matrix<T,3,3> enforceSkewSymmetry(const Matrix<T,3,3> &A) const
  {return 0.5*(A-A.transpose());};

  const Matrix<T,3,3>& R() const {return R_;};
  /* matrix of the 6 directions of the MF */
  Matrix<T,Dynamic,Dynamic> M() const;

  static Matrix<T,Dynamic,Dynamic> Rot2M(const Matrix<T,3,3>& R);

protected:
  T t_max_, dt_;
  uint32_t t_; // timestep
  Matrix<T,3,3> R_; // previous rotation
  Matrix<T,3,6> qKarch_; // karcher means for all axes
  Matrix<T,1,6> Ns_; // number of normals for each axis

  virtual void conjugateGradientPostparation_impl(Matrix<T,3,3>& R);
  virtual T conjugateGradient_impl(Matrix<T,3,3>& R, T res0,
      uint32_t maxIter=100);
  /*
   * evaluate cost function for a given assignment of npormals to axes
   */
  virtual T evalCostFunction(Matrix<T,3,3>& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix<T,3,3>&J, Matrix<T,3,3>& R);

  /*
   * updates G and H from rotation R and jacobian J
   */
  virtual void updateGandH(Matrix<T,3,3>& G, Matrix<T,3,3>& G_prev,
      Matrix<T,3,3>& H, const Matrix<T,3,3>& R, const Matrix<T,3,3>& J,
      const Matrix<T,3,3>& M_t_min, bool resetH);
  /*
   * performs line search starting at R in direction of H returns min
   * of cost function and updates R, and M_t_min
   */
  virtual T linesearch(Matrix<T,3,3>& R, Matrix<T,3,3>& M_t_min, const
      Matrix<T,3,3>& H, T t_max=1.0, T dt=0.1);
  /* convert a Rotation matrix R to a MF representaiton of the axes */
  void Rot2M(Matrix<T,3,3>& R, T *mu);
};


#ifndef HAMILTONIANS_KITAEVHEX_HPP
#define HAMILTONIANS_KITAEVHEX_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class KitaevHex
{
private:
	const int n_;
	const int m_;
	const double Jx_;
	const double Jy_;
	const double Jz_;

	const double h_;

public:

	KitaevHex(int n, int m, double J, double h = 0.0)
		: n_(n), m_(m), Jx_(J), Jy_(J), Jz_(J), h_(h)
	{
		assert(n % 2 == 0);
		assert(m % 2 == 0);
	}

	KitaevHex(int n, int m, double Jx, double Jy, double Jz)
		: n_(n), m_(m), Jx_(Jx), Jy_(Jy), Jz_(Jz), h_{0.0}
	{
		assert(n % 2 == 0);
		assert(m % 2 == 0);
	}

	int blackIdx(int row, int col) const
	{
		const int k = n_/2;
		row = ((row%m_) + m_) % m_;
		col = ((col%k) + k) % k;
		return row*n_ + col;
	}
	int whiteIdx(int row, int col) const
	{
		const int k = n_/2;
		row = ((row%m_) + m_) % m_;
		col = ((col%k) + k) % k;
		return row*n_ + col + (n_/2);
	}

	std::vector<std::pair<int,int> > xLinks() const
	{
		std::vector<std::pair<int,int> > res;
		const int k = n_/2;
		for(int row = 0; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row, i), whiteIdx(row, i-1));
			}
		}
		for(int row = 1; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row,i), whiteIdx(row,i));
			}
		}
		return res;
	}

	std::vector<std::pair<int,int> > yLinks() const
	{
		std::vector<std::pair<int,int> > res;
		const int k = n_/2;
		for(int row = 0; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row, i), whiteIdx(row,i));
			}
		}
		for(int row = 1; row < m_; row += 2)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(blackIdx(row, i), whiteIdx(row,i+1));
			}
		}

		return res;
	}

	std::vector<std::pair<int,int> > zLinks() const
	{
		std::vector<std::pair<int,int> > res;
		const int k = n_/2;
		for(int row = 0; row < m_; row++)
		{
			for(int i = 0; i < k; i++)
			{
				res.emplace_back(whiteIdx(row, i), blackIdx(row+1,i));
			}
		}
		return res;
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "KITAEVHEX"},
			{"Jx", Jx_},
			{"Jy", Jy_},
			{"Jz", Jz_},
			{"h", h_},
			{"n", n_},
			{"m", m_},
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		constexpr std::complex<double> I(0.,1.);

		for(int i = 0; i < n_*m_; ++i)
		{
			//s += h_*smp.sigmaAt(i); //hz
			s += h_*smp.ratio(i); //hx
		}

		for(auto &xx: xLinks())
		{
			s += Jx_*smp.ratio(xx.first, xx.second); //xx
		}
		for(auto &yy: yLinks())
		{
			int zzval = smp.sigmaAt(yy.first)*smp.sigmaAt(yy.second);
			s += -Jy_*zzval*smp.ratio(yy.first, yy.second); //yy
		}
		for(auto &zz: zLinks())
		{
			int zzval = smp.sigmaAt(zz.first)*smp.sigmaAt(zz.second);
			s += Jz_*zzval; //yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;

		for(int i = 0; i < n_*m_; ++i)
		{
			double k = (1-2*int((col >> i) & 1));
			//m[col] += h_*k; //hz
			m[col ^ (1<<i)] += h_; //hx
		}

		for(auto &xx: xLinks())
		{
			int t = (1 << xx.first) | (1 << xx.second);
			m[col ^ t] += Jx_; //xx
		}
		for(auto &yy: yLinks())
		{
			int zzval = (1-2*int((col >> yy.first) & 1))*(1-2*int((col >> yy.second) & 1));
			int t = (1 << yy.first) | (1 << yy.second);
			m[col ^ t] += -Jy_*zzval; //yy
		}
		for(auto &zz: zLinks())
		{
			int zzval = (1-2*int((col >> zz.first) & 1))*(1-2*int((col >> zz.second) & 1));
			m[col] += Jz_*zzval; //zz
		}

		return m;
	}
};
#endif//HAMILTONIANS_KITAEVHEX_HPP


#ifndef TURBOTRACK_HPP
#define TURBOTRACK_HPP

#if defined TURBOTRACK_USE_EIGEN
	#include <Eigen/Dense>

	namespace turbotrack {
		using vec2 = Eigen::Vector2f;
		using vec3 = Eigen::Vector3f;
		using quat = Eigen::Quaternionf;
	}
#elif defined TURBOTRACK_USE_GLM
	#define GLM_ENABLE_EXPERIMENTAL
	#include <glm/ext.hpp>
	#include <glm/glm.hpp>
	#include <glm/gtc/quaternion.hpp>

	namespace turbotrack {
		using vec2 = glm::vec2;
		using vec3 = glm::vec3;
		using quat = glm::quat;
	}
#endif

namespace turbotrack {

enum class TrackballType {
	// chen_et_al, not implemented yet
	shoemake,
	holroyd
};

// See equation 33 in Henriksen et al.
vec3 shoemake_projection(const vec2 &mouse, float radius);

// See equation 46 in Henriksen et al.
vec3 holroyd_projection(const vec2 &mouse, float radius);

quat mouse_move(const vec2 &old_pos,
                const vec2 &new_pos, float radius = 1.0,
                TrackballType type = TrackballType::holroyd);

} // namespace turbotrack

#endif // TURBOTRACK_HPP


// include statements
#include <iostream>
#include <cstddef>
#include <cstdlib>
#include <ctime>
#include <limits>
#include <cmath>
#include <random>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include "matplotlibcpp.h"

// Node definiton
struct Node{
    float x;
    float y;
    Node *parent;
};

//array of all nodes
Node node_list[1000];

// counter to keep track of no of elements in node_list
int counter=0;

// for finding the final path
Node location[500];

// for back propagation
int backprop = 0;

//Obstcale Points
//change point to change the location of obstacle
int obstacle_array[][2]={{4,4}, {5, 4}, {5, 6}, {6, 6}, {6,7}, {5, 7}, {5,8}, {4,8}, {4,7}, {3,7}, {3,6}, {4,6}};

//no of elements in obstacle array
int obstacle_size = sizeof(obstacle_array)/sizeof(obstacle_array[0]);

//  sampling probablity to set rate of sampling
float sampling_probablity;
// variable for sampling radius
float sampling_radius;

// function to sample random points, the rate of sampling is biased by using a sampling probablity
Node sampler(int goal_x, int goal_y, float prob)
{
    std::random_device rd; //Will be used to obtain a seed for the random number engine
    std::mt19937 generator(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<float> distribution_biased(0.0, 1.0);
    std::uniform_real_distribution<float> distribution_x(0.0, 15.0);
	std::uniform_real_distribution<float> distribution_y(0.0, 15.0);
    float random_prob = distribution_biased(generator);
    Node sample_node={};
    if(random_prob>prob)
    {
        sample_node.x= distribution_x(generator);
        sample_node.y= distribution_y(generator);
        sample_node.parent= NULL;
    }
    else
    {
        sample_node.x= goal_x;
        sample_node.y= goal_y;
        sample_node.parent= NULL;
    }

    return sample_node;
}

//finds distance between two Node's
float distance(Node node1,Node node2)
{
    float dist=sqrt(((node1.x-node2.x)*(node1.x-node2.x))+((node1.y-node2.y)*(node1.y-node2.y)));
    return dist;
}

//function to find the nearest node
int find_nearest(Node current)
{
    int nearest_node;
    float minimum_dist= 20;
    for(int i=0;i<counter;i++)
    {
        float dist=distance(current,node_list[i]);
        if(dist<minimum_dist)
        {
            minimum_dist=dist;
            nearest_node=i;
        }
    }

    return nearest_node;

}

//checks intersection between obstacles and path
bool check_collision(Node end1, Node end2)
{

    typedef boost::geometry::model::d2::point_xy<double> point_xy;
    typedef boost::geometry::model::polygon<point_xy> polygon_t;
    typedef boost::geometry::model::linestring<point_xy> linestring_t;

    polygon_t obstacle;
    linestring_t ls1;

    std::vector< point_xy > pointls;

    point_xy point1(end1.x , end1.y);
    pointls.push_back( point1 );

    point_xy point2(end2.x , end2.y);
    pointls.push_back( point2 );

    boost::geometry::assign_points( ls1, pointls );

    std::vector< point_xy > ObstaclePoints;

    for(int i=0;i<obstacle_size;i++)
    {
        point_xy point(obstacle_array[i][0] ,obstacle_array[i][1]);
        ObstaclePoints.push_back( point );
    }
    boost::geometry::assign_points( obstacle, ObstaclePoints);
    bool collision= boost::geometry::intersects(ls1, obstacle);

    return collision;
}
//main planner
bool planner(float start_x, float start_y, float goal_x, float goal_y, float sampling_radius)
{
    //variable to see if goal has been reached
    bool flag = 1;

    // defining the start as a Node
    Node start={start_x,start_y};
    Node goal={goal_x,goal_y};
    Node sample_node={};
    int nearest_node;

    //add start to the node_list
    node_list[counter]=start;
    counter++;

    for (int j=0; j<500;)
    {
        //samples a random point
        sample_node = sampler(goal_x,goal_y,sampling_probablity);

        // finds the nearest node to the sampled point
        nearest_node = find_nearest(sample_node);
        float dist = distance(sample_node,node_list[nearest_node]);

        // checking if tghe sampled node lies inside the sampling radius
        if (dist > sampling_radius)
        {
         //computing the angle b/w near and rand wrt x-axis
         float angle = atan2(sample_node.y - node_list[nearest_node].y,sample_node.x - node_list[nearest_node].x);

        // modifies sample_node in the direction of nearest node 0.5 distance from it
        sample_node.x=  node_list[nearest_node].x + sampling_radius*cos(angle);
        sample_node.y=  node_list[nearest_node].y + sampling_radius*sin(angle);
        }

        //obstacle checking
        bool collision = check_collision(sample_node, node_list[nearest_node]);
        if(collision==1)
        {
            continue;
        }
        else
        {
            sample_node.parent= &node_list[nearest_node];
            node_list[counter]= sample_node;
            counter++;
            j++;

        }

        float goal_dist=distance(sample_node,goal);
        // to check if we have reached the goal, tolerance of 0.1 is considered
        if(goal_dist<=0.1)
        {
            goal.parent= &sample_node;
            node_list[counter]=goal;
            counter++;
            flag = 0;
            //Goal Reached
            break;
        }
    }
    if(flag==1)
    {
        //Goal not found
        return 0;
    }

    //back propogation to find the final path from goal to start
    Node *current= &goal;
    while(current->parent != NULL)
    {
        location[backprop]=*current;
        backprop++;
        current=current->parent;
    }

    // to add the starting node
    location[backprop]=start;
    backprop++;

    return 1;
}

//plotting function
void plotting(bool sucess)
{
    namespace plt = matplotlibcpp;
    //plot obstacle
    std::vector<double> obstacle_x = {};
    std::vector<double> obstacle_y = {};

    for(int i=0;i<obstacle_size;i++)
    {
        obstacle_x.push_back(obstacle_array[i][0]);
        obstacle_y.push_back(obstacle_array[i][1]);
    }
    obstacle_x.push_back(obstacle_array[0][0]);
    obstacle_y.push_back(obstacle_array[0][1]);

    plt::plot(obstacle_x, obstacle_y,"k");

    //plotiing nodes
    std::vector<double> scatter_x = {};
    std::vector<double> scatter_y = {};

    for(int i=0;i<counter;i++)
    {

            scatter_x.push_back(node_list[i].x);
            scatter_y.push_back(node_list[i].y);

    }

    plt::plot(scatter_x, scatter_y,".m");

    //plotting tree
    for(int i=0;i<counter;i++)
    {
        if(node_list[i].parent != NULL)
        {
            std::vector<double> tree_x = {};
            std::vector<double> tree_y = {};
            tree_x.push_back(node_list[i].x);
            tree_y.push_back(node_list[i].y);
            tree_x.push_back(node_list[i].parent->x);
            tree_y.push_back(node_list[i].parent->y);
            plt::plot(tree_x, tree_y,"m");
        }
    }

    if(sucess==1)
    {
        //plot final path from start to goal
        int i = 0;
        while(i<backprop)
        {
            if(location[i].parent != NULL)
            {
                std::vector<double> final_x = {};
                std::vector<double> final_y = {};
                final_x.push_back(location[i].x);
                final_y.push_back(location[i].y);
                final_x.push_back(location[i].parent->x);
                final_y.push_back(location[i].parent->y);
                plt::plot(final_x, final_y,".b-");
            }
            i++;
        }
    }
    // plotting start and goal
    std::vector<double> starting_x = {};
    std::vector<double> starting_y = {};
    starting_x.push_back(location[0].x);
    starting_y.push_back(location[0].y);
    plt::plot(starting_x,starting_y,".k");
    std::vector<double> ending_x = {};
    std::vector<double> ending_y = {};
    ending_x.push_back(location[backprop].x);
    ending_y.push_back(location[backprop].y);
    plt::plot(ending_x,ending_y,".k");
    plt::grid(true);
    plt::show();

}
using namespace std;
//starting of the main function
int main()
{   float start_x, start_y, goal_x, goal_y;

    // taking inputs from the user
    cout<<"Enter sampling probablity : ";
    cin>> sampling_probablity;

    cout<<"Enter sampling radius (prefered to be less than 1): ";
    cin>> sampling_radius;

    cout<<"Enter x postion of start : ";
    cin>> start_x;

    cout<<"Enter y postion of start : ";
    cin>> start_y;

    cout<<"Enter x postion of goal: ";
    cin>> goal_x;

    cout<<"Enter y postion of goal : ";
    cin>> goal_y;

    //calling planner
    bool sucesss = planner(start_x,start_y,goal_x,goal_y,sampling_radius);
    //calling plotting
    plotting(sucesss);

    if(sucesss)
    {
        std::cout<<"goal reached"<<"\n";
    }
    else
    {
        std::cout<<"goal not reached";
    }

    std::cout<<"Total no of nodes = "<<counter<<"\n"<<"No of nodes in final path = "<<backprop<<"\n";

    return 0;
}


/*
 * AttitudeMagCalib.cpp
 *
 *  Copyright (c) 2013 Gareth Cross. All rights reserved.
 *
 *  This file is part of kr_attitude_eskf.
 *
 *	Created on: 23/06/2014
 *		  Author: gareth
 */

#include <kr_attitude_eskf/AttitudeMagCalib.hpp>
#include <Eigen/Cholesky>
#include <cmath>

using namespace Eigen;

namespace kr {

AttitudeMagCalib::AttitudeMagCalib() { reset(); }

void AttitudeMagCalib::reset() {
  binH_.clear();
  binV_.clear();
  calibrated_ = false;
  bias_.setZero();
  scale_.setOnes();
}

void AttitudeMagCalib::appendSample(const quat &att,
                                    const vec3 &field) {
  SampleBin bin;
  bin.field = field;
  bin.q = att;

  const vec3 localG = att.conjugate().matrix() * vec3(0,0,1);

  //  determine local angle
  if (std::abs(localG[2]) < 0.1) {
    //  world vertical is approx. in the local X/Y plane
    const vec3 worldZ = att.matrix() * vec3(0,0,1);
    const scalar_t ang = std::atan2(worldZ[1], worldZ[0]);
    const int key = (ang + M_PI) / (2*M_PI) * kBinMaxCount;
    binV_[key] = bin;
  } else if (std::abs(localG[2]) > 0.9) {
    //  world vertical is approx. vertical
    const vec3 worldX = att.matrix() * vec3(1,0,0);
    const scalar_t ang = std::atan2(worldX[1], worldX[0]);
    const int key = (ang + M_PI) / (2*M_PI) * kBinMaxCount;
    binH_[key] = bin;
  }
}

bool AttitudeMagCalib::isReady() const {
  if (binV_.size() < kBinMaxCount*8/10) {
    return false;
  }
  if (binH_.size() < kBinMaxCount*8/10) {
    return false;
  }
  return true;
}

bool AttitudeMagCalib::isCalibrated() const { return calibrated_; }

void AttitudeMagCalib::calibrate(AttitudeMagCalib::CalibrationType type) {

  if (!isReady()) {
    throw insufficient_data();
  }

  if (type == AttitudeMagCalib::FullCalibration) {
    //  perform full estimation of bias and scale
    vec3 bias(0,0,0), scl(1,1,1);

    std::vector<vec3> meas;
    for (const std::pair<int, SampleBin>& s : binH_) {
      meas.push_back(s.second.field);
    }
    for (const std::pair<int, SampleBin>&s : binV_) {
      meas.push_back(s.second.field);
    }
    const size_t N = meas.size();

    //  fit to sphere
    Matrix<scalar_t,Eigen::Dynamic,Eigen::Dynamic> A(N, 4);
    Matrix<scalar_t,Eigen::Dynamic,1> b(N, 1);
    for (size_t i=0; i < meas.size(); i++) {
      A(i,0) = 2*meas[i][0];
      A(i,1) = 2*meas[i][1];
      A(i,2) = 2*meas[i][2];
      A(i,3) = 1;
      b(i,0) = meas[i][0]*meas[i][0] + meas[i][1]*meas[i][1] +
          meas[i][2]*meas[i][2];
    }
    //  solve system for center
    const Matrix<scalar_t,4,1> x = A.colPivHouseholderQr().solve(b);
    bias = x.block<3,1>(0,0);

    scalar_t mean_rad = 0.0, mean_rad_sqr = 0.0;
    //  calculate estimate of mean radius
    for (const vec3 &v : meas) {
      const scalar_t r = (v - bias).norm();
      mean_rad += r;
      mean_rad_sqr += r * r;
    }
    mean_rad /= N;
    mean_rad_sqr /= N;

    //  refine with GN-NLS
    Matrix<scalar_t, Eigen::Dynamic, 6> J(N, 6);
    Matrix<scalar_t, Eigen::Dynamic, 1> r(N, 1);
    Matrix<scalar_t, Eigen::Dynamic, Eigen::Dynamic> W(N,N);
    W.setZero();

    for (int iter = 0; iter < 20; iter++) {
      for (size_t i = 0; i < meas.size(); i++) {
        const scalar_t x = (meas[i][0] - bias[0]) / scl[0];
        const scalar_t y = (meas[i][1] - bias[1]) / scl[1];
        const scalar_t z = (meas[i][2] - bias[2]) / scl[2];
        const scalar_t r2 = x*x + y*y + z*z;

        //  residual
        r(i,0) = mean_rad*mean_rad - r2;
        //  jacobian
        J(i,0) = -2 * x * x / scl[0];
        J(i,1) = -2 * y * y / scl[1];
        J(i,2) = -2 * z * z / scl[2];
        J(i,3) = -2 * x / scl[0];
        J(i,4) = -2 * y / scl[1];
        J(i,5) = -2 * z / scl[2];
      }

      //  calculate mean squared error
      scalar_t sigmaSquared=0;
      for (int j=0; j < r.rows(); j++) {
        sigmaSquared += r(j,0)*r(j,0);
      }
      sigmaSquared /= r.rows();
      //  calculate cauchy weights
      for (int j=0; j < r.rows(); j++) {
        const scalar_t errSqr = r(j,0)*r(j,0);
        W(j,j) = 1.0 / (1 + errSqr/sigmaSquared);
      }

      Matrix<scalar_t, 6, 6> H = J.transpose() * W * J;
      for (int i = 0; i < 3; i++) {  //  prior on scale
        H(i,i) *= 1.01;
      }
      Eigen::LDLT<Matrix<scalar_t,6,6>> LDLT(H);
      const Matrix<scalar_t, 6, 1> update = LDLT.solve(J.transpose() * W * r);

      scl += update.block<3, 1>(0, 0);
      bias += update.block<3, 1>(3, 0);
    }

    bias_ = bias;
    scale_ = scl;
  } else {
    bias_.setZero();
    scale_.setOnes();
  }

  calibrated_ = true;
}

} //  namespace kr


#include "kalman_filter.hpp"
#include "kalman_filter_cache.hpp"
#include "samplers.hpp"
#include "sampling/simple.hpp"

#include <boost/foreach.hpp>
#include <boost/random.hpp>
//#include <boost/random/exponential_distribution.hpp>
#include <boost/shared_ptr.hpp>
#include <cmath>
#include <ctime>
#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <limits>
#include <cmath>

extern "C" {
#include "third-party/gen_beta.h"
}

#include "detail/random.hpp"

namespace biggles
{

void sample_birth_rate_given_partition_orig(const partition& part_sample, model::parameters& params) {
    off_t n_total_born(part_sample.tracks().size());
    model::mean_new_tracks_per_frame(params) = sampling::sample_gamma(1.f + n_total_born, 1.f) /
        static_cast<float>(part_sample.duration());
    if (not std::isfinite(model::mean_new_tracks_per_frame(params))) {
        std::cerr << "duration = " << part_sample.duration() << std::endl;
        std::cerr << "number of tracks = " << n_total_born << std::endl;
        std::cerr << "sampled birth rate = " << model::mean_new_tracks_per_frame(params) << std::endl;
        BOOST_ASSERT(std::isfinite(model::mean_new_tracks_per_frame(params)));
    }
    //BOOST_ASSERT(model::mean_new_tracks_per_frame(params)>0.f);
    if (model::mean_new_tracks_per_frame(params) == 0.f)
        model::mean_new_tracks_per_frame(params) = std::numeric_limits<float>::min();
}

void sample_birth_rate_given_partition_alt(const partition& part_sample, model::parameters& params) {
    off_t n_total_born(part_sample.tracks().size());
    const size_t min_surv(1);
    model::mean_new_tracks_per_frame(params) = sampling::sample_gamma(1.f + n_total_born, 1.f) /
        static_cast<float>(part_sample.duration() - min_surv); // no birth in the last frames
    BOOST_ASSERT(std::isfinite(model::mean_new_tracks_per_frame(params)));
    BOOST_ASSERT(model::mean_new_tracks_per_frame(params)>0.f);
}

void sample_birth_rate_given_partition(const partition& part_sample, model::parameters& params) {
    sample_birth_rate_given_partition_orig(part_sample, params);
}

void sample_clutter_rate_given_partition(const partition& part_sample, model::parameters& params) {
    off_t n_total_false(part_sample.clutter().size());
    model::mean_false_observations_per_frame(params) = sampling::sample_gamma(1 + n_total_false, 1.f) /
        static_cast<float>(part_sample.duration());
    if (not std::isfinite(model::mean_false_observations_per_frame(params))) {
        std::cerr << "number of clutter = " << n_total_false << std::endl;
        std::cerr << "partition duration = " << part_sample.duration() << std::endl;
        std::cerr << "sampled clutter rate = " << model::mean_false_observations_per_frame(params) << std::endl;
        throw std::logic_error("clutter rate sampling returned a not finite value");
    }
    if (model::mean_false_observations_per_frame(params) == 0.f)
        model::mean_false_observations_per_frame(params) = std::numeric_limits<float>::min();
}

void sample_observation_probability_given_partition_orig(const partition& part_sample, model::parameters& params) {

    if (part_sample.tracks().empty()) {
        model::generate_observation_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }
    off_t n_total_generated(0);
    off_t sum_of_durations(0);
    // calculate sum_of_durations and n_total_generated
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        sum_of_durations += t_ptr->duration();
        // the track's size is the number of observations within it
        n_total_generated += t_ptr->size();
    }
    //BOOST_ASSERT(n_total_generated > 0); // there would be no tracks otherwise
    // the 1.0001s below should be 1 but the sampling functions for beta distributions hit an infinite loop if the sizes
    // are zero.
    // beta(1,1) is uniform distribution FIXME
    model::generate_observation_probability(params) =
        sampling::sample_beta(1.0001f + n_total_generated, 1.0001f + (sum_of_durations - n_total_generated));

}

void sample_observation_probability_given_partition_alt(const partition& part_sample, model::parameters& params) {

    if (part_sample.tracks().empty()) {
        model::generate_observation_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }
    off_t n_total_generated(0);
    off_t sum_of_durations(0);
    const size_t min_obs(2);
    // calculate sum_of_durations and n_total_generated
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        sum_of_durations += t_ptr->duration() - min_obs;
        n_total_generated += t_ptr->size() - min_obs;
    }
    BOOST_ASSERT(n_total_generated >= 0);
    BOOST_ASSERT(sum_of_durations >= n_total_generated);
    model::generate_observation_probability(params) =
        sampling::sample_beta(1.0001f + n_total_generated, 1.0001f + sum_of_durations - n_total_generated);
}

void sample_observation_probability_given_partition(const partition& part_sample, model::parameters& params) {
    sample_observation_probability_given_partition_orig(part_sample, params);
}

void sample_survival_probability_given_partition_orig(const partition& part_sample, model::parameters& params) {
    if(part_sample.tracks().empty()) {
        model::frame_to_frame_survival_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }

    off_t n_total_died(part_sample.tracks().size()); // everything that has a beginning has an end, Neo. Whatever.
    off_t n_total_survived(0);

    // calculate n_total_survived
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks())
    {
        // a track 'survives' for one fewer ticks than it's duration
        if(t_ptr->duration() >= 1)
            n_total_survived += t_ptr->duration() - 1;
    }
    //BOOST_ASSERT(n_total_survived > 0); // there would be no tracks otherwise
    //BOOST_ASSERT(n_total_died > 0); // there would be no tracks otherwise
    do model::frame_to_frame_survival_probability(params) = sampling::sample_beta(
            1.0001f + n_total_survived, 1.0001f + n_total_died);
    while (model::frame_to_frame_survival_probability(params) == 1.f);
}

void sample_survival_probability_given_partition_alt(const partition& part_sample, model::parameters& params) {
    if(part_sample.tracks().empty()) {
        model::frame_to_frame_survival_probability(params) = sampling::uniform_real(0.f, 1.f);
        return;
    }

    off_t n_total_died(0);
    off_t n_total_survived(0);

    const size_t min_surv(1);

    // calculate n_total_survived
    // a track 'survives' for one fewer ticks than it's duration and one survival is guaranteed
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, part_sample.tracks()) {
        n_total_survived += t_ptr->duration() - 1 - min_surv ;
        n_total_died += off_t(t_ptr->last_time_stamp() < part_sample.last_time_stamp());
    }
    BOOST_ASSERT(n_total_survived >= 0);
    BOOST_ASSERT(n_total_died >= 0);
    model::frame_to_frame_survival_probability(params) = sampling::sample_beta(1.0001f + n_total_survived, 1.0001f + n_total_died);
}

void sample_survival_probability_given_partition(const partition& part_sample, model::parameters& params) {
    sample_survival_probability_given_partition_orig(part_sample, params);
}

void sample_tracking_control_parameters_given_partition(const partition& partition, model::parameters& params) {
    sample_birth_rate_given_partition(partition, params);
    sample_clutter_rate_given_partition(partition, params);
    sample_observation_probability_given_partition(partition, params);
    sample_survival_probability_given_partition(partition, params);
}

void sample_observation_error_given_partition(const partition& partition, model::parameters& params) {
    // special case: no tracks. sample from a fairly all-encompassing distribution
    if (not is_symmetric(model::observation_error_covariance(params))) {
        std::cout << measure_asymmetry(model::observation_error_covariance(params)) << std::endl;
        throw std::runtime_error("R is not symmetric at the start");
    }

    if(partition.tracks().empty())
    {
        model::observation_error_covariance(params) = sampling::sample_inverse_wishart(Eigen::Matrix2f::Identity() * 2.f, 5);
        //model::observation_error_covariance(params) = Eigen::Matrix2f::Identity() * 0.09f; // FIXME PRIOR calculation
        return;
    }

    if (not is_symmetric(model::observation_error_covariance(params))) {
        std::cout << measure_asymmetry(model::observation_error_covariance(params)) << std::endl;
        throw std::runtime_error("R is not symmetric after track empty.");
    }

    // calculate Wishart Phi parameters for sampling R

    // this is the Wishart prior
    Eigen::Matrix2f Phi = Eigen::Matrix2f::Identity() * 2.f;
    size_t s = 5;

    if (not is_symmetric(Phi)) {
        std::cout << measure_asymmetry(Phi) << std::endl;
        throw std::runtime_error("Phi is not symmetric after init");
    }

    // update parameters for each track
    BOOST_FOREACH(const boost::shared_ptr<const track>& t_ptr, partition.tracks())
    {
        Phi += wishart_phi_parameter_for_track(t_ptr, params);
        s += t_ptr->size(); // number of observations in track
    }

    if (not is_symmetric(Phi)) {
        std::cout << measure_asymmetry(Phi) << std::endl;
        throw std::runtime_error("Phi is not symmetric after update");
    }

    model::observation_error_covariance(params) = sampling::sample_inverse_wishart(Phi, s);
    //model::observation_error_covariance(params) = Eigen::Matrix2f::Identity() * 0.09f; // FIXME PRIOR test
    if (not is_symmetric(model::observation_error_covariance(params))) {
        std::cout << measure_asymmetry(model::observation_error_covariance(params)) << std::endl;
        throw std::runtime_error("R is not symmetric after sampling.");
    }

}

void sample_model_parameters_given_partition(const partition& partition, model::parameters& params) {
    sample_tracking_control_parameters_given_partition(partition, params);
    /*
    model::mean_new_tracks_per_frame(params) = 0.2f; // FIXME PRIOR calculation
    model::mean_false_observations_per_frame(params) = 0.01f; // FIXME PRIOR calculation
    model::generate_observation_probability(params) = .9f; // FIXME PRIOR calculation
    model::frame_to_frame_survival_probability(params) = .9f; // FIXME PRIOR calculation
    */

    // sample constraint radius [removed]

    sample_observation_error_given_partition(partition, params);

}

Eigen::Matrix2f wishart_phi_parameter_for_track(const boost::shared_ptr<const track>& track_p, model::parameters& params)
{
    const track& track(*track_p);
    Eigen::Matrix2f Phi = Eigen::Matrix2f::Zero();
    BOOST_ASSERT(is_symmetric(Phi));

    // create a Kalman filter for the track
    static kalman_filter_cache kf_cache;
    const matrix2f& R(model::observation_error_covariance(params));
    const matrix4f& Q(model::process_noise_covariance(params));
    const kalman_filter& kf(kf_cache.get(track_p, R, Q));

    // ... and a list of smoothed states and covariances
    kalman_filter::states_and_cov_deque smoothed_states_and_covariances;

    // use a _front_ inserter since we generate results in reverse order
    rts_smooth(kf, std::front_inserter(smoothed_states_and_covariances));

    // the observation matrix
    Eigen::Matrix<float, 2, 4> B;
    B << 1, 0, 0, 0,
         0, 0, 1, 0;

    // iterate over state and covariances for the track. keep a track of which time stamp this is
    time_stamp time_stamp(track.first_time_stamp());
    track::const_iterator track_obs_it(track.begin()); // also start iterating over track observations
    BOOST_FOREACH(const kalman_filter::state_covariance_pair& state_and_cov, smoothed_states_and_covariances)
    {
        BOOST_ASSERT(time_stamp < track.last_time_stamp());

        // extract the state and covariance
        const Eigen::Vector4f& state(state_and_cov.first);
        const Eigen::Matrix4f& cov(state_and_cov.second);

        // do we have an associated observation?
        if((track_obs_it != track.end()) && (t(*track_obs_it) == time_stamp))
        {
            // extract observation
            const observation& obs(*track_obs_it);

            // sample state
            Eigen::Vector4f sampled_state(sampling::sample_multivariate_gaussian(state, cov));

            // sampled observation
            Eigen::Vector2f sampled_obs = B * sampled_state;

            // calculate delta
            Eigen::Vector2f obs_vector, delta;
            obs_vector << x(obs), y(obs);
            delta = obs_vector - sampled_obs;

            // update Wishart parameter
            Phi += delta * delta.transpose();
            if (not is_symmetric(Phi)) {
                OK1(cov);
                OK(time_stamp - track.first_time_stamp());
                OK1(Phi);
                OK1(obs_vector);
                OK1(sampled_obs);
                BOOST_ASSERT(is_symmetric(Phi));
            }

            ++track_obs_it;
        }

        ++time_stamp;
    }
    if (not is_symmetric(Phi)) {
        std::cout << "Phi = " << std::endl;
        std::cout << Phi << std::endl;
        throw std::runtime_error("Phi is not symmetric");
    }
    return Phi;
}

} // namespace biggles


#include <iostream>
#include <armadillo>

int main() {
    arma::vec v0(5, arma::fill::randn);
    arma::vec v = 1.0 / (1 + arma::exp(-v0));
    std::cout << v << std::endl;
}


#include "pose_regression.h"
#include <Eigen/Dense>
#include <Eigen/LU>
#include <cmath>

void PoseRegression::InitializePose(const HybridPredictionContainer& predictions,
                                    const PoseRegressionPara& para,
                                    AffineXform3d* rigid_pose) {

  const vector<Keypoint>* keypoints = predictions.GetKeypoints();
  const vector<EdgeVector>* edges = predictions.GetEdgeVectors();
  const vector<SymmetryCorres>* symcorres = predictions.GetSymmetryCorres();

  // initial weights of different representations
  vector<double> weight_keypts (keypoints->size()), weight_edges (edges->size()), weight_symcorres (symcorres->size());
  for (unsigned id = 0; id < keypoints->size(); ++id)
    weight_keypts[id] = 1;
  for (unsigned id = 0; id < edges->size(); ++id)
    weight_edges[id] = 1;
  for (unsigned id = 0; id < symcorres->size(); ++id)
    weight_symcorres[id] = 1;

  // Generate 12x12 Data matrix from different representations
  Matrix<double, 12, 12> DataMatrix;
  GenerateDataMatrix(predictions, weight_keypts, weight_edges, weight_symcorres,
    para, &DataMatrix);

  // Calculate Pose initial
  vector<Vector12d> eigenVectors;
  const unsigned numEigs = 4;
  //LeadingEigenSpace(DataMatrix, numEigs, predictions, &eigenVectors);
  LeadingEigenSpace(DataMatrix, numEigs, predictions, &eigenVectors, rigid_pose);
}
/*
Compute the 12x12 matrix
*/
void PoseRegression::GenerateDataMatrix(const HybridPredictionContainer& predictions,
                                        const vector<double>& weight_keypts,
                                        const vector<double>& weight_edges,
                                        const vector<double>& weight_symcorres,
                                        const PoseRegressionPara& para,
                                        Matrix12d* data_matrix) {
  const vector<Keypoint>* keypoints = predictions.GetKeypoints();
  const vector<EdgeVector>* edges = predictions.GetEdgeVectors();
  const vector<SymmetryCorres>* symcorres = predictions.GetSymmetryCorres();

  // Step1: add contribution of keypoint to data matrix
  unsigned numKpts = keypoints->size();
  Matrix<double, 12, Dynamic> J_kpts;
  Matrix<double, Dynamic, Dynamic> W_kpts;
  J_kpts.resize(12, numKpts*3);
  W_kpts.resize(numKpts*3, numKpts*3);
  Matrix<double, 12, 12> M_kpts;

  J_kpts.fill(0.0);
  W_kpts.fill(0.0);
  M_kpts.fill(0.0);

  for (unsigned ptId = 0; ptId < numKpts; ++ptId) {
    const Keypoint& kp = (*keypoints)[ptId];
    //kp.point3D_gt
    //kp.point2D_pred
    double x_hc, y_hc, z_hc;
    x_hc = kp.point2D_pred[0];
    y_hc = kp.point2D_pred[1];
    z_hc = 1.0;

    double x_3d, y_3d, z_3d;
    x_3d = kp.point3D_gt[0];
    y_3d = kp.point3D_gt[1];
    z_3d = kp.point3D_gt[2];

    J_kpts(0, ptId * 3) = 0 * x_3d;     J_kpts(0, ptId * 3 + 1) = -z_hc * x_3d; J_kpts(0, ptId * 3 + 2) = y_hc * x_3d;
    J_kpts(1, ptId * 3) = z_hc * x_3d;  J_kpts(1, ptId * 3 + 1) = 0 * x_3d;     J_kpts(1, ptId * 3 + 2) = -x_hc * x_3d;
    J_kpts(2, ptId * 3) = -y_hc * x_3d; J_kpts(2, ptId * 3 + 1) = x_hc * x_3d;  J_kpts(2, ptId * 3 + 2) = 0 * x_3d;

    J_kpts(3, ptId * 3) = 0 * y_3d;     J_kpts(3, ptId * 3 + 1) = -z_hc * y_3d; J_kpts(3, ptId * 3 + 2) = y_hc * y_3d;
    J_kpts(4, ptId * 3) = z_hc * y_3d;  J_kpts(4, ptId * 3 + 1) = 0 * y_3d;     J_kpts(4, ptId * 3 + 2) = -x_hc * y_3d;
    J_kpts(5, ptId * 3) = -y_hc * y_3d; J_kpts(5, ptId * 3 + 1) = x_hc * y_3d;  J_kpts(5, ptId * 3 + 2) = 0 * y_3d;

    J_kpts(6, ptId * 3) = 0 * z_3d;     J_kpts(6, ptId * 3 + 1) = -z_hc * z_3d; J_kpts(6, ptId * 3 + 2) = y_hc * z_3d;
    J_kpts(7, ptId * 3) = z_hc * z_3d;  J_kpts(7, ptId * 3 + 1) = 0 * z_3d;     J_kpts(7, ptId * 3 + 2) = -x_hc * z_3d;
    J_kpts(8, ptId * 3) = -y_hc * z_3d; J_kpts(8, ptId * 3 + 1) = x_hc * z_3d;  J_kpts(8, ptId * 3 + 2) = 0 * z_3d;

    J_kpts(9, ptId * 3) = 0 * 1.0;      J_kpts(9, ptId * 3 + 1) = -z_hc * 1.0; J_kpts(9, ptId * 3 + 2) = y_hc * 1.0;
    J_kpts(10, ptId * 3) = z_hc * 1.0;  J_kpts(10, ptId * 3 + 1) = 0 * 1.0;    J_kpts(10, ptId * 3 + 2) = -x_hc * 1.0;
    J_kpts(11, ptId * 3) = -y_hc * 1.0; J_kpts(11, ptId * 3 + 1) = x_hc * 1.0; J_kpts(11, ptId * 3 + 2) = 0 * 1.0;

    // calculate weight for keypoints
    //kp.inv_half_var
    EigenSolver<Matrix2d> es_kp(kp.inv_half_var);
    W_kpts(ptId * 3, ptId * 3) = es_kp.eigenvalues().norm() * 0.02;
    W_kpts(ptId * 3 + 1, ptId * 3 + 1) = es_kp.eigenvalues().norm() * 0.02;
    W_kpts(ptId * 3 + 2, ptId * 3 + 2) = es_kp.eigenvalues().norm() * 2;
  }
  // add J_kpts into Datamatrix
  M_kpts = J_kpts * W_kpts * J_kpts.transpose();
  for (unsigned row_id = 0; row_id < 12; ++row_id) {
    for (unsigned com_id = 0; com_id < 12; ++com_id) {
      (*data_matrix)(row_id, com_id) = M_kpts(row_id, com_id) * para.gamma_kpts;
    }
  }
  //  Step2: add contribution of edge for data matrix
  unsigned numedges = edges->size();
  Matrix<double, 12, Dynamic> J_edge(12, numedges*3);
  Matrix<double, Dynamic, Dynamic> W_edge(numedges*3, numedges*3);
  Matrix<double, 12, 12> M_edge;
  J_edge.fill(0.0);
  W_edge.fill(0.0);
  M_edge.fill(0.0);

  for (unsigned edgeId = 0; edgeId < numedges; ++edgeId) {
    const EdgeVector& ev = (*edges)[edgeId];
    const Keypoint& kp_start = (*keypoints)[ev.start_id]; // edge = s - e
    const Keypoint& kp_end = (*keypoints)[ev.end_id];
    // ev.vec_pred
    // pts2d_s
    // pts3d_t
    // edge3d
    // Part1 of step2
    double x_hc, y_hc, z_hc; // denotes pts2d_s
    x_hc = kp_end.point2D_pred[0];
    y_hc = kp_end.point2D_pred[1];
    z_hc = 1.0;

    double x_3d, y_3d, z_3d; // denotes edge3d
    x_3d = kp_start.point3D_gt[0] - kp_end.point3D_gt[0];
    y_3d = kp_start.point3D_gt[1] - kp_end.point3D_gt[1];
    z_3d = kp_start.point3D_gt[2] - kp_end.point3D_gt[2];

    J_edge(0, edgeId * 3) = 0 * x_3d;     J_edge(0, edgeId * 3 + 1) = -z_hc * x_3d; J_edge(0, edgeId * 3 + 2) = y_hc * x_3d;
    J_edge(1, edgeId * 3) = z_hc * x_3d;  J_edge(1, edgeId * 3 + 1) = 0 * x_3d;     J_edge(1, edgeId * 3 + 2) = -x_hc * x_3d;
    J_edge(2, edgeId * 3) = -y_hc * x_3d; J_edge(2, edgeId * 3 + 1) = x_hc * x_3d;  J_edge(2, edgeId * 3 + 2) = 0 * x_3d;

    J_edge(3, edgeId * 3) = 0 * y_3d;     J_edge(3, edgeId * 3 + 1) = -z_hc * y_3d; J_edge(3, edgeId * 3 + 2) = y_hc * y_3d;
    J_edge(4, edgeId * 3) = z_hc * y_3d;  J_edge(4, edgeId * 3 + 1) = 0 * y_3d;     J_edge(4, edgeId * 3 + 2) = -x_hc * y_3d;
    J_edge(5, edgeId * 3) = -y_hc * y_3d; J_edge(5, edgeId * 3 + 1) = x_hc * y_3d;  J_edge(5, edgeId * 3 + 2) = 0 * y_3d;

    J_edge(6, edgeId * 3) = 0 * z_3d;     J_edge(6, edgeId * 3 + 1) = -z_hc * z_3d; J_edge(6, edgeId * 3 + 2) = y_hc * z_3d;
    J_edge(7, edgeId * 3) = z_hc * z_3d;  J_edge(7, edgeId * 3 + 1) = 0 * z_3d;     J_edge(7, edgeId * 3 + 2) = -x_hc * z_3d;
    J_edge(8, edgeId * 3) = -y_hc * z_3d; J_edge(8, edgeId * 3 + 1) = x_hc * z_3d;  J_edge(8, edgeId * 3 + 2) = 0 * z_3d;

    //Part2 of step2
    x_hc = ev.vec_pred[0];   // denote edge_pred_2d
    y_hc = ev.vec_pred[1];
    z_hc = 0.0;

    x_3d = kp_start.point3D_gt[0];// denotes pts3d_t
    y_3d = kp_start.point3D_gt[1];
    z_3d = kp_start.point3D_gt[2];

    J_edge(0, edgeId * 3) += 0 * x_3d;     J_edge(0, edgeId * 3 + 1) += -z_hc * x_3d; J_edge(0, edgeId * 3 + 2) += y_hc * x_3d;
    J_edge(1, edgeId * 3) += z_hc * x_3d;  J_edge(1, edgeId * 3 + 1) += 0 * x_3d;     J_edge(1, edgeId * 3 + 2) += -x_hc * x_3d;
    J_edge(2, edgeId * 3) += -y_hc * x_3d; J_edge(2, edgeId * 3 + 1) += x_hc * x_3d;  J_edge(2, edgeId * 3 + 2) += 0 * x_3d;

    J_edge(3, edgeId * 3) += 0 * y_3d;     J_edge(3, edgeId * 3 + 1) += -z_hc * y_3d; J_edge(3, edgeId * 3 + 2) += y_hc * y_3d;
    J_edge(4, edgeId * 3) += z_hc * y_3d;  J_edge(4, edgeId * 3 + 1) += 0 * y_3d;     J_edge(4, edgeId * 3 + 2) += -x_hc * y_3d;
    J_edge(5, edgeId * 3) += -y_hc * y_3d; J_edge(5, edgeId * 3 + 1) += x_hc * y_3d;  J_edge(5, edgeId * 3 + 2) += 0 * y_3d;

    J_edge(6, edgeId * 3) += 0 * z_3d;     J_edge(6, edgeId * 3 + 1) += -z_hc * z_3d; J_edge(6, edgeId * 3 + 2) += y_hc * z_3d;
    J_edge(7, edgeId * 3) += z_hc * z_3d;  J_edge(7, edgeId * 3 + 1) += 0 * z_3d;     J_edge(7, edgeId * 3 + 2) += -x_hc * z_3d;
    J_edge(8, edgeId * 3) += -y_hc * z_3d; J_edge(8, edgeId * 3 + 1) += x_hc * z_3d;  J_edge(8, edgeId * 3 + 2) += 0 * z_3d;

    J_edge(9, edgeId * 3) = 0 * 1.0;      J_edge(9, edgeId * 3 + 1) = -z_hc * 1.0;  J_edge(9, edgeId * 3 + 2) = y_hc * 1.0;
    J_edge(10, edgeId * 3) = z_hc * 1.0;  J_edge(10, edgeId * 3 + 1) = 0 * 1.0;     J_edge(10, edgeId * 3 + 2) = -x_hc * 1.0;
    J_edge(11, edgeId * 3) = -y_hc * 1.0; J_edge(11, edgeId * 3 + 1) = x_hc * 1.0;  J_edge(11, edgeId * 3 + 2) = 0 * 1.0;

    W_edge(edgeId * 3, edgeId * 3) = weight_edges[edgeId];
    W_edge(edgeId * 3 + 1, edgeId * 3 + 1) = weight_edges[edgeId];
    W_edge(edgeId * 3 + 2, edgeId * 3 + 2) = weight_edges[edgeId];
  }
  // add J_edge into Datamatrix
  M_edge = J_edge * W_edge * J_edge.transpose();
  for (unsigned row_id = 0; row_id < 12; ++row_id) {
    for (unsigned com_id = 0; com_id < 12; ++com_id) {
      (*data_matrix)(row_id, com_id) += M_edge(row_id, com_id) * para.gamma_edge;
    }
  }

  // Step3: add contribution of symmetry for data matrix
  unsigned numSym = symcorres->size();
  Matrix<double, 9, Dynamic> J_sym(9, numSym);
  Matrix<double, Dynamic, Dynamic> W_sym(numSym, numSym);
  Matrix<double, 9, 9> M_sym;

  J_sym.fill(0.0);
  W_sym.fill(0.0);
  M_sym.fill(0.0);

  const Vector3d &normal_gt = predictions.GetReflectionPlaneNormal();
  for (unsigned symId = 0; symId < numSym; ++symId) {
    const SymmetryCorres& sc = (*symcorres)[symId];

    double x_hc, y_hc, z_hc; // denotes sym_pred_2d
    x_hc = sc.qs1_cross_qs2[0];
    y_hc = sc.qs1_cross_qs2[1];
    z_hc = sc.qs1_cross_qs2[2];

    J_sym(0, symId) = x_hc * normal_gt[0]; J_sym(1, symId) = y_hc * normal_gt[0]; J_sym(2, symId) = z_hc * normal_gt[0];
    J_sym(3, symId) = x_hc * normal_gt[1]; J_sym(4, symId) = y_hc * normal_gt[1]; J_sym(5, symId) = z_hc * normal_gt[1];
    J_sym(6, symId) = x_hc * normal_gt[2]; J_sym(7, symId) = y_hc * normal_gt[2]; J_sym(8, symId) = z_hc * normal_gt[2];
    W_sym(symId, symId) = weight_symcorres[symId];
  }  // add J_kpts into Datamatrix
  M_sym = J_sym * W_sym * J_sym.transpose();
  for (unsigned row_id = 0; row_id < 9; ++row_id) {
    for (unsigned com_id = 0; com_id < 9; ++com_id) {
      (*data_matrix)(row_id, com_id) += M_sym(row_id, com_id) * para.gamma_sym;
    }
  }
}

// Leading eigen-space computation
void PoseRegression::LeadingEigenSpace(Matrix12d& data_matrix, const unsigned& numEigs,
                                       const HybridPredictionContainer& predictions, vector<Vector12d>* eigenVectors,
                                       AffineXform3d* rigid_pose) {
  Matrix<double, 12, 4> eigenvector;
  // SVD of data_matrix svd.singularValues()[0]
  JacobiSVD<Matrix12d> svd(data_matrix, ComputeFullV | ComputeFullU);
  for (unsigned com_id = 0; com_id < numEigs; ++com_id) {
    for (unsigned row_id = 0; row_id < 12; ++row_id)
      eigenvector(row_id, com_id) = svd.matrixV().col(11 - com_id)[row_id];
  }
  // Calculate initial coefficient(beta) of different eigenvectors
  Map<MatrixXd> A1(eigenvector.col(0).head(9).data(), 3,3);
  Map<MatrixXd> A2(eigenvector.col(1).head(9).data(), 3,3);
  Map<MatrixXd> A3(eigenvector.col(2).head(9).data(), 3,3);
  Map<MatrixXd> A4(eigenvector.col(3).head(9).data(), 3,3);

  Matrix3d B1 = A1.transpose() * A1;
  Matrix3d B2 = A1.transpose() * A2 + A2.transpose() * A1;
  Matrix3d B3 = A1.transpose() * A3 + A3.transpose() * A1;
  Matrix3d B4 = A2.transpose() * A2;
  Matrix3d B5 = A2.transpose() * A3 + A3.transpose() * A2;
  Matrix3d B6 = A3.transpose() * A3;

  // form linear system Cx = y to solve gamma(x) which contains coefficient
  Matrix6d C;
  Vector6d y(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
  Vector6d gamma;

  C(0, 0) = B1(0, 0); C(0, 1) = B2(0, 0); C(0, 2) = B3(0, 0); C(0, 3) = B4(0, 0); C(0, 4) = B5(0, 0); C(0, 5) = B6(0, 0);
  C(1, 0) = B1(0, 1); C(1, 1) = B2(0, 1); C(1, 2) = B3(0, 1); C(1, 3) = B4(0, 1); C(1, 4) = B5(0, 1); C(1, 5) = B6(0, 1);
  C(2, 0) = B1(0, 2); C(2, 1) = B2(0, 2); C(2, 2) = B3(0, 2); C(2, 3) = B4(0, 2); C(2, 4) = B5(0, 2); C(2, 5) = B6(0, 2);
  C(3, 0) = B1(1, 1); C(3, 1) = B2(1, 1); C(3, 2) = B3(1, 1); C(3, 3) = B4(1, 1); C(3, 4) = B5(1, 1); C(3, 5) = B6(1, 1);
  C(4, 0) = B1(1, 2); C(4, 1) = B2(1, 2); C(4, 2) = B3(1, 2); C(4, 3) = B4(1, 2); C(4, 4) = B5(1, 2); C(4, 5) = B6(1, 2);
  C(5, 0) = B1(2, 2); C(5, 1) = B2(2, 2); C(5, 2) = B3(2, 2); C(5, 3) = B4(2, 2); C(5, 4) = B5(2, 2); C(5, 5) = B6(2, 2);

  gamma = C.lu().solve(y);
  //project gamma in to valid coefficient space
  MatrixXd Gamma = MatrixXd::Random(3, 3);
  Gamma(0, 0) = gamma[0];  Gamma(0, 1) = gamma[1];  Gamma(0, 2) = gamma[2];
  Gamma(1, 0) = gamma[1];  Gamma(1, 1) = gamma[3];  Gamma(1, 2) = gamma[4];
  Gamma(2, 0) = gamma[2];  Gamma(2, 1) = gamma[4];  Gamma(2, 2) = gamma[5];
  // calculated sorted eigen vector and eigen value in increasing order
  SelfAdjointEigenSolver<MatrixXd> es(Gamma);
  MatrixXcd D = es.eigenvalues().asDiagonal();
  MatrixXcd V = es.eigenvectors();
  D(0, 0) = 0;
  D(1, 1) = 0;
  Gamma = (V * D * V.inverse()).real();
  // assign new gamma
  gamma[0] = Gamma(0, 0); gamma[1] = Gamma(0, 1); gamma[2] = Gamma(0, 2);
  gamma[3] = Gamma(1, 1); gamma[4] = Gamma(1, 2); gamma[5] = Gamma(2, 2);
  if (gamma[0] < 0)
    gamma *= -1.0;
  // recover initial coefficient beta from gamma with following equations:
  // gamma[0] = beta[0]^2; gamma[1] = beta[0] * beta[1]; gamma[2] = beta[0]*beta[2]
  // gamma[3] = beta[1]^2; gamma[4] = beta[1] * beta[2]; gamma[5] = beta[2]^2;
  Vector4d beta;
  double temp;
  beta[3] = 0;
  beta[0] = sqrt(gamma[0]);
  beta[1] = gamma[1] / beta[0];
  temp = sqrt(gamma[3]);
  if (gamma[1] < 0)
    temp = temp * -1.0;
  beta[1] = 0.5 * (temp + beta[1]);
  beta[2] = gamma[4] / beta[1];
  temp = gamma[2] / beta[0];
  beta[2] = 0.5 * (beta[2] + temp);

  // check the validation of betas
  Matrix3d R_init;
  Vector3d t_init;
  Matrix<double, 12, 9> At1;
  Matrix<double, 12, 3> At2;
  At1 = data_matrix.leftCols(9);
  At2 = data_matrix.rightCols(3);
  R_init = beta[0] * A1 + beta[1] * A2 + beta[2] * A3 + beta[3] * A4;

  // check1 det(R) > 0
  if (R_init.determinant() < 0) {
    beta = beta * -1.0;
    R_init = beta[0] * A1 + beta[1] * A2 + beta[2] * A3 + beta[3] * A4;
  }

  Map<VectorXd> R_vec(R_init.data(), 9, 1);
  t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec);

 // check2 depth > 0
  const vector<Keypoint>* keypoints = predictions.GetKeypoints();
  double z = 0;
  unsigned numKpts = keypoints->size();

  for (unsigned ptId = 0; ptId < numKpts; ++ptId) {
    const Keypoint& kp = (*keypoints)[ptId];
    z = z + (R_init * kp.point3D_gt + t_init)[2];
  }

  if (z < 0){
    t_init = t_init * -1.0;
    R_vec = -1.0 * (At1.transpose() * At1).lu().solve(At1.transpose() * At2 * t_init);
    Map<Matrix3d> R_temp(R_vec.data(), 3,3);
    // map R_init into SO(3)
    JacobiSVD<Matrix3d> svd(R_temp, ComputeFullV | ComputeFullU);
    R_init = svd.matrixU() * svd.matrixV().transpose();
    if (R_init.determinant() < 0){
      Vector3d dia(1.0,1.0,-1.0);
      R_init = svd.matrixU() * dia.asDiagonal() * svd.matrixV().transpose();
    }
    t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec);
  }

  // optimize beta and R_init simutaneously
  Matrix3d A = R_init;
  Matrix3d R;
  for(unsigned iter = 0; iter < 10; ++iter) {
    // optimize R_init
    JacobiSVD<Matrix3d> svd(A, ComputeFullV | ComputeFullU);
    if (A.determinant() > 0) {
      R = svd.matrixU() * svd.matrixV().transpose();
    }else {
      Vector3d dia(1.0, 1.0, -1.0);
      R = svd.matrixU() * dia.asDiagonal() * svd.matrixV().transpose();
    }
    // optimize betas by forming a linear system for beta
    Matrix<double, 9, 4> D;
    unsigned row_id;
    for(row_id = 0; row_id < 3; ++row_id) {
      D(row_id, 0) = A1(0, row_id); D(row_id, 1) = A2(0, row_id);
      D(row_id, 2) = A3(0, row_id); D(row_id, 3) = A4(0, row_id);
    }
    for(row_id = 3; row_id < 6; ++row_id) {
      D(row_id, 0) = A1(1, row_id - 3); D(row_id, 1) = A2(1, row_id - 3);
      D(row_id, 2) = A3(1, row_id - 3); D(row_id, 3) = A4(1, row_id - 3);
    }
    for(row_id = 6; row_id < 9; ++row_id) {
      D(row_id, 0) = A1(2, row_id - 6); D(row_id, 1) = A2(2, row_id - 6);
      D(row_id, 2) = A3(2, row_id - 6); D(row_id, 3) = A4(2, row_id - 6);
    }
    Matrix3d R_temp = R.transpose();
    Map<VectorXd> dy(R_temp.data(), 9, 1);
    beta = D.bdcSvd(ComputeFullV | ComputeFullU).solve(dy);
    A = beta[0] * A1 + beta[1] * A2 + beta[2] * A3 + beta[3] * A4;
  }

  R_init = R;
  Map<Matrix<double, 9, 1>> R_vec1(R.data(), 9, 1);
  t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec1);
  // check R_init and t_init again
  z = 0;
  for (unsigned ptId = 0; ptId < numKpts; ++ptId) {
    const Keypoint& kp = (*keypoints)[ptId];
    z = z + (R_init * kp.point3D_gt + t_init)[2];
  }
  if (z < 0) {
    t_init = t_init * -1.0;
    R_vec = -1.0 * (At1.transpose() * At1).lu().solve(At1.transpose() * At2 * t_init);
    Map<Matrix3d> R_temp1(R_vec.data(), 3, 3);

    // map R_init into SO(3)
    JacobiSVD<Matrix3d> svd(R_temp1, ComputeFullV | ComputeFullU);
    R_init = svd.matrixU() * svd.matrixV().transpose();
    if (R_init.determinant() < 0) {
      Vector3d dia1(1.0, 1.0, -1.0);
      R_init = svd.matrixU() * dia1.asDiagonal() * svd.matrixV().transpose();
    }
    t_init = -1.0 * (At2.transpose() * At2).lu().solve(At2.transpose() * At1 * R_vec);
  }

 // Assign pose initial to rigid_pose
  for (unsigned row_id = 0; row_id < 3; ++row_id) {
    (*rigid_pose)[0][row_id] = t_init[row_id];
    for (unsigned com_id = 0; com_id < 3; ++com_id)
      (*rigid_pose)[com_id + 1][row_id] = R_init(row_id, com_id);
  }
}


#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <base/timer.hpp>
#include <boost/program_options.hpp>
#include <ridgelet/construction/ft.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include "ridgelet/basis.hpp"

#include <fstream>
#include <iostream>
#include <sstream>

using namespace std;

void dump(const RidgeletFrame& RF)
{
  const char* fname = "assemble.h5";
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  auto& lambdas = RF.lambdas();

  for (auto& lam : lambdas) {
    if (lam.t == rt_type::S) {
      auto& M = RF.get_dense(lam);
      stringstream ss;
      ss << lam;
      string dset = ss.str();
      //      cout << "creating dset: " << dset << "\n";
      eigen2hdf::save(file, dset, M);
    } else {
      auto& M = RF.get_sparse(lam);
      stringstream ss;
      ss << lam;
      string dset = ss.str();
      //      cout << "creating dset:" << dset << "\n";
      eigen2hdf::save_sparse(file, dset, M);
    }
  }
  H5Fclose(file);
  cout << "wrote ridgelet coeffs to " << fname << "\n";
}

void listb(const RidgeletFrame& RF, unsigned int rho_x, unsigned int rho_y)
{
  for (auto& lam : RF.lambdas()) {
    unsigned int tx = translation_size(lam.t, lam.j, rho_x, 0);
    unsigned int ty = translation_size(lam.t, lam.j, rho_y, 1);

    if (lam.t == rt_type::S) {
      auto& M = RF.get_dense(lam);
      stringstream ss;
      ss << lam;
      string sl = ss.str();
      stringstream dim;
      dim << M.rows() << " x " << M.cols();
      cout << setw(15) << sl << ", rt_size: " << setw(15) << dim.str() << ", T: " << setw(5) << tx
           << " x " << setw(5) << ty << "\n";
    } else {
      auto& M = RF.get_sparse(lam);
      stringstream ss;
      ss << lam;
      string sl = ss.str();
      stringstream dim;
      dim << M.rows() << " x " << M.cols();
      cout << setw(15) << sl << ", rt_size: " << setw(15) << dim.str() << ", T: " << setw(5) << tx
           << " x " << setw(5) << ty << "\n";
    }
  }
  cout << "T: translation set dimensions, rt_size: ridgelet coefficient array dimensions"
       << "\n";
}

int main(int argc, char* argv[])
{
  SOURCE_INFO();
  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("dump", "dump coefficients to hdf5")
      ("list", "list basis and sizes");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }
  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n";

  RDTSCTimer timer;
  timer.start();
  RidgeletFrame frame(Jx, Jy, rho_x, rho_y);
  auto tlap = timer.stop();
  cout << "RidgeletFrame::RidgeletFrame(): " << tlap / 1e9 << " [Gcycles]"
       << "\n";

  cout << "frame.size: " << frame.size() << "\n";
  cout << "frame.Nx: " << frame.Nx() << "\n";
  cout << "frame.Ny: " << frame.Ny() << "\n";

  {
    ofstream fout("assemble_rt.txt");
    for (auto& lam : frame.lambdas()) {
      fout << lam << endl;
    }
    fout.close();
  }

  if (vm.count("dump")) dump(frame);

  if (vm.count("list")) listb(frame, rho_x, rho_y);

  return 0;
}


#ifndef DIJKSTRA_HPP
#define DIJKSTRA_HPP

#include <boost/range.hpp>

#include <list>
#include <optional>

template <typename Graph>
using Vertex = typename Graph::vertex_descriptor;

template <typename Graph>
using Edge = typename Graph::edge_descriptor;

/**
 * Find the shortest path in graph g to t.
 */
template <typename Graph, typename Solution, typename Queue,
          typename Label, typename Functor>
void
dijkstra(const Graph &g, Solution &S, Queue &Q, const Label &l,
         const Functor &f, Vertex<Graph> t)
{
  // Boot the search.
  Q.push(l);

  while(!Q.empty())
    {
      const Label &l = move_label(S, Q);
      Vertex<Graph> v = get_target(l);

      // Stop searching when we reach the destination node.
      if (v == t)
        break;

      // Itereate over the out edges of vertex v.
      for(const auto &e: boost::make_iterator_range(out_edges(v, g)))
        relax(g, S, Q, e, l, f);
    }
}

/**
 * Try to relax edge e, given label l.
 */
template <typename Graph, typename Solution, typename Queue,
          typename Label, typename Functor>
void
relax(const Graph &g, Solution &S, Queue &Q, const Edge<Graph> &e,
      const Label &l, const Functor &f)
{
  try
    {
      // Candidate labels.
      auto cls = f(e, l);

      for (auto &cl: cls)
	if (!has_better_or_equal(S, cl) && !has_better_or_equal(Q, cl))
	  {
	    purge_worse(Q, cl);
	    // We push the new label after purging, so that purging
	    // has less work, i.e., a smaller Q.  Furthermore, in
	    // purge_worse we are using the <= operator, which would
	    // remove the label we push below.
	    Q.push(std::move(cl));
	  }
    } catch (bool no_label)
    {
      assert(no_label);
    }
}

/**
 * Build the path by tracing labels.
 */
template <typename Solution, typename Vertex, typename Label,
          typename Tracer>
std::optional<typename Tracer::path_type>
trace(const Solution &S, Vertex dst, const Label &sl, Tracer &t)
{
  // Find the solution for the dst node, the destination iterator.
  // This mundane work should be done here, not in the user functor.
  // We want to reuse the iterator i, and so the user functor should
  // accept this iterator.
  auto i = S.find(dst);

  // Check whether there is a solution for node dst in S.
  if (i != S.end())
    {
      // This is the path we're building.
      typename Tracer::path_type result;

      // Get the initial label, i.e. the label for the destination.
      for(auto li = t.init(result, i); *li != sl; li = t.advance(S, li))
        t.push(result, li);

      // Move the result to the optional object we return.
      return std::move(result);
    }

  // We return an empty optional, becase no path was found.
  return std::optional<typename Tracer::path_type>();
}

#endif // DIJKSTRA_HPP


/**
* @file: CrowdBT.hpp
* @brief:
* @author: Changjiang Cai, ccai1@stevens.edu, caicj5351@gmail.com
* @version: 0.0.1
* @creation date: 17-12-2015
* @last modified: Thu 10 Mar 2016 09:32:02 AM EST
*/

#ifndef __HEADER__CROWD_SOURCING_CROWD_BT_H_
#define __HEADER__CROWD_SOURCING_CROWD_BT_H_
#include <cmath>
#include <vector>
#include <random>
#include <algorithm>
#include <ctime>
#include <climits>
#include <utility> // std::pair, std::make_pair
#include <tuple>
#include <boost/math/special_functions/digamma.hpp>
#include <iostream>
#include "ktaub.hpp"
#include "MyUtility.hpp"

using namespace std;
/*
* KL(p || q) = ...
* p = N(mu_1, var_1), q = N(mu_2, var_2);
* where var_1 = sigma_1^2; and var_2 = sigma_2^2;
*/
// KL divergence of two Gaussian distributions.
// See its definition in Equation (1.113) on Page 55, PRML. M. Bishop.
double KLGaussian(const double mu_p, const double var_p, const double mu_q,
	const double var_q);

// Beta function B(a, b) = Gamma(a)* Gamma(b)/ Gamma(a+ b);
// Note that Beat function is different from Beta distribution.
// see Wikipedia for detailed information about its definition.
double Beta(const double & a, const double & b);

// KL divergence of two Beta distributions.
// Beta(x | a, b) = constant * x^(a -1) *(1-x)^(b -1)
// = Gamma(a + b)/ (Gamma(a)*Gamma(b)) * x^(a -1) *(1-x)^(b -1).
double KLBeta(const double alpha_p, const double beta_p,
	const double alpha_q, const double beta_q);

/* ***************************************
* for the first voting, (o_i, o_j), this pairwise comparison could generate
* the two possible voting results, i.e., o_i > o_j, and o_i < o_j.
* How will they affect the final global ranking, if they are chosen separately?
* ***************************************
*/

/* Here we want to study the problem that how two opposite outcomes of the
* first pair comparison, like (o_i, o_j) will impact the final global ranking?
* Specifically speaking,
* 1) For the first pair comparison, I do not set a specific one. Which pair
is chosen totally depends on maximum KL divergence in Equation 10 in this paper.
* 2) 1st-trial: For example, there are two cases (or two curves), one is normal,
the other is reverse. They are assigned two copies of the same initial prior distributions
(that is with the same \mu and \sigma etc, even though those parameters
are randomly generated.) Therefore, the same first pair comparison,
for example, (object 13, object 16) will be chosen for both of them.
Then they will vote (object 13 > object 16) and (object 13 < object 16),
respectively. The subsequent processes will continue as usual.

* 3) 2nd-trial: Since a different setting of initialization of those prior distributions,
probably a new pair, different from that in Fig 1, will be selected,
for example, (object 1, object 5). And so on ...
*/
void run_CrowdBT_1st_pair(const int & num_o, const int * p_num_w,
	const int num_w_size, const int & num_repeat);

// different setting of number of workers;
// meaning graphs with different degrees.
void run_CrowdBT_diff_w_num(const int & num_o,
	const int & num_repeat);

struct INITIALS{
	double alpha_init;
	double beta_init;
	double eta_init;
	double mu_init;
	double var_init;
	double kappa_init;
};

struct PARAMS {
	// vector<double> v_alpha; // Beta distribution
	// vector<double> v_beta; // Beta distribution
	double alpha, beta, kappa; // Beta distribution
	/* the probability that the k-th annotator agrees
	 * with the true pairwise preference.
	 */
	// vector<double> v_eta;
	double eta;
	vector<double> v_mu; // mean of Gaussian;
	vector<double> v_emu;// exp(mu)
	vector<double> v_var; // variance of Gaussian;
	vector<vector<double>> vv_history;

	// 1) default constructor;
	PARAMS(){}
	// 2) user defined constructor;
	PARAMS(const int & num_o, const INITIALS & a){
		//v_alpha = vector<double>(num_o, a.alpha_init);
		//v_beta =  vector<double>(num_o, a.beta_init);
		alpha = a.alpha_init;
		beta = a.beta_init;
		kappa = a.kappa_init;
		// v_eta =   vector<double>(num_o, a.eta_init);
		eta = a.eta_init;

		srand((unsigned)time(NULL));

		v_mu = vector<double>(num_o, a.mu_init);
		v_emu = vector<double>(num_o, 0.0);

		for (int i = 0; i < num_o; ++i){
			// generate a uniform random value [0, 1]
			v_mu[i] *= rand() / double(RAND_MAX);
			v_emu[i] = exp(v_mu[i]);
		}
		v_var = vector<double>(num_o, a.var_init);
		vv_history = vector<vector<double>>(num_o, vector<double>(num_o, 0));
	}

	// 3) copy constructor;
	PARAMS(const PARAMS & p){
		//v_alpha = vector<double>(num_o, a.alpha_init);
		//v_beta =  vector<double>(num_o, a.beta_init);
		alpha = p.alpha;
		beta = p.beta;
		kappa = p.kappa;
		eta = p.eta;

		// vector : copy constructor
		v_mu = vector<double>(p.v_mu);
		v_emu = vector<double>(p.v_emu);
		v_var = vector<double>(p.v_var);
		vv_history = vector<vector<double>>(p.vv_history);
	}

	// destructor
	~PARAMS(){
		// release memory space;
		//vector<double>().swap(v_alpha);
		//vector<double>().swap(v_beta);
		//vector<double>().swap(v_eta);
		vector<double>().swap(v_mu);
		vector<double>().swap(v_emu);
		vector<double>().swap(v_var);
		vector<vector<double>>().swap(vv_history);
	}

};

struct NEW_PARAMS {
	// we assume object i > object j;
	// for the opposite case (i.e., object j > object i),
	// just swap i and j before calling this function,
	// or consider always "mu_i_new" as the preferred object,
	// and "mu_j_new" as the less one.
	double mu_i_new,
		mu_j_new,
		var_i_new,
		var_j_new,
		alpha_new,
		beta_new;
};

class Crowd_BT{

private:
	int num_o; // number of objects;
	PARAMS params;
public:
	// constructor
	Crowd_BT(const int & num_o, const INITIALS & a){
		params = PARAMS(num_o, a);
		this->num_o = num_o;
	}

	// copy constructor
	// constructor
	Crowd_BT(const Crowd_BT & bt){
		params = PARAMS(bt.params);
		this->num_o = bt.num_o;
	}
	// destructor
	~Crowd_BT(){ params.~PARAMS(); }

	// To rank objects by sorting the obtained {mu_i}(means of Gaussians.);
	// return the object ranking in an ascending order.
	int * get_ascending_ranking();
	/*Equation (19), (16)*/
	void get_C(const int & i, const int & j, double & c1, double & c);

	/*
	* Update the parameters based on the equations shown in the paper:
	* "Pairwise Ranking Aggregation in a CrowdSourced Setting".
	* The equation will be named according to the number originally shown
	* in this paper.
	* Pay attention to this function:
	* its first parameter means the preferred object,
	* therefore, if o_j > o_i, we should input j as the first
	* parameter to this function. For the opposite case of o_i > o_j,
	* we should pass i as the first parameter to this function.
	*/
	NEW_PARAMS get_updated_parameters(const int & i, const int& j,
		double & c1, double & c);

	/* v_layers describes how to measure the similarity of objects to be ranked.
	 * If we have L = 20 objects, the ranking is 1, 2, ..., L.
	 * We can set the ambiguity level, for example, we set v_layers = [0.25, 0.5, 1],
	 * generating the following ranking layers:
	 *  1) level 1: [0, 0.25]* L  = [0, 5];
	 *  2) level 2: [0.25, 0.75] * L = [5, 14];
	 *  3) level 3: [0.75, 1.0] * L = [14, 20];
	 * If object 1 and object 2 come from the same layer, like, they belong to level 2,
	 * there is a possibility that wrong ranking will occur,
	 * according to some probability distribution.
	 * We will implement the above idea in the following function.
	 */
	bool get_preference(const vector<double> & v_scores,
		const int & i, const int & j, const vector<double> & v_layers);

	/* Select a pair (o_i, o_j) for some annotator k which
	 * maximize the expected information gain in E.q.(10).
	 * parameter: gamma, exploration-exploitation trade-off,
	 * see Section 5.1.2 in this paper.
	 */
	//**********************************
	// before actual voting, try to select a pair of objects o_i and o_j,
	// such that they can maximize the expected information gain
	// in E.q.(10) in this paper.
	//**********************************
	std::tuple<int, int, double> get_pair(const double & gamma);

	/* assign the new parameters to the member variables in this class.
	 * Inputs:
	 * - i_max: (object i, object j) with maximum KL divergence;
	 * - j_max: (object i, object j) with maximum KL divergence;
	 */
	void keep_new_params(const NEW_PARAMS & new_para, const int & i_max,
		const int & j_max);

	/* For one worker, like worker k, he/she will select a pair (o_i, o_j),
	* 1) after he/she do once (i.e., 1 time ) voting, we could update the
	* parameters of those distribution according to Bayes' Theorem.
	* 2) If we consider the wrong voting probability that a worker will give wrong
	* voting result, probably due to the high similarity between o_i, o_j, or
	* due to the work quality or even the malicious behavior of this worker.
	*/
	vector<pair<int, int>> repeated_voting_1_worker(const int * p_rank_gt,
		const int & num_repeat, const vector<double> & v_scores,
		const vector<double> & v_layers, const double & gamma,
		const bool & IsDisplay, const bool & IsReverse,
		const pair<int, int> & p_ij);

	// calculate the Kendall-tau distance between two ranking:
	//  1) ranking 1: is the input, usually the ground truth ranking or
	//     the result obtained from other algorithms;
	//  2) ranking 2: is the ranking obtained by this algorithm.
	double get_kendall_tau_distance(
		// ranking 1
		const int * p_rank_gt, /*ground truth ranking*/
		const bool & IsDisplay);

	void Crowd_BT::repeated_voting_1_worker(
		const vector<int> & v_scores, // scores of each object.
		const int & n, // vertex number
		const int & distribution,
		const int & num_repeat,
		//const double & stddev, // Gaussian variance to control the worker's quality.
		const int& quality, // different levels of error-rate for the worker's quality.
		const bool & IsDisplay, const pair<int, int> & p_ij,
		const bool & Is_Beta_Simu_data_type // == true, using Beta distribution to simulate the voting result.
		);

};

double one_worker_reverse_or_not(const int & num_o, const int num_w,
	const int * p_rank_gt,
	const int & num_repeat, const vector<double> & v_scores,
	const vector<double> & v_layers,
	const double & gamma, const bool & IsDisplay, const bool & IsReverse,
	Crowd_BT & c_bt1);

#endif


#pragma once

#include "cfl_condition.hpp"
#include "mesh.hpp"
#include "numerical_flux.hpp"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
#include <fmt/format.h>

void print_progress(
    int n_steps,
    double T,
    double t,
    double dt,
    const std::chrono::high_resolution_clock::time_point &time_start);

Eigen::MatrixXd solveEuler(Eigen::MatrixXd U0, const Mesh &mesh, double T) {

    int n_cells = mesh.getNumberOfTriangles();

    Eigen::MatrixXd U_tmp(n_cells, 4);
    Eigen::MatrixXd dUdt(n_cells, 4);

    double t = 0;

    auto cfl_condition = CFLCondition(mesh);
    auto computeNetFlux = FluxRateOfChange(n_cells);

    int n_steps = 0;

    auto time_start = std::chrono::high_resolution_clock::now();

    std::cout << std::endl;
    while (t < T) {
        double dt = cfl_condition(U0);

        // Compute u^(*)
        computeNetFlux(dUdt, U0, mesh);
#pragma omp parallel for
        for (int i = 0; i < n_cells; ++i) {
            U_tmp.row(i) = U0.row(i) + dt * dUdt.row(i);
        }

        // Compute u^(**) and u^{n+1} in one step. We can update U0 directly.
        computeNetFlux(dUdt, U_tmp, mesh);
#pragma omp parallel for
        for (int i = 0; i < n_cells; ++i) {
            U0.row(i) = 0.5 * (U0.row(i) + U_tmp.row(i) + dt * dUdt.row(i));
        }

        t += dt;
        n_steps++;

        print_progress(n_steps, T, t, dt, time_start);
    }
    std::cout << "\n";

    return U0;
}

void print_progress(
    int n_steps,
    double T,
    double t,
    double dt,
    const std::chrono::high_resolution_clock::time_point &time_start) {

    auto now = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::duration<double>>(
        now - time_start);
    double elapsed = duration.count();
    double eta = elapsed * (T - t) / t;

    std::cout << fmt::format("\r"
                             "n_steps = {: 3d}, t = {:.3e}, dt = {:.2e}, "
                             "elased = {:6.1f}s, eta = {:6.1f}s",
                             n_steps,
                             t,
                             dt,
                             elapsed,
                             eta);
    std::cout.flush();
}