#pragma once

#include "craam/RMDP.hpp"

#include <Eigen/Dense>
#include <rm/range.hpp>

namespace craam{namespace algorithms{

using namespace std;
using namespace Eigen;

/// Internal helper functions
namespace internal{

    /// Helper function to deal with variable indexing
    template<class SType>
    inline Transition mean_transition_state(const SType& state, long index, const pair<indvec,vector<numvec>>& policies){
        return state.mean_transition(policies.first[index], policies.second[index]);
    }

    /// Helper function to deal with variable indexing
    template<class SType>
    inline Transition mean_transition_state(const SType& state, long index, const indvec& policy){
        return state.mean_transition(policy[index]);
    }

    /// Helper function to deal with variable indexing
    template<class SType>
    inline prec_t mean_reward_state(const SType& state, long index, const pair<indvec,vector<numvec>>& policies){
        return state.mean_reward(policies.first[index], policies.second[index]);
    }

    /// Helper function to deal with variable indexing
    template<class SType>
    inline prec_t mean_reward_state(const SType& state, long index, const indvec& policy){
        return state.mean_reward(policy[index]);
    }
}

/**
Constructs the transition (or its transpose) matrix for the policy.

\tparam SType Type of the state in the MDP (regular vs robust)
\tparam Policy Type of the policy. Either a single policy for
                the standard MDP evaluation, or a pair of a deterministic
                policy and a randomized policy of the nature
\param rmdp Regular or robust MDP
\param policies The policy (indvec) or the pair of the policy and the policy
        of nature (pair<indvec,vector<numvec> >). The nature is typically
        a randomized policy
\param transpose (optional, false) Whether to return the transpose of the transition matrix.
        This is useful for computing occupancy frequencies
*/
template<typename SType, typename Policies>
inline MatrixXd transition_mat(const GRMDP<SType>& rmdp, const Policies& policies, bool transpose = false) {
    const size_t n = rmdp.state_count();
    MatrixXd result = MatrixXd::Zero(n,n);

    const auto& states = rmdp.get_states();
    #pragma omp parallel for
    for(size_t s = 0; s < n; s++){
        const Transition&& t = internal::mean_transition_state(states[s], s, policies);

        const auto& indexes = t.get_indices();
        const auto& probabilities = t.get_probabilities();

        if(!transpose){
            for(size_t j=0; j < t.size(); j++)
                result(s,indexes[j]) = probabilities[j];
        }else{
            for(size_t j=0; j < t.size(); j++)
                result(indexes[j],s) = probabilities[j];
        }
    }
    return result;
}

/**
Constructs the rewards vector for each state for the RMDP.

\tparam Policy Type of the policy. Either a single policy for
                the standard MDP evaluation, or a pair of a deterministic
                policy and a randomized policy of the nature
\param rmdp Regular or robust MDP
\param policies The policy (indvec) or the pair of the policy and the policy
        of nature (pair<indvec,vector<numvec> >). The nature is typically
        a randomized policy
 */
template<typename SType, typename Policy>
inline numvec rewards_vec(const GRMDP<SType>& rmdp, const Policy& policies){

    const auto n = rmdp.state_count();
    numvec rewards(n);

    #pragma omp parallel for
    for(size_t s=0; s < n; s++){
        const SType& state = rmdp[s];
        if(state.is_terminal())
            rewards[s] = 0;
        else
            rewards[s] = internal::mean_reward_state(state, s, policies);
    }
    return rewards;
}

/**
Computes occupancy frequencies using matrix representation of transition
probabilities. This method may not scale well

\tparam SType Type of the state in the MDP (regular vs robust)
\tparam Policy Type of the policy. Either a single policy for
                the standard MDP evaluation, or a pair of a deterministic
                policy and a randomized policy of the nature
\param init Initial distribution (alpha)
\param discount Discount factor (gamma)
\param policies The policy (indvec) or the pair of the policy and the policy
        of nature (pair<indvec,vector<numvec> >). The nature is typically
        a randomized policy
*/
template<typename SType, typename Policies>
inline numvec
occfreq_mat(const GRMDP<SType>& rmdp, const Transition& init, prec_t discount,
                 const Policies& policies) {
    const auto n = rmdp.state_count();

    // initial distribution
    const numvec& ivec = init.probabilities_vector(n);
    const VectorXd initial_vec = Map<const VectorXd,Unaligned>(ivec.data(),ivec.size());

    // get transition matrix and construct (I - gamma * P^T)
    MatrixXd t_mat = MatrixXd::Identity(n,n)  - discount * transition_mat(rmdp, policies, true);

    // solve set of linear equations
    numvec result(n,0);
    Map<VectorXd,Unaligned>(result.data(),result.size()) = HouseholderQR<MatrixXd>(t_mat).solve(initial_vec);

    return result;
}

}}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_UBLAS_TYPE_CHECK_EPSILON (type_traits<real_type>::type_sqrt (boost::math::tools::epsilon <real_type>()))
#define BOOST_UBLAS_TYPE_CHECK_MIN (type_traits<real_type>::type_sqrt ( boost::math::tools::min_value<real_type>()))
#define BOOST_UBLAS_NDEBUG

#include <boost/math/bindings/rr.hpp>
namespace std{
using boost::math::ntl::pow;
} // workaround for spirit parser.
#include <boost/math/tools/remez.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_actor.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <iomanip>
#include <string>
#include <boost/test/included/test_exec_monitor.hpp> // for test_main

extern boost::math::ntl::RR f(const boost::math::ntl::RR& x, int variant);
extern void show_extra(
   const boost::math::tools::polynomial<boost::math::ntl::RR>& n,
   const boost::math::tools::polynomial<boost::math::ntl::RR>& d,
   const boost::math::ntl::RR& x_offset,
   const boost::math::ntl::RR& y_offset,
   int variant);

using namespace boost::spirit::classic;

boost::math::ntl::RR a(0), b(1);   // range to optimise over
bool rel_error(true);
bool pin(false);
int orderN(3);
int orderD(1);
int target_precision = boost::math::tools::digits<long double>();
int working_precision = target_precision * 2;
bool started(false);
int variant(0);
int skew(0);
int brake(50);
boost::math::ntl::RR x_offset(0), y_offset(0), x_scale(1);
bool auto_offset_y;

boost::shared_ptr<boost::math::tools::remez_minimax<boost::math::ntl::RR> > p_remez;

boost::math::ntl::RR the_function(const boost::math::ntl::RR& val)
{
   return f(x_scale * (val + x_offset), variant) + y_offset;
}

void step_some(unsigned count)
{
   try{
      NTL::RR::SetPrecision(working_precision);
      if(!started)
      {
         //
         // If we have an automatic y-offset calculate it now:
         //
         if(auto_offset_y)
         {
            boost::math::ntl::RR fa, fb, fm;
            fa = f(x_scale * (a + x_offset), variant);
            fb = f(x_scale * (b + x_offset), variant);
            fm = f(x_scale * ((a+b)/2 + x_offset), variant);
            y_offset = -(fa + fb + fm) / 3;
            NTL::RR::SetOutputPrecision(5);
            std::cout << "Setting auto-y-offset to " << y_offset << std::endl;
         }
         //
         // Truncate offsets to float precision:
         //
         x_offset = NTL::RoundToPrecision(x_offset.value(), 20);
         y_offset = NTL::RoundToPrecision(y_offset.value(), 20);
         //
         // Construct new Remez state machine:
         //
         p_remez.reset(new boost::math::tools::remez_minimax<boost::math::ntl::RR>(
            &the_function,
            orderN, orderD,
            a, b,
            pin,
            rel_error,
            skew,
            working_precision));
         std::cout << "Max error in interpolated form: " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(p_remez->max_error()) << std::endl;
         //
         // Signal that we've started:
         //
         started = true;
      }
      unsigned i;
      for(i = 0; i < count; ++i)
      {
         std::cout << "Stepping..." << std::endl;
         p_remez->set_brake(brake);
         boost::math::ntl::RR r = p_remez->iterate();
         NTL::RR::SetOutputPrecision(3);
         std::cout
            << "Maximum Deviation Found:                     " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(p_remez->max_error()) << std::endl
            << "Expected Error Term:                         " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(p_remez->error_term()) << std::endl
            << "Maximum Relative Change in Control Points:   " << std::setprecision(3) << std::scientific << boost::math::tools::real_cast<double>(r) << std::endl;
      }
   }
   catch(const std::exception& e)
   {
      std::cout << "Step failed with exception: " << e.what() << std::endl;
   }
}

void step(const char*, const char*)
{
   step_some(1);
}

void show(const char*, const char*)
{
   NTL::RR::SetPrecision(working_precision);
   if(started)
   {
      boost::math::tools::polynomial<boost::math::ntl::RR> n = p_remez->numerator();
      boost::math::tools::polynomial<boost::math::ntl::RR> d = p_remez->denominator();
      std::vector<boost::math::ntl::RR> cn = n.chebyshev();
      std::vector<boost::math::ntl::RR> cd = d.chebyshev();
      int prec = 2 + (target_precision * 3010LL)/10000;
      std::cout << std::scientific << std::setprecision(prec);
      NTL::RR::SetOutputPrecision(prec);
      boost::numeric::ublas::vector<boost::math::ntl::RR> v = p_remez->zero_points();

      std::cout << "  Zeros = {\n";
      unsigned i;
      for(i = 0; i < v.size(); ++i)
      {
         std::cout << "    " << v[i] << std::endl;
      }
      std::cout << "  }\n";

      v = p_remez->chebyshev_points();
      std::cout << "  Chebeshev Control Points = {\n";
      for(i = 0; i < v.size(); ++i)
      {
         std::cout << "    " << v[i] << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "X offset: " << x_offset << std::endl;
      std::cout << "X scale:  " << x_scale << std::endl;
      std::cout << "Y offset: " << y_offset << std::endl;

      std::cout << "P = {";
      for(i = 0; i < n.size(); ++i)
      {
         std::cout << "    " << n[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "Q = {";
      for(i = 0; i < d.size(); ++i)
      {
         std::cout << "    " << d[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "CP = {";
      for(i = 0; i < cn.size(); ++i)
      {
         std::cout << "    " << cn[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      std::cout << "CQ = {";
      for(i = 0; i < cd.size(); ++i)
      {
         std::cout << "    " << cd[i] << "L," << std::endl;
      }
      std::cout << "  }\n";

      show_extra(n, d, x_offset, y_offset, variant);
   }
   else
   {
      std::cerr << "Nothing to display" << std::endl;
   }
}

void do_graph(unsigned points)
{
   NTL::RR::SetPrecision(working_precision);
   boost::math::ntl::RR step = (b - a) / (points - 1);
   boost::math::ntl::RR x = a;
   while(points > 1)
   {
      NTL::RR::SetOutputPrecision(10);
      std::cout << std::setprecision(10) << std::setw(30) << std::left
         << boost::lexical_cast<std::string>(x) << the_function(x) << std::endl;
      --points;
      x += step;
   }
   std::cout << std::setprecision(10) << std::setw(30) << std::left
      << boost::lexical_cast<std::string>(b) << the_function(b) << std::endl;
}

void graph(const char*, const char*)
{
   do_graph(3);
}

template <class T>
void do_test(T, const char* name)
{
   boost::math::ntl::RR::SetPrecision(working_precision);
   if(started)
   {
      //
      // We want to test the approximation at fixed precision:
      // either float, double or long double.  Begin by getting the
      // polynomials:
      //
      boost::math::tools::polynomial<T> n, d;
      boost::math::tools::polynomial<boost::math::ntl::RR> nr, dr;
      nr = p_remez->numerator();
      dr = p_remez->denominator();
      n = nr;
      d = dr;

      std::vector<boost::math::ntl::RR> cn1, cd1;
      cn1 = nr.chebyshev();
      cd1 = dr.chebyshev();
      std::vector<T> cn, cd;
      for(unsigned i = 0; i < cn1.size(); ++i)
      {
         cn.push_back(boost::math::tools::real_cast<T>(cn1[i]));
      }
      for(unsigned i = 0; i < cd1.size(); ++i)
      {
         cd.push_back(boost::math::tools::real_cast<T>(cd1[i]));
      }
      //
      // We'll test at the Chebeshev control points which is where
      // (in theory) the largest deviation should occur.  For good
      // measure we'll test at the zeros as well:
      //
      boost::numeric::ublas::vector<boost::math::ntl::RR>
         zeros(p_remez->zero_points()),
         cheb(p_remez->chebyshev_points());

      boost::math::ntl::RR max_error(0), cheb_max_error(0);

      //
      // Do the tests at the zeros:
      //
      std::cout << "Starting tests at " << name << " precision...\n";
      std::cout << "Absissa        Error (Poly)   Error (Cheb)\n";
      for(unsigned i = 0; i < zeros.size(); ++i)
      {
         boost::math::ntl::RR true_result = the_function(zeros[i]);
         T absissa = boost::math::tools::real_cast<T>(zeros[i]);
         boost::math::ntl::RR test_result = n.evaluate(absissa) / d.evaluate(absissa);
         boost::math::ntl::RR cheb_result = boost::math::tools::evaluate_chebyshev(cn, absissa) / boost::math::tools::evaluate_chebyshev(cd, absissa);
         boost::math::ntl::RR err, cheb_err;
         if(rel_error)
         {
            err = boost::math::tools::relative_error(test_result, true_result);
            cheb_err = boost::math::tools::relative_error(cheb_result, true_result);
         }
         else
         {
            err = fabs(test_result - true_result);
            cheb_err = fabs(cheb_result - true_result);
         }
         if(err > max_error)
            max_error = err;
         if(cheb_err > cheb_max_error)
            cheb_max_error = cheb_err;
         std::cout << std::setprecision(6) << std::setw(15) << std::left << absissa
            << std::setw(15) << std::left << boost::math::tools::real_cast<T>(err) << boost::math::tools::real_cast<T>(cheb_err) << std::endl;
      }
      //
      // Do the tests at the Chebeshev control points:
      //
      for(unsigned i = 0; i < cheb.size(); ++i)
      {
         boost::math::ntl::RR true_result = the_function(cheb[i]);
         T absissa = boost::math::tools::real_cast<T>(cheb[i]);
         boost::math::ntl::RR test_result = n.evaluate(absissa) / d.evaluate(absissa);
         boost::math::ntl::RR cheb_result = boost::math::tools::evaluate_chebyshev(cn, absissa) / boost::math::tools::evaluate_chebyshev(cd, absissa);
         boost::math::ntl::RR err, cheb_err;
         if(rel_error)
         {
            err = boost::math::tools::relative_error(test_result, true_result);
            cheb_err = boost::math::tools::relative_error(cheb_result, true_result);
         }
         else
         {
            err = fabs(test_result - true_result);
            cheb_err = fabs(cheb_result - true_result);
         }
         if(err > max_error)
            max_error = err;
         std::cout << std::setprecision(6) << std::setw(15) << std::left << absissa
            << std::setw(15) << std::left << boost::math::tools::real_cast<T>(err) <<
            boost::math::tools::real_cast<T>(cheb_err) << std::endl;
      }
      std::string msg = "Max Error found at ";
      msg += name;
      msg += " precision = ";
      msg.append(62 - 17 - msg.size(), ' ');
      std::cout << msg << std::setprecision(6) << "Poly: " << std::setw(20) << std::left
         << boost::math::tools::real_cast<T>(max_error) << "Cheb: " << boost::math::tools::real_cast<T>(cheb_max_error) << std::endl;
   }
   else
   {
      std::cout << "Nothing to test: try converging an approximation first!!!" << std::endl;
   }
}

void test_float(const char*, const char*)
{
   do_test(float(0), "float");
}

void test_double(const char*, const char*)
{
   do_test(double(0), "double");
}

void test_long(const char*, const char*)
{
   do_test((long double)(0), "long double");
}

void test_all(const char*, const char*)
{
   do_test(float(0), "float");
   do_test(double(0), "double");
   do_test((long double)(0), "long double");
}

template <class T>
void do_test_n(T, const char* name, unsigned count)
{
   boost::math::ntl::RR::SetPrecision(working_precision);
   if(started)
   {
      //
      // We want to test the approximation at fixed precision:
      // either float, double or long double.  Begin by getting the
      // polynomials:
      //
      boost::math::tools::polynomial<T> n, d;
      boost::math::tools::polynomial<boost::math::ntl::RR> nr, dr;
      nr = p_remez->numerator();
      dr = p_remez->denominator();
      n = nr;
      d = dr;

      std::vector<boost::math::ntl::RR> cn1, cd1;
      cn1 = nr.chebyshev();
      cd1 = dr.chebyshev();
      std::vector<T> cn, cd;
      for(unsigned i = 0; i < cn1.size(); ++i)
      {
         cn.push_back(boost::math::tools::real_cast<T>(cn1[i]));
      }
      for(unsigned i = 0; i < cd1.size(); ++i)
      {
         cd.push_back(boost::math::tools::real_cast<T>(cd1[i]));
      }

      boost::math::ntl::RR max_error(0), max_cheb_error(0);
      boost::math::ntl::RR step = (b - a) / count;

      //
      // Do the tests at the zeros:
      //
      std::cout << "Starting tests at " << name << " precision...\n";
      std::cout << "Absissa        Error (poly)   Error (Cheb)\n";
      for(boost::math::ntl::RR x = a; x <= b; x += step)
      {
         boost::math::ntl::RR true_result = the_function(x);
         T absissa = boost::math::tools::real_cast<T>(x);
         boost::math::ntl::RR test_result = n.evaluate(absissa) / d.evaluate(absissa);
         boost::math::ntl::RR cheb_result = boost::math::tools::evaluate_chebyshev(cn, absissa) / boost::math::tools::evaluate_chebyshev(cd, absissa);
         boost::math::ntl::RR err, cheb_err;
         if(rel_error)
         {
            err = boost::math::tools::relative_error(test_result, true_result);
            cheb_err = boost::math::tools::relative_error(cheb_result, true_result);
         }
         else
         {
            err = fabs(test_result - true_result);
            cheb_err = fabs(cheb_result - true_result);
         }
         if(err > max_error)
            max_error = err;
         if(cheb_err > max_cheb_error)
            max_cheb_error = cheb_err;
         std::cout << std::setprecision(6) << std::setw(15) << std::left << boost::math::tools::real_cast<double>(absissa)
            << (test_result < true_result ? "-" : "") << std::setw(20) << std::left
            << boost::math::tools::real_cast<double>(err)
            << boost::math::tools::real_cast<double>(cheb_err) << std::endl;
      }
      std::string msg = "Max Error found at ";
      msg += name;
      msg += " precision = ";
      //msg.append(62 - 17 - msg.size(), ' ');
      std::cout << msg << "Poly: " << std::setprecision(6)
         //<< std::setw(15) << std::left
         << boost::math::tools::real_cast<T>(max_error)
         << " Cheb: " << boost::math::tools::real_cast<T>(max_cheb_error) << std::endl;
   }
   else
   {
      std::cout << "Nothing to test: try converging an approximation first!!!" << std::endl;
   }
}

void test_n(unsigned n)
{
   do_test_n(boost::math::ntl::RR(), "boost::math::ntl::RR", n);
}

void test_float_n(unsigned n)
{
   do_test_n(float(0), "float", n);
}

void test_double_n(unsigned n)
{
   do_test_n(double(0), "double", n);
}

void test_long_n(unsigned n)
{
   do_test_n((long double)(0), "long double", n);
}

void rotate(const char*, const char*)
{
   if(p_remez)
   {
      p_remez->rotate();
   }
   else
   {
      std::cerr << "Nothing to rotate" << std::endl;
   }
}

void rescale(const char*, const char*)
{
   if(p_remez)
   {
      p_remez->rescale(a, b);
   }
   else
   {
      std::cerr << "Nothing to rescale" << std::endl;
   }
}

void graph_poly(const char*, const char*)
{
   int i = 50;
   boost::math::ntl::RR::SetPrecision(working_precision);
   if(started)
   {
      //
      // We want to test the approximation at fixed precision:
      // either float, double or long double.  Begin by getting the
      // polynomials:
      //
      boost::math::tools::polynomial<boost::math::ntl::RR> n, d;
      n = p_remez->numerator();
      d = p_remez->denominator();

      boost::math::ntl::RR max_error(0);
      boost::math::ntl::RR step = (b - a) / i;

      std::cout << "Evaluating Numerator...\n";
      boost::math::ntl::RR val;
      for(val = a; val <= b; val += step)
         std::cout << n.evaluate(val) << std::endl;
      std::cout << "Evaluating Denominator...\n";
      for(val = a; val <= b; val += step)
         std::cout << d.evaluate(val) << std::endl;
   }
   else
   {
      std::cout << "Nothing to test: try converging an approximation first!!!" << std::endl;
   }
}

int test_main(int, char* [])
{
   std::string line;
   real_parser<long double/*boost::math::ntl::RR*/ > const rr_p;
   while(std::getline(std::cin, line))
   {
      if(parse(line.c_str(), str_p("quit"), space_p).full)
         return 0;
      if(false == parse(line.c_str(),
         (

            str_p("range")[assign_a(started, false)] && real_p[assign_a(a)] && real_p[assign_a(b)]
      ||
            str_p("relative")[assign_a(started, false)][assign_a(rel_error, true)]
      ||
            str_p("absolute")[assign_a(started, false)][assign_a(rel_error, false)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("true")[assign_a(pin, true)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("false")[assign_a(pin, false)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("1")[assign_a(pin, true)]
      ||
            str_p("pin")[assign_a(started, false)] && str_p("0")[assign_a(pin, false)]
      ||
            str_p("pin")[assign_a(started, false)][assign_a(pin, true)]
      ||
            str_p("order")[assign_a(started, false)] && uint_p[assign_a(orderN)] && uint_p[assign_a(orderD)]
      ||
            str_p("order")[assign_a(started, false)] && uint_p[assign_a(orderN)]
      ||
            str_p("target-precision") && uint_p[assign_a(target_precision)]
      ||
            str_p("working-precision")[assign_a(started, false)] && uint_p[assign_a(working_precision)]
      ||
            str_p("variant")[assign_a(started, false)] && int_p[assign_a(variant)]
      ||
            str_p("skew")[assign_a(started, false)] && int_p[assign_a(skew)]
      ||
            str_p("brake") && int_p[assign_a(brake)]
      ||
            str_p("step") && int_p[&step_some]
      ||
            str_p("step")[&step]
      ||
            str_p("poly")[&graph_poly]
      ||
            str_p("info")[&show]
      ||
            str_p("graph") && uint_p[&do_graph]
      ||
            str_p("graph")[&graph]
      ||
            str_p("x-offset") && real_p[assign_a(x_offset)]
      ||
            str_p("x-scale") && real_p[assign_a(x_scale)]
      ||
            str_p("y-offset") && str_p("auto")[assign_a(auto_offset_y, true)]
      ||
            str_p("y-offset") && real_p[assign_a(y_offset)][assign_a(auto_offset_y, false)]
      ||
            str_p("test") && str_p("float") && uint_p[&test_float_n]
      ||
            str_p("test") && str_p("float")[&test_float]
      ||
            str_p("test") && str_p("double") && uint_p[&test_double_n]
      ||
            str_p("test") && str_p("double")[&test_double]
      ||
            str_p("test") && str_p("long") && uint_p[&test_long_n]
      ||
            str_p("test") && str_p("long")[&test_long]
      ||
            str_p("test") && str_p("all")[&test_all]
      ||
            str_p("test") && uint_p[&test_n]
      ||
            str_p("rotate")[&rotate]
      ||
            str_p("rescale") && real_p[assign_a(a)] && real_p[assign_a(b)] && epsilon_p[&rescale]

         ), space_p).full)
      {
         std::cout << "Unable to parse directive: \"" << line << "\"" << std::endl;
      }
      else
      {
         std::cout << "Variant              = " << variant << std::endl;
         std::cout << "range                = [" << a << "," << b << "]" << std::endl;
         std::cout << "Relative Error       = " << rel_error << std::endl;
         std::cout << "Pin to Origin        = " << pin << std::endl;
         std::cout << "Order (Num/Denom)    = " << orderN << "/" << orderD << std::endl;
         std::cout << "Target Precision     = " << target_precision << std::endl;
         std::cout << "Working Precision    = " << working_precision << std::endl;
         std::cout << "Skew                 = " << skew << std::endl;
         std::cout << "Brake                = " << brake << std::endl;
         std::cout << "X Offset             = " << x_offset << std::endl;
         std::cout << "X scale              = " << x_scale << std::endl;
         std::cout << "Y Offset             = ";
         if(auto_offset_y)
            std::cout << "Auto (";
         std::cout << y_offset;
         if(auto_offset_y)
            std::cout << ")";
         std::cout << std::endl;
     }
   }
   return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  InitializePose3.h
 *  @author Luca Carlone
 *  @author Frank Dellaert
 *  @date   August, 2014
 */

#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/timing.h>

#include <boost/math/special_functions.hpp>

using namespace std;

namespace gtsam {
namespace InitializePose3 {

static const Matrix I9 = I_9x9;
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33 = Z_3x3;

static const Key keyAnchor = symbol('Z', 9999999);

/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {

  GaussianFactorGraph linearGraph;
  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);

  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
    Matrix3 Rij;

    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between)
      Rij = pose3Between->measured().rotation().matrix();
    else
      std::cout << "Error in buildLinearOrientationGraph" << std::endl;

    const FastVector<Key>& keys = factor->keys();
    Key key1 = keys[0], key2 = keys[1];
    Matrix M9 = Z_9x9;
    M9.block(0,0,3,3) = Rij;
    M9.block(3,3,3,3) = Rij;
    M9.block(6,6,3,3) = Rij;
    linearGraph.add(key1, -I9, key2, M9, zero9, model);
  }
  // prior on the anchor orientation
  linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0).finished(), model);
  return linearGraph;
}

/* ************************************************************************* */
// Transform VectorValues into valid Rot3
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
  gttic(InitializePose3_computeOrientationsChordal);

  Matrix ppm = Z_3x3; // plus plus minus
  ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;

  Values validRot3;
  for(const VectorValues::value_type& it: relaxedRot3) {
    Key key = it.first;
    if (key != keyAnchor) {
      const Vector& rotVector = it.second;
      Matrix3 rotMat;
      rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
      rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
      rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);

      Matrix U, V; Vector s;
      svd(rotMat, U, s, V);
      Matrix3 normalizedRotMat = U * V.transpose();

      //      std::cout << "rotMat \n" << rotMat << std::endl;
      //      std::cout << "U V' \n" << U * V.transpose() << std::endl;
      //      std::cout << "V \n" << V << std::endl;

      if(normalizedRotMat.determinant() < 0)
        normalizedRotMat = U * ppm * V.transpose();

      Rot3 initRot = Rot3(normalizedRotMat);
      validRot3.insert(key, initRot);
    }
  }
  return validRot3;
}

/* ************************************************************************* */
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
  gttic(InitializePose3_buildPose3graph);
  NonlinearFactorGraph pose3Graph;

  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {

    // recast to a between on Pose3
    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between)
      pose3Graph.add(pose3Between);

    // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
    boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
        boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
    if (pose3Prior)
      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
              pose3Prior->prior(), pose3Prior->noiseModel());
  }
  return pose3Graph;
}

/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
  gttic(InitializePose3_computeOrientationsChordal);

  // regularize measurements and plug everything in a factor graph
  GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);

  // Solve the LFG
  VectorValues relaxedRot3 = relaxedGraph.optimize();

  // normalize and compute Rot3
  return normalizeRelaxedRotations(relaxedRot3);
}

/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
  gttic(InitializePose3_computeOrientationsGradient);

  // this works on the inverse rotations, according to Tron&Vidal,2011
  Values inverseRot;
  inverseRot.insert(keyAnchor, Rot3());
  for(const Values::ConstKeyValuePair& key_value: givenGuess) {
    Key key = key_value.key;
    const Pose3& pose = givenGuess.at<Pose3>(key);
    inverseRot.insert(key, pose.rotation().inverse());
  }

  // Create the map of edges incident on each node
  KeyVectorMap adjEdgesMap;
  KeyRotMap factorId2RotMap;

  createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);

  // calculate max node degree & allocate gradient
  size_t maxNodeDeg = 0;
  VectorValues grad;
  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
    Key key = key_value.key;
    grad.insert(key,Vector3::Zero());
    size_t currNodeDeg = (adjEdgesMap.at(key)).size();
    if(currNodeDeg > maxNodeDeg)
      maxNodeDeg = currNodeDeg;
  }

  // Create parameters
  double b = 1;
  double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
  double a = (M_PI*M_PI)/(2*f0);
  double rho = 2*a*b;
  double mu_max = maxNodeDeg * rho;
  double stepsize = 2/mu_max; // = 1/(a b dG)

  std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
  double maxGrad;
  // gradient iterations
  size_t it;
  for(it=0; it < maxIter; it++){
    //////////////////////////////////////////////////////////////////////////
    // compute the gradient at each node
    //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
    //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
    maxGrad = 0;
    for(const Values::ConstKeyValuePair& key_value: inverseRot) {
      Key key = key_value.key;
      //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
      Vector gradKey = Vector3::Zero();
      // collect the gradient for each edge incident on key
      for(const size_t& factorId: adjEdgesMap.at(key)){
        Rot3 Rij = factorId2RotMap.at(factorId);
        Rot3 Ri = inverseRot.at<Rot3>(key);
        if( key == (pose3Graph.at(factorId))->keys()[0] ){
          Key key1 = (pose3Graph.at(factorId))->keys()[1];
          Rot3 Rj = inverseRot.at<Rot3>(key1);
          gradKey = gradKey + gradientTron(Ri, Rij  * Rj, a, b);
          //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij  * Rj, a, b) \n " << gradientTron(Ri, Rij  * Rj, a, b) << std::endl;
        }else if( key == (pose3Graph.at(factorId))->keys()[1] ){
          Key key0 = (pose3Graph.at(factorId))->keys()[0];
          Rot3 Rj = inverseRot.at<Rot3>(key0);
          gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
          //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse()  * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
        }else{
          std::cout << "Error in gradient computation" << std::endl;
        }
      } // end of i-th gradient computation
      grad.at(key) = stepsize *  gradKey;

      double normGradKey = (gradKey).norm();
      //std::cout << "key  " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
      if(normGradKey>maxGrad)
        maxGrad = normGradKey;
    } // end of loop over nodes

    //////////////////////////////////////////////////////////////////////////
    // update estimates
    inverseRot = inverseRot.retract(grad);

    //////////////////////////////////////////////////////////////////////////
    // check stopping condition
    if (it>20 && maxGrad < 5e-3)
      break;
  } // enf of gradient iterations

  std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl;

  // Return correct rotations
  const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
  Values estimateRot;
  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
    Key key = key_value.key;
    if (key != keyAnchor) {
      const Rot3& R = inverseRot.at<Rot3>(key);
      if(setRefFrame)
        estimateRot.insert(key, Rref.compose(R.inverse()));
      else
        estimateRot.insert(key, R.inverse());
    }
  }
  return estimateRot;
}

/* ************************************************************************* */
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
  size_t factorId = 0;
  for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between){
      Rot3 Rij = pose3Between->measured().rotation();
      factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));

      Key key1 = pose3Between->key1();
      if (adjEdgesMap.find(key1) != adjEdgesMap.end()){  // key is already in
        adjEdgesMap.at(key1).push_back(factorId);
      }else{
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
      }
      Key key2 = pose3Between->key2();
      if (adjEdgesMap.find(key2) != adjEdgesMap.end()){  // key is already in
        adjEdgesMap.at(key2).push_back(factorId);
      }else{
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
      }
    }else{
      std::cout << "Error in computeOrientationsGradient" << std::endl;
    }
    factorId++;
  }
}

/* ************************************************************************* */
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
  Vector3 logRot = Rot3::Logmap(R1.between(R2));

  double th = logRot.norm();
  if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
    Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation
    logRot = Rot3::Logmap(R1pert.between(R2));
    th = logRot.norm();
  }
  // exclude small or invalid rotations
  if (th > 1e-5 && th == th){ // nonzero valid rotations
    logRot = logRot / th;
  }else{
    logRot = Vector3::Zero();
    th = 0.0;
  }

  double fdot = a*b*th*exp(-b*th);
  return fdot*logRot;
}

/* ************************************************************************* */
Values initializeOrientations(const NonlinearFactorGraph& graph) {

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  return computeOrientationsChordal(pose3Graph);
}

///* ************************************************************************* */
Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
  gttic(InitializePose3_computePoses);

  // put into Values structure
  Values initialPose;
  for(const Values::ConstKeyValuePair& key_value: initialRot){
    Key key = key_value.key;
    const Rot3& rot = initialRot.at<Rot3>(key);
    Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
    initialPose.insert(key, initializedPose);
  }
  // add prior
  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
  initialPose.insert(keyAnchor, Pose3());
  pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);

  // Create optimizer
  GaussNewtonParams params;
  bool singleIter = true;
  if(singleIter){
    params.maxIterations = 1;
  }else{
    std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl;
    params.setVerbosity("TERMINATION");
  }
  GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
  Values GNresult = optimizer.optimize();

  // put into Values structure
  Values estimate;
  for(const Values::ConstKeyValuePair& key_value: GNresult) {
    Key key = key_value.key;
    if (key != keyAnchor) {
      const Pose3& pose = GNresult.at<Pose3>(key);
      estimate.insert(key, pose);
    }
  }
  return estimate;
}

/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph) {
  gttic(InitializePose3_initialize);

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  Values valueRot3 = computeOrientationsChordal(pose3Graph);

  // Compute the full poses (1 GN iteration on full poses)
  return computePoses(pose3Graph, valueRot3);
}

/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
  Values initialValues;

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  Values orientations;
  if(useGradient)
    orientations = computeOrientationsGradient(pose3Graph, givenGuess);
  else
    orientations = computeOrientationsChordal(pose3Graph);

//  orientations.print("orientations\n");

  // Compute the full poses (1 GN iteration on full poses)
  return computePoses(pose3Graph, orientations);

  //  for(const Values::ConstKeyValuePair& key_value: orientations) {
  //    Key key = key_value.key;
  //    if (key != keyAnchor) {
  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
  //      const Rot3& rot = orientations.at<Rot3>(key);
  //      Pose3 initializedPoses = Pose3(rot, pos);
  //      initialValues.insert(key, initializedPoses);
  //    }
  //  }
  //  return initialValues;
}

} // end of namespace lago
} // end of namespace gtsam


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file  Pose3.cpp
 * @brief 3D Pose
 */

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <cmath>

using namespace std;

namespace gtsam {

/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3);

/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);

static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
static const Matrix6 I6 = eye(6);

/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
    R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
        Point3(pose2.x(), pose2.y(), 0)) {
}

/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const {
  const Matrix3 R = R_.matrix();
  const Vector3 t = t_.vector();
  Matrix3 A = skewSymmetric(t) * R;
  Matrix6 adj;
  adj << R, Z3, A, R;
  return adj;
}

/* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector& xi) {
  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
  Matrix6 adj;
  adj << w_hat, Z3, v_hat, w_hat;

  return adj;
}

/* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
    boost::optional<Matrix&> H) {
  if (H) {
    *H = zeros(6, 6);
    for (int i = 0; i < 6; ++i) {
      Vector dxi = zero(6);
      dxi(i) = 1.0;
      Matrix Gi = adjointMap(dxi);
      (*H).col(i) = Gi * y;
    }
  }
  return adjointMap(xi) * y;
}

/* ************************************************************************* */
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
    boost::optional<Matrix&> H) {
  if (H) {
    *H = zeros(6, 6);
    for (int i = 0; i < 6; ++i) {
      Vector dxi = zero(6);
      dxi(i) = 1.0;
      Matrix GTi = adjointMap(dxi).transpose();
      (*H).col(i) = GTi * y;
    }
  }
  Matrix adjT = adjointMap(xi).transpose();
  return adjointMap(xi).transpose() * y;
}

/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
  // Bernoulli numbers, from Wikipedia
  static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
      0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
  static const int N = 5; // order of approximation
  Matrix res = I6;
  Matrix6 ad_i = I6;
  Matrix6 ad_xi = adjointMap(xi);
  double fac = 1.0;
  for (int i = 1; i < N; ++i) {
    ad_i = ad_xi * ad_i;
    fac = fac * i;
    res = res + B(i) / fac * ad_i;
  }
  return res;
}

/* ************************************************************************* */
void Pose3::print(const string& s) const {
  cout << s;
  R_.print("R:\n");
  t_.print("t: ");
}

/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
  return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
}

/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi) {

  // get angular velocity omega and translational velocity v from twist xi
  Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));

  double theta = w.norm();
  if (theta < 1e-10) {
    static const Rot3 I;
    return Pose3(I, v);
  } else {
    Point3 n(w / theta); // axis unit vector
    Rot3 R = Rot3::rodriguez(n.vector(), theta);
    double vn = n.dot(v); // translation parallel to n
    Point3 n_cross_v = n.cross(v); // points towards axis
    Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
    return Pose3(R, t);
  }
}

/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p) {
  Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
  double t = w.norm();
  if (t < 1e-10) {
    Vector6 log;
    log << w, T;
    return log;
  } else {
    Matrix3 W = skewSymmetric(w / t);
    // Formula from Agrawal06iros, equation (14)
    // simplified with Mathematica, and multiplying in T to avoid matrix math
    double Tan = tan(0.5 * t);
    Vector3 WT = W * T;
    Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
    Vector6 log;
    log << w, u;
    return log;
  }
}

/* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
  Vector3 omega(sub(xi, 0, 3));
  Point3 v(sub(xi, 3, 6));
  Rot3 R = R_.retract(omega); // R is done exactly
  Point3 t = t_ + R_ * v; // First order t approximation
  return Pose3(R, t);
}

/* ************************************************************************* */
// Different versions of retract
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
  if (mode == Pose3::EXPMAP) {
    // Lie group exponential map, traces out geodesic
    return compose(Expmap(xi));
  } else if (mode == Pose3::FIRST_ORDER) {
    // First order
    return retractFirstOrder(xi);
  } else {
    // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
    // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
// different versions of localCoordinates
Vector6 Pose3::localCoordinates(const Pose3& T,
    Pose3::CoordinatesMode mode) const {
  if (mode == Pose3::EXPMAP) {
    // Lie group logarithm map, exact inverse of exponential map
    return Logmap(between(T));
  } else if (mode == Pose3::FIRST_ORDER) {
    // R is always done exactly in all three retract versions below
    Vector3 omega = R_.localCoordinates(T.rotation());

    // Incorrect version
    // Independently computes the logmap of the translation and rotation
    // Vector v = t_.localCoordinates(T.translation());

    // Correct first order t inverse
    Point3 d = R_.unrotate(T.translation() - t_);

    // TODO: correct second order t inverse
    Vector6 local;
    local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z();
    return local;
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
  const Matrix3 R = R_.matrix();
  const Vector3 T = t_.vector();
  Eigen::Matrix<double, 1, 4> A14;
  A14 << 0.0, 0.0, 0.0, 1.0;
  Matrix4 mat;
  mat << R, T, A14;
  return mat;
}

/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
  Rot3 cRv = R_ * Rot3(pose.R_.inverse());
  Point3 t = pose.transform_to(t_);
  return Pose3(cRv, t);
}

/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
    boost::optional<Matrix&> Dpoint) const {
  if (Dpose) {
    const Matrix R = R_.matrix();
    Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    Dpose->resize(3, 6);
    (*Dpose) << DR, R;
  }
  if (Dpoint)
    *Dpoint = R_.matrix();
  return R_ * p + t_;
}

/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
    boost::optional<Matrix&> Dpoint) const {
  const Point3 result = R_.unrotate(p - t_);
  if (Dpose) {
    const Point3& q = result;
    Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
    Dpose->resize(3, 6);
    (*Dpose) << DR, _I3;
  }
  if (Dpoint)
    *Dpoint = R_.transpose();
  return result;
}

/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  if (H1)
    *H1 = p2.inverse().AdjointMap();
  if (H2)
    *H2 = I6;
  return (*this) * p2;
}

/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
  if (H1)
    *H1 = -AdjointMap();
  Rot3 Rt = R_.inverse();
  return Pose3(Rt, Rt * (-t_));
}

/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  Pose3 result = inverse() * p2;
  if (H1)
    *H1 = -result.inverse().AdjointMap();
  if (H2)
    *H2 = I6;
  return result;
}

/* ************************************************************************* */
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  if (!H1 && !H2)
    return transform_to(point).norm();
  Point3 d = transform_to(point, H1, H2);
  double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
      d2);
  Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n);
  if (H1)
    *H1 = D_result_d * (*H1);
  if (H2)
    *H2 = D_result_d * (*H2);
  return n;
}

/* ************************************************************************* */
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  double r = range(point.translation(), H1, H2);
  if (H2) {
    Matrix H2_ = *H2 * point.rotation().matrix();
    *H2 = zeros(1, 6);
    insertSub(*H2, H2_, 0, 3);
  }
  return r;
}

/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
  const size_t n = pairs.size();
  if (n < 3)
    return boost::none; // we need at least three pairs

  // calculate centroids
  Vector cp = zero(3), cq = zero(3);
  BOOST_FOREACH(const Point3Pair& pair, pairs){
  cp += pair.first.vector();
  cq += pair.second.vector();
}
  double f = 1.0 / n;
  cp *= f;
  cq *= f;

  // Add to form H matrix
  Matrix H = zeros(3, 3);
  BOOST_FOREACH(const Point3Pair& pair, pairs){
  Vector dp = pair.first.vector() - cp;
  Vector dq = pair.second.vector() - cq;
  H += dp * dq.transpose();
}

// Compute SVD
  Matrix U, V;
  Vector S;
  svd(H, U, S, V);

  // Recover transform with correction from Eggert97machinevisionandapplications
  Matrix UVtranspose = U * V.transpose();
  Matrix detWeighting = eye(3, 3);
  detWeighting(2, 2) = UVtranspose.determinant();
  Rot3 R(Matrix(V * detWeighting * U.transpose()));
  Point3 t = Point3(cq) - R * Point3(cp);
  return Pose3(R, t);
}

/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
  os << pose.rotation() << "\n" << pose.translation() << endl;
  return os;
}

} // namespace gtsam


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include <boost/python/def.hpp>

#include "pxr/pxr.h"
#include "pxr/base/gf/homogeneous.h"

using namespace boost::python;

PXR_NAMESPACE_USING_DIRECTIVE

void wrapHomogeneous()
{
    def("GetHomogenized", (GfVec4d (*)(const GfVec4d &)) GfGetHomogenized);
    def("GetHomogenized", (GfVec4f (*)(const GfVec4f &)) GfGetHomogenized);

    def("HomogeneousCross", (GfVec4d (*)(const GfVec4d &, const GfVec4d &)) GfHomogeneousCross);
    def("HomogeneousCross", (GfVec4f (*)(const GfVec4f &, const GfVec4f &)) GfHomogeneousCross);

    def("Project", (GfVec3d (*)(const GfVec4d &)) GfProject);
    def("Project", (GfVec3f (*)(const GfVec4f &)) GfProject);
}


#include <cmath>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <sys/time.h>
#include <chrono>
#include <sys/resource.h>   // check the memory usage
#include <stdio.h>
#include <thread>
#include <fstream>
#include <sstream>

#include <NTL/RR.h>
#include <NTL/xdouble.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/BasicThreadPool.h>

#include "../src/CZZ.h"
#include "../src/Params.h"
#include "../src/PubKey.h"
#include "../src/Scheme.h"
#include "../src/SchemeAlgo.h"
#include "../src/SecKey.h"
#include "../src/TestScheme.h"
#include "../src/TimeUtils.h"
#include "../src/Ring2Utils.h"
#include "../src/StringUtils.h"
#include "../src/EvaluatorUtils.h"

#include "Database.h"
#include "LRtest.h"
#include "HELR.h"

using namespace NTL;
using namespace std;

int main(int Argc, char** Argv) {

    if(Argc != 3){
        cout << "-------------------------------------------------------------" << endl;
        cerr << "Enter the File and degree of approximation \t"  << "(e.g. $test edin.txt 3) \n ";
    }

    char* filename  =  Argv[1];
    int polydeg = atoi(Argv[2]);  // degree of approximation polynomial

    dMat  zData;
    dMat* zTest = new dMat[5];
    dMat* zTrain = new dMat[5];

    int nLine= readData(zData, filename);

    cout << "Sample the learning and test data ..." << endl;
    cvRandomSamplingData(zTrain, zTest, zData, filename);

    //----------------------------------------------------------------
    // Parameters for Logistic regression
    //----------------------------------------------------------------

    long logN= 17;
    long logp= 28;
    long logl= 10;
    long logq, cBit1, cBit2;
    int max_iter;

    struct LRpar LRparams;
    ReadLRparams(LRparams, max_iter, zTrain[0], polydeg, logp);

    SetNumThreads(LRparams.dim1);
    //SetNumThreads(4);

    switch(polydeg){
        case 3:
            cBit1=  (LRparams.logn - LRparams.log2polyscale);          // 1st iteration
            cBit2 =  (3*logp+ LRparams. logn - LRparams.log2polyscale);  // 2nd~ iteration
            logq = cBit1 + (LRparams.max_iter-1)*(cBit2)+ logp + logl;                  // max-bitlength we need
            break;

        case 7:
            cBit1=  (LRparams.logn - LRparams.log2polyscale);          // 1st iteration
            cBit2=  (4*logp+ LRparams.logn - LRparams.log2polyscale);
            logq= cBit1 + (LRparams.max_iter-1)*(cBit2)+ logp + logl;
            break;
    }

    cout << "Data dimension with dummy vectors: " << LRparams.dim1 << ", Number of lines: " << nLine << endl;

    cout << "-------------------------------------------------------------" << endl;
    cout << "Key Generation ... (logN,logp,logq, nslots)= (" ;
    cout << logN << "," << logp << "," << logq << "," << LRparams.nslots << ")" <<endl;

    auto start= chrono::steady_clock::now();

    Params params(logN, logq);
    SecKey secretKey(params);
    PubKey publicKey(params, secretKey);
    SchemeAux schemeaux(logN);
    Scheme scheme(params, publicKey, schemeaux);
    SchemeAlgo algo(scheme);

    auto end = std::chrono::steady_clock::now();
    auto diff = end - start;
    cout << "KeyGen time= " << chrono::duration <double, milli> (diff).count()/1000.0 << " s" << endl;

    LogReg LR(scheme, secretKey, LRparams);
    dMat HEtheta_list;

    ofstream fout;
    fout.open("beta.txt");

    for(int k = 0; k < 1; ++k){

        cout << "-------------------------------------------------------------" << endl;
        cout << k << "th Data Encryption ... " << endl;

        struct rusage usage;

        start= chrono::steady_clock::now();

        Cipher* zTrainCipher = new Cipher[LRparams.dim1];

        LR.EncryptData(zTrainCipher, zTrain[k]);

        end = std::chrono::steady_clock::now();
        diff = end - start;
        cout << "Enc time= "  << chrono::duration <double, milli> (diff).count()/1000.0 << "(s), " ;

        int ret = getrusage(RUSAGE_SELF,&usage);
        cout<< "Mem: " << usage.ru_maxrss/(1024)  << "(MB)" << endl;

        cout << "-------------------------------------------------------------" << endl;
        cout << "HE Logistic Regression ... "  << endl;

        Cipher* thetaCipher= new Cipher[LRparams.dim1];

        start= chrono::steady_clock::now();

        LR.HElogreg(thetaCipher, zTrainCipher, zTrain[k]);

        end = std::chrono::steady_clock::now();
        diff = end - start;
        cout << "Eval time= "  << chrono::duration <double, milli> (diff).count()/1000.0 << "(s), " ;

        ret = getrusage(RUSAGE_SELF,&usage);
        cout<< "Mem: " << usage.ru_maxrss/(1024)  << "(MB)" << endl;

        cout << "-------------------------------------------------------------" << endl;
        cout << "Decryption ... "  << endl;

        dVec HEtheta(LRparams.dim1, 0.0);

        CZZ* dtheta = new CZZ[LRparams.dim1];

        for(int i=0; i< LRparams.dim1; ++i){
            dtheta[i] = (scheme.decrypt(secretKey, thetaCipher[i]))[0];

            conv(HEtheta[i], dtheta[i].r);
            HEtheta[i] = scaledown(HEtheta[i], LRparams.logp);
            cout << "[" << HEtheta[i] << "] " ;
        }
        cout << ": enc " << endl;

        getAUC(HEtheta, zTest[k]);
        HEtheta_list.push_back(HEtheta);

        cout << "-------------------------------------------------------------" << endl;
        cout << "Compare with unenc LR " << endl;
        dVec mtheta(LRparams.dim1, 0.0);

        for(int i= 0; i< LRparams.max_iter; i++){
            LR_poly(mtheta, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta[i] << "] " ;
        cout << ": unenc " << endl;

        getAUC(mtheta, zTest[k]);

        cout << "MSE (HELR/non-HELR): " << getMSE(HEtheta, mtheta) << endl;

        cout << "-------------------------------------------------------------" << endl;
        cout << "Compare with sigmoid LR " << endl;
        dVec mtheta_sig(LRparams.dim1, 0.0);

        for(int i= 0; i< LRparams.max_iter; i++){
            LR_sigmoid(mtheta_sig, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta_sig[i] << "] " ;
        cout << ": unenc " << endl;

        getAUC(mtheta_sig, zTest[k]);

        cout << "MSE (HELR/non-HELR): " << getMSE(HEtheta, mtheta_sig) << endl;

    }

    //! write the beta results in the text file
    fout << "-------------------------------------------------------------" << endl;
    fout << "[" << endl;
    for(int i = 0; i < LRparams.dim1; ++i){
        for(int k = 0; k < 4; ++k){
            fout << HEtheta_list[k][i] << "," ;
        }
        fout << HEtheta_list[4][i] << ";" << endl;
    }
    fout << "];" << endl;
    fout.close();

    delete[] zTest;
    delete[] zTrain;

    return 0;
}


#include <cmath>

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

#include <TList.h>

#include "ProcessMain.hpp"
#include "../export_tree/TreeExportHook.hpp"

using namespace boost::numeric;

template<typename T>
ublas::vector<T>    cross( const ublas::vector<T> &a, const ublas::vector<T> &b )
{
	BOOST_ASSERT(a.size() == 3);
	BOOST_ASSERT(b.size() == 3);

	ublas::vector<T>	result(3);
	result(0) = a(1) * b(2) - a(2) * b(1);
	result(1) = a(2) * b(0) - a(0) * b(2);
	result(2) = a(0) * b(1) - a(1) * b(0);
	return result;
}

enum cham_group_t {prop_2nd, drift_left, drift_right};

double  Psi_L = 1.0447;
double  Psi_R = -1.0209;

template<cham_group_t cham_group>
track3d_t make_track( int event_id, TrackGroup &tg_X, TrackGroup &tg_Y )
{
	static ublas::matrix<double> m1(3, 3), m2(3, 3), A(3, 3);

	m1(0, 0) = 1; m1(1, 0) = 0;  m1(2, 0) = 0;
	m1(0, 1) = 0; m1(1, 1) = 0;  m1(2, 1) = 1;
	m1(0, 2) = 0; m1(1, 2) = -1; m1(2, 2) = 0;

	double Psi;
	if (cham_group == cham_group_t::drift_left)
	{
		Psi = Psi_L;
	}
	else if (cham_group == cham_group_t::drift_right)
	{
		Psi = Psi_R;
	}
	else if (cham_group == cham_group_t::prop_2nd)
	{
		Psi = 0;
	}
	else
	{
		throw;
	}

	m2(0, 0) = cos(Psi); m2(1, 0) = 0; m2(2, 0) = -sin(Psi);
	m2(0, 1) = 0;        m2(1, 1) = 1; m2(2, 1) = 0;
	m2(0, 2) = sin(Psi); m2(1, 2) = 0; m2(2, 2) = cos(Psi);

	A = ublas::prod(m1, m2);

	ublas::vector<double> o(3), a(3), b(3);

	if (cham_group == cham_group_t::drift_left)
	{
		o(0) = 165.2;
		o(1) = 512.4;
		o(2) = 0.3;
	}
	else if (cham_group == cham_group_t::drift_right)
	{
		o(0) = 159.7;
		o(1) = -524.0;
		o(2) = -2.8;
	}
	else if (cham_group == cham_group_t::prop_2nd)
	{
		o(0) = -1486.0;
		o(1) = 0;
		o(2) = 2;
	}
	else
	{
		throw;
	}

	b(0) = 1;
	b(1) = tg_Y.c1[0];
	b(2) = tg_X.c1[0];
	b = ublas::prod(A, b);

	ublas::unit_vector<double> xk(3, 1);
	ublas::unit_vector<double> xl(3, 2);
	a = o + ublas::prod(A, xk) * tg_Y.c0[0] + ublas::prod(A, xl) * tg_X.c0[0];

	return track3d_t({a, b});
}

void	find_intersection_points(
	const track3d_t &t1, const track3d_t &t2,
	intersection_t *i1, intersection_t *i2
	)
{
	auto cr = cross(t1.b, t2.b);
	double cr_norm = norm_2(cr);

	// Following code solves linear system
	// (t1.b, t2.b, cr) * x = t1.a - t2.a
	ublas::matrix<double>	B(3, 3);
	ublas::vector<double>	x;
	ublas::permutation_matrix<>	pm(B.size1());

	for(unsigned int row = 0; row < B.size1(); row++)
	{
		B(row, 0) = t1.b(row);
		B(row, 1) = t2.b(row);
		B(row, 2) = cr(row);
	}
	x = t1.a - t2.a; // put RHS into x

	int	singular = ublas::lu_factorize(B, pm);
	if(singular)
	{
		throw "lu_factorize()==0";
	}

	ublas::lu_substitute(B, pm, x);

	x(2) -= ublas::inner_prod(cr, t1.a - t2.a) / cr_norm;
	ublas::vector<double>	iv1 = t1.a - x(0) * t1.b;
	ublas::vector<double>	iv2 = t2.a + x(1) * t2.b;

	i1->x = iv1(0); i1->y = iv1(1); i1->z = iv1(2);
	i2->x = iv2(0); i2->y = iv2(1); i2->z = iv2(2);
}

double	calc_theta(track3d_t track)
{
	double	cos_theta =
	    ublas::inner_prod(track.b, ublas::unit_vector<double>(3, 0))
	    / norm_2(track.b);
	return acos(cos_theta);
}

double	calc_phi(track3d_t track)
{
	return atan2(
		ublas::inner_prod(track.b, ublas::unit_vector<double>(3, 1)),
		ublas::inner_prod(track.b, ublas::unit_vector<double>(3, 2))
		);
}

double	calc_incident_momentum(double beam_momentum, intersection_t &i1, intersection_t &i2)
{
	const double	TARGET_START = -130; // mm
	double	z = (i1.x + i2.x) / 2 - TARGET_START; // mm
	const double	dE_over_dx = 4; // MeV g^-1 cm^2
	const double	lih2_density = 0.0708; // g cm^-3

	return beam_momentum - dE_over_dx * lih2_density * (z/10);
}

void	TTree_UnfriendAll(TTree *tree)
{
	TList *friends = tree->GetListOfFriends();

	while(friends->GetSize() != 0)
	{
		friends->RemoveLast();
	}
}

TTree*	Process( TTree *events, TTree *cycle_efficiency, Geometry &geom, double central_momentum, intersection_set_t *s )
{
	TTree	*events_new;
	uint32_t	event_cause, timestamp;
	double	theta_l, theta_r, phi_l, phi_r;
	double	beam_momentum, incident_momentum_l, incident_momentum_r;
	TrackGroup	tg_F1X, tg_F1Y, tg_F2X, tg_F2Y, tg_LX, tg_LY, tg_RX, tg_RY;
	const double	F1_length = geom.normal_pos[1][DEV_AXIS_X].back();

	events->GetBranch("event_cause")->SetAddress(&event_cause);
	events->GetBranch("timestamp")->SetAddress(&timestamp);
	events->GetBranch("t1X_track_count")->SetAddress(&tg_F1X.track_count);
	events->GetBranch("t1Y_track_count")->SetAddress(&tg_F1Y.track_count);
	events->GetBranch("t2X_track_count")->SetAddress(&tg_F2X.track_count);
	events->GetBranch("t2Y_track_count")->SetAddress(&tg_F2Y.track_count);
	events->GetBranch("t3X_track_count")->SetAddress(&tg_LX.track_count);
	events->GetBranch("t3Y_track_count")->SetAddress(&tg_LY.track_count);
	events->GetBranch("t4X_track_count")->SetAddress(&tg_RX.track_count);
	events->GetBranch("t4Y_track_count")->SetAddress(&tg_RY.track_count);

	events->SetBranchAddress("t1X_c0", &tg_F1X.c0_ptr);
	events->SetBranchAddress("t1X_c1", &tg_F1X.c1_ptr);
	events->SetBranchAddress("t1X_hits_count", &tg_F1X.hits_count_ptr);
	events->SetBranchAddress("t1Y_c0", &tg_F1Y.c0_ptr);
	events->SetBranchAddress("t1Y_c1", &tg_F1Y.c1_ptr);
	events->SetBranchAddress("t1Y_hits_count", &tg_F1Y.hits_count_ptr);

	events->SetBranchAddress("t2X_c0", &tg_F2X.c0_ptr);
	events->SetBranchAddress("t2X_c1", &tg_F2X.c1_ptr);
	events->SetBranchAddress("t2X_hits_count", &tg_F2X.hits_count_ptr);
	events->SetBranchAddress("t2Y_c0", &tg_F2Y.c0_ptr);
	events->SetBranchAddress("t2Y_c1", &tg_F2Y.c1_ptr);
	events->SetBranchAddress("t2Y_hits_count", &tg_F2Y.hits_count_ptr);

	events->SetBranchAddress("t3X_c0", &tg_LX.c0_ptr);
	events->SetBranchAddress("t3X_c1", &tg_LX.c1_ptr);
	events->SetBranchAddress("t3X_hits_count", &tg_LX.hits_count_ptr);
	events->SetBranchAddress("t3Y_c0", &tg_LY.c0_ptr);
	events->SetBranchAddress("t3Y_c1", &tg_LY.c1_ptr);
	events->SetBranchAddress("t3Y_hits_count", &tg_LY.hits_count_ptr);

	events->SetBranchAddress("t4X_c0", &tg_RX.c0_ptr);
	events->SetBranchAddress("t4X_c1", &tg_RX.c1_ptr);
	events->SetBranchAddress("t4X_hits_count", &tg_RX.hits_count_ptr);
	events->SetBranchAddress("t4Y_c0", &tg_RY.c0_ptr);
	events->SetBranchAddress("t4Y_c1", &tg_RY.c1_ptr);
	events->SetBranchAddress("t4Y_hits_count", &tg_RY.hits_count_ptr);

	// select branches to work with
	events->SetBranchStatus("*", 0);
	events->SetBranchStatus("event_cause", 1);
	events->SetBranchStatus("timestamp", 1);
	events->SetBranchStatus("*_track_count", 1);
	events->SetBranchStatus("*_hits_count", 1);
	events->SetBranchStatus("*_used_chambers_mask", 1);
	events->SetBranchStatus("*_c0", 1);
	events->SetBranchStatus("*_c1", 1);
	// clone tree headers
	// this also copies branches addresses
	events_new = events->CloneTree(0);
	TTree_UnfriendAll(events_new);

	double min_cycle_efficiency;
	cycle_efficiency->SetBranchStatus("min_cycle_efficiency", 1);
	cycle_efficiency->SetBranchAddress("min_cycle_efficiency", &min_cycle_efficiency);
	events_new->Branch("min_cycle_efficiency", &min_cycle_efficiency, "min_cycle_efficiency/D");

	s->br_lr = events_new->Branch("LR", &s->i_lr, "LR_x/D:LR_y/D:LR_z/D");
	s->br_rl = events_new->Branch("RL", &s->i_rl, "RL_x/D:RL_y/D:RL_z/D");
	s->br_f2r = events_new->Branch("F2R", &s->i_f2r, "F2R_x/D:F2R_y/D:F2R_z/D");
	s->br_f2l = events_new->Branch("F2L", &s->i_f2l, "F2L_x/D:F2L_y/D:F2L_z/D");
	s->br_rf2 = events_new->Branch("RF2", &s->i_rf2, "RF2_x/D:RF2_y/D:RF2_z/D");
	s->br_lf2 = events_new->Branch("LF2", &s->i_lf2, "LF2_x/D:LF2_y/D:LF2_z/D");

	events_new->Branch("theta_l", &theta_l, "theta_l/D");
	events_new->Branch("theta_r", &theta_r, "theta_r/D");
	events_new->Branch("phi_l", &phi_l, "phi_l/D");
	events_new->Branch("phi_r", &phi_r, "phi_r/D");
	events_new->Branch("beam_momentum", &beam_momentum, "beam_momentum/D");
	events_new->Branch("incident_momentum_l", &incident_momentum_l, "incident_momentum_l/D");
	events_new->Branch("incident_momentum_r", &incident_momentum_r, "incident_momentum_r/D");

	for(int i = 0; i < events->GetEntries(); i++)
	{
		track3d_t	t_L, t_R, t_F2;
		events->GetEntry(i);
		cycle_efficiency->GetEntry(i);

		bool	left_arm = (tg_LX.track_count == 1) && (tg_LY.track_count == 1);
		bool	right_arm = (tg_RX.track_count == 1) && (tg_RY.track_count == 1);
		bool	incident =
		    (tg_F1X.track_count == 1) && (tg_F1Y.track_count == 1) &&
		    (tg_F2X.track_count == 1) && (tg_F2Y.track_count == 1);

		if (!incident)
		{
			continue;
		}

		t_F2 = make_track<cham_group_t::prop_2nd>(i, tg_F2X, tg_F2Y);

		double	F1_x = tg_F1X.c0[0] + F1_length * tg_F1X.c1[0];
		const double	DISPERSION = (1.0 / 55) * 0.01; // 55 mm/%
		beam_momentum = (1 + F1_x * DISPERSION) * central_momentum;

		if (left_arm)
		{
			t_L = make_track<cham_group_t::drift_left>(i, tg_LX, tg_LY);
			theta_l = calc_theta(t_L);
			phi_l = calc_phi(t_L);
			find_intersection_points(t_F2, t_L, &s->i_f2l, &s->i_lf2);
			incident_momentum_l = calc_incident_momentum(beam_momentum, s->i_f2l, s->i_lf2);
		}
		else
		{
			theta_l = NAN;
			phi_l = NAN;
			s->i_f2l.x = NAN; s->i_f2l.y = NAN; s->i_f2l.z = NAN;
			s->i_lf2.x = NAN; s->i_lf2.y = NAN; s->i_lf2.z = NAN;
			incident_momentum_l = NAN;
		}
		if (right_arm)
		{
			t_R = make_track<cham_group_t::drift_right>(i, tg_RX, tg_RY);
			theta_r = calc_theta(t_R);
			phi_r = calc_phi(t_R);
			find_intersection_points(t_F2, t_R, &s->i_f2r, &s->i_rf2);
			incident_momentum_r = calc_incident_momentum(beam_momentum, s->i_f2r, s->i_rf2);
		}
		else
		{
			theta_r = NAN;
			phi_r = NAN;
			s->i_f2r.x = NAN; s->i_f2r.y = NAN; s->i_f2r.z = NAN;
			s->i_rf2.x = NAN; s->i_rf2.y = NAN; s->i_rf2.z = NAN;
			incident_momentum_r = NAN;
		}

		if (left_arm && right_arm)
		{
			find_intersection_points(t_L, t_R, &s->i_lr, &s->i_rl);
		}
		else
		{
			s->i_lr.x = NAN; s->i_lr.y = NAN; s->i_lr.z = NAN;
			s->i_rl.x = NAN; s->i_rl.y = NAN; s->i_rl.z = NAN;
		}

		events_new->Fill();
	}

	return events_new;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#include <cmath>

#include <Eigen/Core>

#include "tudat/math/basic/sphericalHarmonics.h"
#include "tudat/math/basic/basicMathematicsFunctions.h"

namespace tudat
{
namespace basic_mathematics
{

//! Update maximum degree and order of cache
void SphericalHarmonicsCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder )
{
    maximumDegree_ = maximumDegree;
    maximumOrder_ = maximumOrder;

    if( maximumOrder_ > maximumDegree_ )
    {
        maximumOrder_ = maximumDegree_;
    }
    legendreCache_->resetMaximumDegreeAndOrder( maximumDegree_, maximumOrder_ );

    sinesOfLongitude_.resize( maximumOrder_ + 1 );
    cosinesOfLongitude_.resize( maximumOrder_ + 1 );
    referenceRadiusRatioPowers_.resize( maximumDegree_ + 2 );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient(
        const double distance,
        const double radiusPowerTerm,
        const double cosineOfOrderLongitude,
        const double sineOfOrderLongitude,
        const double cosineOfLatitude,
        const double preMultiplier,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        const double legendrePolynomial,
        const double legendrePolynomialDerivative )
{
    // Return result.
    return ( Eigen::Vector3d( ) <<
             - preMultiplier / distance
             * radiusPowerTerm
             * ( static_cast< double >( degree ) + 1.0 ) * legendrePolynomial
             * ( cosineHarmonicCoefficient * cosineOfOrderLongitude
                 + sineHarmonicCoefficient * sineOfOrderLongitude ),
             preMultiplier * radiusPowerTerm
             * legendrePolynomialDerivative * cosineOfLatitude * (
                 cosineHarmonicCoefficient * cosineOfOrderLongitude
                 + sineHarmonicCoefficient * sineOfOrderLongitude ),
             preMultiplier * radiusPowerTerm
             * static_cast< double >( order ) * legendrePolynomial
             * ( sineHarmonicCoefficient * cosineOfOrderLongitude
                 - cosineHarmonicCoefficient * sineOfOrderLongitude ) ).finished( );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient(
        const Eigen::Vector3d& sphericalPosition,
        const double referenceRadius,
        const double preMultiplier,
        const int degree,
        const int order,
        const double cosineHarmonicCoefficient,
        const double sineHarmonicCoefficient,
        const double legendrePolynomial,
        const double legendrePolynomialDerivative )
{
    return computePotentialGradient(
                sphericalPosition( radiusIndex ),
                basic_mathematics::raiseToIntegerPower
                ( referenceRadius / sphericalPosition( radiusIndex ), static_cast< double >( degree ) + 1.0 ),
                std::cos( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ),
                std::sin( static_cast< double >( order ) * sphericalPosition( longitudeIndex ) ),
                std::cos( sphericalPosition( latitudeIndex ) ), preMultiplier, degree, order,
                cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative );
}

//! Compute the gradient of a single term of a spherical harmonics potential field.
Eigen::Vector3d computePotentialGradient( const Eigen::Vector3d& sphericalPosition,
                                          const double preMultiplier,
                                          const int degree,
                                          const int order,
                                          const double cosineHarmonicCoefficient,
                                          const double sineHarmonicCoefficient,
                                          const double legendrePolynomial,
                                          const double legendrePolynomialDerivative,
                                          const std::shared_ptr< SphericalHarmonicsCache > sphericalHarmonicsCache )
{
    return computePotentialGradient(
                sphericalPosition( radiusIndex ),
                sphericalHarmonicsCache->getReferenceRadiusRatioPowers( degree + 1 ),
                sphericalHarmonicsCache->getCosineOfMultipleLongitude( order ),
                sphericalHarmonicsCache->getSineOfMultipleLongitude( order ),
                sphericalHarmonicsCache->getLegendreCache( )->getCurrentPolynomialParameterComplement( ),
                preMultiplier, degree, order,
                cosineHarmonicCoefficient, sineHarmonicCoefficient, legendrePolynomial,legendrePolynomialDerivative );
}

} // namespace basic_mathematics
} // namespace tudat


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_CGS_INCLUDE
#define ITL_CGS_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/operation/dot.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Conjugate Gradient Squared
template < typename LinearOperator, typename Vector,
	   typename Preconditioner, typename Iteration >
int cgs(const LinearOperator &A, Vector &x, const Vector &b,
	const Preconditioner &M, Iteration& iter)
{
    mtl::vampir_trace<7007> tracer;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    Scalar     rho_1(0), rho_2(0), alpha(0), beta(0);
    Vector     p(resource(x)), phat(resource(x)), q(resource(x)), qhat(resource(x)), vhat(resource(x)),
	       u(resource(x)), uhat(resource(x)), r(b - A * x), rtilde= r;

    while (! iter.finished(r)) {
	++iter;
	rho_1= dot(rtilde, r);

	if (rho_1 == 0.) iter.fail(2, "cgs breakdown");

	if (iter.first())
	    p= u= r;
	else {
	    beta = rho_1 / rho_2;
	    u= r + beta * q;
	    p= u + beta * (q + beta * p);
	}

        vhat= A * Vector(solve(M, p));
	alpha = rho_1 / dot(rtilde, vhat);
	q= u - alpha * vhat;

	u+= q;
	uhat= solve(M, u);

	x+= alpha * uhat;
	qhat= A * uhat;
	r-= alpha * qhat;

	rho_2= rho_1;
    }
    return iter;
}

/// Solver class for CGS method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class cgs_solver
  : public base_solver< cgs_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< cgs_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit cgs_solver(const LinearOperator& A) : base(A), L(A)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Construct solver from a linear operator and (left) preconditioner
    cgs_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return cgs(this->A, x, b, L, iter);
    }

  private:
    Preconditioner        L;
};

} // namespace itl

#endif // ITL_CGS_INCLUDE


#pragma once

#include "../geometry/LineSegment/LineSegment2/linesegment2.hh"
#include <Eigen/Core>
#include <Eigen/LU>

namespace bold
{
  class IncrementalRegression
  {
  public:
    IncrementalRegression()
      : d_xxSum{Eigen::Matrix2f::Zero()},
      d_xySum{Eigen::Vector2f::Zero()},
      d_nPoints{0},
      d_sqErrorSum{0.0f},
      d_dirty{false}
    {}

    void setSqError(float sqError)
    {
      d_sqErrorSum = sqError;
    }

    Eigen::Vector2f head() const
    {
      return d_head;
    }

    float getY(float x) const
    {
      return d_beta.dot(Eigen::Vector2f{x, 1});
    }

    float fit(Eigen::Vector2f const& point)
    {
      if (d_nPoints == 1)
        return (point - head()).norm();
      else if (d_xStart == d_xEnd)
      {
        return std::abs(point.x() - d_xStart);
      }
      else
      {
        solve();
        auto sigma = sigmaAt(point.x());
        auto yPred = getY(point.x());
        auto error = std::abs(yPred - point.y());
        return sigma == 0 ? error : error / sigma;
      }
    }

    float sigmaAt(float x)
    {
      auto _x = Eigen::Vector2f{x, 1};
      return sqrt(_x.transpose() * (1.0 / d_sqErrorSum * d_xxSum).inverse() * _x);
    }

    bool isVertical() const
    {
      return d_xStart == d_xEnd;
    }

    void addPoint(Eigen::Vector2f const& point)
    {
      d_points.push_back(point);

      auto x = Eigen::Vector2f{point.x(), 1};
      d_xxSum += x * x.transpose();
      d_xySum += x * point.y();
      d_xEnd = point.x();
      ++d_nPoints;

      if (d_nPoints == 1)
      {
        d_xStart = point.x();
        d_head = point;
      }
      else if (d_nPoints == 2)
        d_head = point;
      else
      {
        d_head = Eigen::Vector2f{point.x(), getY(point.x())};
      }
      d_dirty = true;
    }

    void solve()
    {
      if (d_dirty && d_nPoints > 1)
      {
        d_beta = (d_xxSum / d_nPoints).inverse() * d_xySum / d_nPoints;
        d_dirty = false;
      }
    }

    Eigen::Vector2f getBeta() const
    {
      return d_beta;
    }

    LineSegment2f getLineSegment()
    {
      solve();
      Eigen::Vector2f p1{d_xStart, getY(d_xStart)};
      Eigen::Vector2f p2{d_xEnd, getY(d_xEnd)};
      return LineSegment2f{p1, p2};
    }

    unsigned getNPoints() const
    {
      return d_nPoints;
    }

    float determineStandardError() const
    {
      auto X = Eigen::MatrixXf(2, d_points.size());
      auto y = Eigen::VectorXf(d_points.size());
      unsigned idx = 0;
      for (auto const& point : d_points)
      {
        X.col(idx) = Eigen::Vector2f{point.x(), 1};
        y(idx) = point.y();
        ++idx;
      }
      auto res = y - X.transpose() * d_beta;
      return sqrt(res.dot(res) / (d_points.size() - 2));
    }

    void merge(IncrementalRegression const& other)
    {
      d_xxSum += other.d_xxSum;
      d_xySum += other.d_xySum;
      d_nPoints += other.d_nPoints;
      d_xStart = std::min(d_xStart, other.d_xStart);
      d_xEnd = std::max(d_xEnd, other.d_xEnd);
      solve();
    }

private:
    std::vector<Eigen::Vector2f> d_points;

    Eigen::Matrix2f d_xxSum;
    Eigen::Vector2f d_xySum;
    unsigned d_nPoints;
    Eigen::Vector2f d_beta;
    unsigned d_xStart;
    unsigned d_xEnd;
    Eigen::Vector2f d_head;
    float d_sqErrorSum;

    bool d_dirty;
  };
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "harmonic.h"
#include "adjacency_matrix.h"
#include "cotmatrix.h"
#include "diag.h"
#include "invert_diag.h"
#include "isdiag.h"
#include "massmatrix.h"
#include "min_quad_with_fixed.h"
#include "speye.h"
#include "sum.h"
#include <Eigen/Sparse>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::harmonic(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  using namespace Eigen;
  typedef typename DerivedV::Scalar Scalar;
  SparseMatrix<Scalar> L,M;
  cotmatrix(V,F,L);
  massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,M);
  return harmonic(L,M,b,bc,k,W);
}

template <
  typename DerivedF,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::harmonic(
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  using namespace Eigen;
  typedef typename Derivedbc::Scalar Scalar;
  SparseMatrix<Scalar> A;
  adjacency_matrix(F,A);
  // sum each row
  SparseVector<Scalar> Asum;
  sum(A,1,Asum);
  // Convert row sums into diagonal of sparse matrix
  SparseMatrix<Scalar> Adiag;
  diag(Asum,Adiag);
  SparseMatrix<Scalar> L = A-Adiag;
  SparseMatrix<Scalar> M;
  speye(L.rows(),M);
  return harmonic(L,M,b,bc,k,W);
}

template <
  typename DerivedL,
  typename DerivedM,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::harmonic(
  const Eigen::SparseMatrix<DerivedL> & L,
  const Eigen::SparseMatrix<DerivedM> & M,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  const int n = L.rows();
  assert(n == L.cols() && "L must be square");
  assert(n == M.cols() && "M must be same size as L");
  assert(n == M.rows() && "M must be square");
  assert(igl::isdiag(M) && "Mass matrix should be diagonal");

  Eigen::SparseMatrix<DerivedL> Q;
  igl::harmonic(L,M,k,Q);

  typedef DerivedL Scalar;
  min_quad_with_fixed_data<Scalar> data;
  min_quad_with_fixed_precompute(Q,b,Eigen::SparseMatrix<Scalar>(),true,data);
  W.resize(n,bc.cols());
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
  const VectorXS B = VectorXS::Zero(n,1);
  for(int w = 0;w<bc.cols();w++)
  {
    const VectorXS bcw = bc.col(w);
    VectorXS Ww;
    if(!min_quad_with_fixed_solve(data,B,bcw,VectorXS(),Ww))
    {
      return false;
    }
    W.col(w) = Ww;
  }
  return true;
}

template <
  typename DerivedL,
  typename DerivedM,
  typename DerivedQ>
IGL_INLINE void igl::harmonic(
  const Eigen::SparseMatrix<DerivedL> & L,
  const Eigen::SparseMatrix<DerivedM> & M,
  const int k,
  Eigen::SparseMatrix<DerivedQ> & Q)
{
  assert(L.rows() == L.cols()&&"L should be square");
  assert(M.rows() == M.cols()&&"M should be square");
  assert(L.rows() == M.rows()&&"L should match M's dimensions");
  Eigen::SparseMatrix<DerivedM> Mi;
  invert_diag(M,Mi);
  Q = -L;
  for(int p = 1;p<k;p++)
  {
    Q = (Q*Mi*-L).eval();
  }
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedQ>
IGL_INLINE void igl::harmonic(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const int k,
  Eigen::SparseMatrix<DerivedQ> & Q)
{
  Eigen::SparseMatrix<DerivedQ> L,M;
  cotmatrix(V,F,L);
  massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,M);
  return harmonic(L,M,k,Q);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);

template bool igl::harmonic<Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::harmonic<Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template bool igl::harmonic<Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, int, Eigen::SparseMatrix<double, 0, int>&);
#endif