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

  @copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotomnmbo_3.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#endif
#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/negate.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <tuple>

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

  BOOST_DISPATCH_OVERLOAD ( cbrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::double_<A0> >
                          )
  {

    inline A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      A0 z =  bs::abs(a0);
    #ifndef BOOST_SIMD_NO_INFINITIES
      if (z == bs::Inf<A0>() || (z == 0)) return a0;
    #else
      if (z == 0) return a0;
    #endif
    #ifndef BOOST_SIMD_NO_DENORMALS
      A0 f = One<A0>();
      if (z < Smallestposval<A0>())
      {
        z *= Twotonmb<A0>();
        f  = Twotomnmbo_3<A0>();
      }
    #endif
      const A0 CBRT2  = Constant< A0, 0x3ff428a2f98d728bll> ();
      const A0 CBRT4  = Constant< A0, 0x3ff965fea53d6e3dll> ();
      const A0 CBRT2I = Constant< A0, 0x3fe965fea53d6e3dll> ();
      const A0 CBRT4I = Constant< A0, 0x3fe428a2f98d728bll> ();
      using i_t = bd::as_integer_t<A0, signed>;
      i_t e;
      A0 x;
      std::tie(x, e) = fast_(frexp)(z);
      x = horn<A0,
               0x3fd9c0c12122a4fell,
               0x3ff23d6ee505873all,
               0xbfee8a4ca3ba37b8ll,
               0x3fe17e1fc7e59d58ll,
               0xbfc13c93386fdff6ll
               > (x);
      const auto flag = is_gez(e);
      i_t e1 =  bs::abs(e);
      i_t rem = e1;
      e1 /= Three<i_t>();
      rem -= e1*Three<i_t>();
      e =  negate(e1, e);
      const A0 cbrt2 = flag ? CBRT2 : CBRT2I;
      const A0 cbrt4 = flag ? CBRT4 : CBRT4I;
      A0 fact = (rem == One<i_t>()) ? cbrt2: One<A0>();
      fact = (rem == Two<i_t>() ? cbrt4 : fact);
      x = fast_(ldexp)(x*fact, e);
      x -= (x-z/sqr(x))*Third<A0>();
      x -= (x-z/sqr(x))*Third<A0>(); //two newton passes
    #ifndef BOOST_SIMD_NO_DENORMALS
      return bitwise_or(x, bitofsign(a0))*f;
    #else
      return bitwise_or(x, bitofsign(a0));
    #endif
    }
  };
  BOOST_DISPATCH_OVERLOAD ( cbrt_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    inline A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      A0 z =  bs::abs(a0);
    #ifndef BOOST_SIMD_NO_INFINITIES
      if (z == bs::Inf<A0>() || (z == 0)) return a0;
    #else
      if  (z == 0) return a0;
    #endif
    #ifndef BOOST_SIMD_NO_DENORMALS
      A0 f = One<A0>();
      if (z < Smallestposval<A0>())
      {
        z *= Twotonmb<A0>();
        f = Twotomnmbo_3<A0>();
      }
    #endif
      const A0 CBRT2  = Constant< A0, 0x3fa14518> ();
      const A0 CBRT4  = Constant< A0, 0x3fcb2ff5> ();
      const A0 CBRT2I = Constant< A0, 0x3f4b2ff5> ();
      const A0 CBRT4I = Constant< A0, 0x3f214518> ();
      using i_t = bd::as_integer_t<A0, signed>;
      i_t e;
      A0 x;
      std::tie(x, e)= fast_(frexp)(z);
      x = horn<A0,
               0x3ece0609,
               0x3f91eb77,
               0xbf745265,
               0x3f0bf0fe,
               0xbe09e49a
               > (x);
      const auto flag = is_gez(e);
      i_t e1 =  bs::abs(e);
      i_t rem = e1;
      e1 /= Three<i_t>();
      rem -= e1*Three<i_t>();
      e =  negate(e1, e);

      const A0 cbrt2 = flag ? CBRT2 : CBRT2I;
      const A0 cbrt4 = flag ? CBRT4 : CBRT4I;
      A0 fact = (rem ==  One<i_t>()) ? cbrt2 : One<A0>();
      fact = (rem == Two<i_t>()) ? cbrt4 : fact;
      x = fast_(ldexp)(x*fact, e);
      x -= (x-z/sqr(x))*Third<A0>();
    #ifndef BOOST_SIMD_NO_DENORMALS
      return bitwise_or(x, bitofsign(a0))*f;
    #else
      return bitwise_or(x, bitofsign(a0));
    #endif
    }
  };
  BOOST_DISPATCH_OVERLOAD ( cbrt_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &,  A0  a0) const BOOST_NOEXCEPT
    {
      return std::cbrt(a0);
    }
  };
} } }

#endif


#include "nearest_neighbour_search.hpp"

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/property_map.h>
#include <boost/iterator/zip_iterator.hpp>
#include <utility>

namespace kinect {

NearestNeighbourSearch::NearestNeighbourSearch(const std::vector<sample_t>& samples)
 :m_samples(samples)
 ,m_tree()
{
  std::vector<Point_3> points;
  std::vector<size_t> indices;

  for(std::size_t i = 0; i < samples.size(); ++i){
    points.emplace_back(samples[i]);
    indices.emplace_back(i);
  }

  m_tree = std::unique_ptr<Tree>{new Tree(
    boost::make_zip_iterator(boost::make_tuple(points.begin(), indices.begin())),
    boost::make_zip_iterator(boost::make_tuple(points.end(), indices.end()))
  )};
}

std::vector<sample_t>
NearestNeighbourSearch::search(glm::fvec3 const& curr_point, unsigned num_neighbours) const {

  K_neighbor_search search(*(m_tree.get()), Point_3{curr_point.x, curr_point.y, curr_point.z}, num_neighbours);

  std::vector<sample_t> result;
  for(K_neighbor_search::iterator it = search.begin(); it != search.end(); it++) {
    result.emplace_back(m_samples[boost::get<1>(it->first)]);
  }

  return result;
}

}


#include <armadillo>
#include <json.hpp>
#include <iostream>
#include <ForwardBackward.hpp>
#include <HSMM.hpp>
#include <memory>

using namespace arma;
using namespace hsmm;
using namespace std;
using json = nlohmann::json;

int main() {
    int ndurations = 4;
    int min_duration = 4;
    mat transition = {{0.0, 0.1, 0.4, 0.5},
                      {0.3, 0.0, 0.6, 0.1},
                      {0.2, 0.2, 0.0, 0.6},
                      {0.4, 0.4, 0.2, 0.0}};
    int nstates = transition.n_rows;
    vec pi(nstates, fill::eye);
    pi.fill(1.0/nstates);
    // mat durations(nstates, ndurations, fill::eye);
    mat durations =  {{0.0, 0.1, 0.4, 0.5},
                      {0.3, 0.0, 0.6, 0.1},
                      {0.2, 0.2, 0.0, 0.6},
                      {0.4, 0.4, 0.2, 0.0}};

    // Instantiating the emission process.
    vec means = {0, 5, 10, 15};
    vec std_devs =  {0.5, 1.0, 0.1, 2.0};
    shared_ptr<AbstractEmission> ptr_emission(new DummyGaussianEmission(
            means, std_devs));

    // Multivariate emission.
    mat mult_means(nstates, 2, fill::zeros);
    for(int i = 0; i < nstates; i++)
        mult_means.row(i).fill(i);
    shared_ptr<AbstractEmission> ptr_mult_emission(
            new DummyMultivariateGaussianEmission(mult_means, 0.1));

    // Instantiating the HSMM.
    HSMM dhsmm(ptr_emission, transition, pi, durations, min_duration);

    ivec hiddenStates, hiddenDurations;
    int nSampledSegments = 50;
    field<mat> samples = dhsmm.sampleSegments(nSampledSegments, hiddenStates,
            hiddenDurations);
    int nobs = samples.n_elem;

    cout << "Generated samples" << endl;
    // cout << samples << endl;
    cout << "Generated states and durations" << endl;
    cout << join_horiz(hiddenStates, hiddenDurations) << endl;

    mat alpha(nstates, nobs, fill::zeros);
    mat beta(nstates, nobs, fill::zeros);
    mat alpha_s(nstates, nobs, fill::zeros);
    mat beta_s(nstates, nobs, fill::zeros);
    vec beta_s_0(nstates, fill::zeros);
    cube eta(nstates, ndurations, nobs, fill::zeros);
    cube zeta(nstates, nstates, nobs - 1, fill::zeros);
    cube logpdf = dhsmm.computeEmissionsLogLikelihood(samples);
    Labels obs_segments;
    logsFB(log(transition), log(pi), log(durations), logpdf, obs_segments,
            alpha, beta, alpha_s, beta_s, beta_s_0, eta, zeta, min_duration,
            nobs);
    mat compare_alpha = exp(alpha);
    mat compare_alpha_s = exp(alpha_s);
    mat compare_beta = exp(beta);
    mat compare_beta_s = exp(beta_s);
    cube compare_eta = exp(eta);
    mat compare_beta_s_0 = exp(beta_s_0);
    cube pdf = dhsmm.computeEmissionsLikelihood(samples);
    FB(transition, pi, durations, pdf, obs_segments, alpha, beta, alpha_s,
            beta_s, beta_s_0, eta, min_duration, nobs);

    cout << "TEST" << endl;
    mat a = compare_beta - beta;
    mat b = compare_beta_s - beta_s;
    cube c = compare_eta - eta;
    mat d = compare_beta_s_0 - beta_s_0;
    mat e = compare_alpha - alpha;
    mat f = compare_alpha_s - alpha_s;
    cout << a.min() << " " << a.max() << endl;
    cout << b.min() << " " << b.max() << endl;
    cout << c.min() << " " << c.max() << endl;
    cout << d.min() << " " << d.max() << endl;
    cout << e.min() << " " << e.max() << endl;
    cout << f.min() << " " << f.max() << endl;

    cout << "Sums rows" << endl;
    cout << sum(alpha, 1) << endl;
    cout << sum(alpha_s, 1) << endl;
    cout << sum(beta, 1) << endl;
    cout << sum(beta_s, 1) << endl;

    imat psi_duration(nstates, nobs, fill::zeros);
    imat psi_state(nstates, nobs, fill::zeros);
    mat delta(nstates, nobs, fill::zeros);
    Viterbi(transition, pi, durations, logpdf, delta, psi_duration, psi_state,
            min_duration, nobs);
    cout << "Delta last column" << endl;
    cout << delta.col(nobs - 1) << endl;
    ivec viterbiStates, viterbiDurations;
    viterbiPath(psi_duration, psi_state, delta, viterbiStates,
            viterbiDurations);

    cout << "Viterbi states and durations" << endl;
    cout << join_horiz(viterbiStates, viterbiDurations) << endl;

    // Debug
    int differences = 0;
    if (viterbiStates.n_elem == hiddenStates.n_elem) {
        for(int t = 0; t < viterbiStates.n_elem; t++)
            if (hiddenStates(t) != viterbiStates(t))
                differences++;
        cout << " Differences: " << differences << endl;
    }
    else
        cout << "The dimensions don't match." << endl;

    // Generating multiple synthetic sequences.
    int nseq = 10;
    int nsegments = 100;
    field<field<mat>> mobs;
    field<ivec> mhs, mdur;
    mobs = dhsmm.sampleMultipleSequences(nseq, nsegments, mhs, mdur);

    // Initializing uniformly the transitions, initial state pmf and durations.
    transition.fill(1.0/(nstates-1));
    transition.diag().zeros();  // No self-loops.
    dhsmm.setTransition(transition);
    pi.fill(1.0/nstates);
    dhsmm.setPi(pi);
    durations.fill(1.0/ndurations);
    dhsmm.setDuration(durations);

    // Resetting emission parameters.
    vec new_means = {0.1, 0.2, 0.3, 30};
    vec new_std_devs = ones<vec>(nstates) * 10;
    shared_ptr<AbstractEmission> init_emission(new DummyGaussianEmission(
            new_means, new_std_devs));
    dhsmm.setEmission(init_emission);

    // Testing the learning algorithm.
    dhsmm.fit(mobs, 100, 1e-10);

    vec best_pi(nstates, fill::zeros);
    for(int s = 0; s < nseq; s++)
        best_pi(mhs(s)(0))++;
    best_pi /= nseq;
    cout << "Best initial state pmf we can aim at:" << endl << best_pi << endl;

    cout << "Learnt pi:" << endl;
    cout << dhsmm.pi_ << endl;

    cout << "Best transition matrix we can aim at:" << endl;
    mat prueba(nstates, nstates, fill::zeros);
    for(int s = 0; s < nseq; s++) {
        ivec& hiddenStates = mhs(s);
        for(int i = 0; i < hiddenStates.n_elem - 1; i++)
            prueba(hiddenStates(i), hiddenStates(i + 1))++;
    }
    mat pruebasum = sum(prueba, 1);
    for(int i = 0; i < nstates; i++)
        prueba.row(i) /= pruebasum(i);
    cout << prueba << endl;

    cout << "Learnt matrix:" << endl;
    cout << dhsmm.transition_ << endl;

    cout << "Best duration matrix we can aim at:" << endl;
    mat emp_durations(nstates, ndurations, fill::zeros);
    for(int s = 0; s < nseq; s++) {
        ivec& hiddenStates = mhs(s);
        ivec& hiddenDurations = mdur(s);
        for(int i = 0; i < hiddenStates.n_elem; i++)
            emp_durations(hiddenStates(i), hiddenDurations(i) - min_duration)++;
    }
    mat emp_durations_sum = sum(emp_durations, 1);
    for(int i = 0; i < nstates; i++)
        emp_durations.row(i) /= emp_durations_sum(i);
    cout << emp_durations << endl;
    cout << "Learnt durations:" << endl;
    cout << dhsmm.duration_ << endl;

    cout << "Learnt parameters (json)" << endl;
    json params = dhsmm.to_stream();
    cout << params.dump(4) << endl;
    return 0;
}


/*
 * Copyright (c) 2011, Laurent Kneip, ETH Zurich
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL ETH ZURICH 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.
 */

/*
 * P3P.cpp
 *
 *  Created on: Nov 2, 2010
 *      Author: Laurent Kneip
 * Description: Compute the absolute pose of a camera using three 3D-to-2D correspondences
 *   Reference: A Novel Parametrization of the P3P-Problem for a Direct Computation of
 *              Absolute Camera Position and Orientation
 *
 *       Input: feature_vectors: 3x3 matrix with UNITARY feature vectors (each column is a vector)
 *              world_points: 3x3 matrix with corresponding 3D world points (each column is a point)
 *              solutions: 3x16 matrix that will contain the solutions
 *                         form: [ 3x1 position(solution1) 3x3 orientation(solution1) 3x1 position(solution2) 3x3 orientation(solution2) ... ]
 *                         the obtained orientation matrices are defined as transforming points from the cam to the world frame
 *      Output: int: 0 if correct execution
 *                  -1 if world points aligned
 */

#include "p3p/P3p.h"

#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <complex>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace p3p_kneip {

int P3PComputePoses(const std::vector<Eigen::Vector3d>& feature_vectors,
                    const std::vector<Eigen::Vector3d>& world_points,
                    std::vector<Eigen::Matrix3d>* rotations,
                    std::vector<Eigen::Vector3d>* camera_centers) {
  // Extraction of world points
  Eigen::Vector3d P1 = world_points[0];
  Eigen::Vector3d P2 = world_points[1];
  Eigen::Vector3d P3 = world_points[2];

  // Verification that world points are not colinear

  Eigen::Vector3d temp1 = P2 - P1;
  Eigen::Vector3d temp2 = P3 - P1;

  if (temp1.cross(temp2).squaredNorm() == 0.0)
    return -1;

  // Extraction of feature vectors
  Eigen::Vector3d f1 = feature_vectors[0].normalized();
  Eigen::Vector3d f2 = feature_vectors[1].normalized();
  Eigen::Vector3d f3 = feature_vectors[2].normalized();

  // Creation of intermediate camera frame

  Eigen::Vector3d e1 = f1;
  Eigen::Vector3d e3 = f1.cross(f2);
  e3.normalize();
  Eigen::Vector3d e2 = e3.cross(e1);

  Eigen::Matrix3d T;
  T << e1.transpose(), e2.transpose(), e3.transpose();

  f3 = T * f3;

  // Reinforce that f3[2] > 0 for having theta in [0;pi]

  if (f3(2) > 0.0) {
    f1 = feature_vectors[1];
    f2 = feature_vectors[0];
    f3 = feature_vectors[2];

    e1 = f1;
    e3 = f1.cross(f2);
    e3.normalize();
    e2 = e3.cross(e1);

    T << e1.transpose(), e2.transpose(), e3.transpose();

    f3 = T * f3;

    P1 = world_points[1];
    P2 = world_points[0];
    P3 = world_points[2];
  }

  // Creation of intermediate world frame

  Eigen::Vector3d n1 = P2 - P1;
  n1.normalize();
  Eigen::Vector3d n3 = n1.cross(P3 - P1);
  n3.normalize();
  Eigen::Vector3d n2 = n3.cross(n1);

  Eigen::Matrix3d N;
  N << n1.transpose(), n2.transpose(), n3.transpose();

  // Extraction of known parameters

  P3 = N * (P3 - P1);

  double d_12 = (P2 - P1).norm();
  double f_1 = f3(0) / f3(2);
  double f_2 = f3(1) / f3(2);
  double p_1 = P3(0);
  double p_2 = P3(1);

  double cos_beta = f1.dot(f2);
  double b = 1.0 / (1.0 - (cos_beta * cos_beta)) - 1.0;

  if (cos_beta < 0.0)
    b = -sqrt(b);
  else
    b = sqrt(b);

  // Definition of temporary variables for avoiding multiple computation

  double f_1_pw2 = f_1 * f_1;
  double f_2_pw2 = f_2 * f_2;
  double p_1_pw2 = p_1 * p_1;
  double p_1_pw3 = p_1_pw2 * p_1;
  double p_1_pw4 = p_1_pw3 * p_1;
  double p_2_pw2 = p_2 * p_2;
  double p_2_pw3 = p_2_pw2 * p_2;
  double p_2_pw4 = p_2_pw3 * p_2;
  double d_12_pw2 = d_12 * d_12;
  double b_pw2 = b * b;

  // Computation of factors of 4th degree polynomial

  Eigen::Matrix<double, 5, 1> factors;

  factors(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;

  factors(1) = 2.0 * p_2_pw3 * d_12 * b + 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b
      - 2.0 * f_2 * p_2_pw3 * f_1 * d_12;

  factors(2) = -f_2_pw2 * p_2_pw2 * p_1_pw2
      - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2
      + f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 + 2.0 * p_1 * p_2_pw2 * d_12
      + 2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b - p_2_pw2 * p_1_pw2 * f_1_pw2
      + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12 - p_2_pw2 * d_12_pw2 * b_pw2
      - 2.0 * p_1_pw2 * p_2_pw2;

  factors(3) = 2.0 * p_1_pw2 * p_2 * d_12 * b + 2.0 * f_2 * p_2_pw3 * f_1 * d_12
      - 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b - 2.0 * p_1 * p_2 * d_12_pw2 * b;

  factors(4) = -2.0 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b
      + f_2_pw2 * p_2_pw2 * d_12_pw2 + 2.0 * p_1_pw3 * d_12 - p_1_pw2 * d_12_pw2
      + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4
      - 2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12 + p_2_pw2 * f_1_pw2 * p_1_pw2
      + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;

  // Computation of roots

  Eigen::Vector4d real_roots;

  SolveQuartic(factors, &real_roots);

  // Backsubstitution of each solution
  rotations->clear();
  camera_centers->clear();
  for (int i = 0; i < 4; ++i) {
    // TORSTEN: Checks if this solution has already been used.
    bool used = false;
    for (int j = i - 1; j >= 0 && !used; --j) {
      used = (real_roots(i) == real_roots(j));
    }
    if (used)
      continue;

    double cot_alpha = (-f_1 * p_1 / f_2 - real_roots(i) * p_2 + d_12 * b)
        / (-f_1 * real_roots(i) * p_2 / f_2 + p_1 - d_12);

    double cos_theta = real_roots(i);
    double sin_theta = sqrt(1.0 - (cos_theta * cos_theta));
    double sin_alpha = sqrt(1.0 / ((cot_alpha * cot_alpha) + 1.0));
    double cos_alpha = sqrt(1.0 - (sin_alpha * sin_alpha));

    if (cot_alpha < 0.0)
      cos_alpha = -cos_alpha;

    Eigen::Vector3d C;
    C << d_12 * cos_alpha * (sin_alpha * b + cos_alpha), cos_theta * d_12
        * sin_alpha * (sin_alpha * b + cos_alpha), sin_theta * d_12 * sin_alpha
        * (sin_alpha * b + cos_alpha);

    C = P1 + N.transpose() * C;

    Eigen::Matrix3d R;
    R << -cos_alpha, -sin_alpha * cos_theta, -sin_alpha * sin_theta,
         sin_alpha, -cos_alpha  * cos_theta, -cos_alpha * sin_theta,
         0, -sin_theta, cos_theta;

    R = N.transpose() * R.transpose() * T;

    camera_centers->push_back(C);
    rotations->push_back(R.transpose());
  }

  return 0;
}

int SolveQuartic(const Eigen::Matrix<double, 5, 1>& factors,
                        Eigen::Vector4d* real_roots) {
  double A = factors[0];
  double B = factors[1];
  double C = factors[2];
  double D = factors[3];
  double E = factors[4];

  double A_pw2 = A * A;
  double B_pw2 = B * B;
  double A_pw3 = A_pw2 * A;
  double B_pw3 = B_pw2 * B;
  double A_pw4 = A_pw3 * A;
  double B_pw4 = B_pw3 * B;

  double alpha = -3.0 * B_pw2 / (8.0 * A_pw2) + C / A;
  double beta = B_pw3 / (8.0 * A_pw3) - B * C / (2.0 * A_pw2) + D / A;
  double gamma = -3.0 * B_pw4 / (256.0 * A_pw4) + B_pw2 * C / (16.0 * A_pw3)
      - B * D / (4.0 * A_pw2) + E / A;

  double alpha_pw2 = alpha * alpha;
  double alpha_pw3 = alpha_pw2 * alpha;

  std::complex<double> P(-alpha_pw2 / 12.0 - gamma, 0.0);
  std::complex<double> Q(
      -alpha_pw3 / 108.0 + alpha * gamma / 3.0 - beta * beta / 8.0, 0.0);
  std::complex<double> R = -Q / 2.0
      + sqrt(pow(Q, 2.0) / 4.0 + P * P * P / 27.0);

  std::complex<double> U = pow(R, (1.0 / 3.0));
  std::complex<double> y;

  if (U.real() == 0.0)
    y = -5.0 * alpha / 6.0 - pow(Q, (1.0 / 3.0));
  else
    y = -5.0 * alpha / 6.0 - P / (3.0 * U) + U;

  std::complex<double> w = sqrt(alpha + 2.0 * y);

  std::complex<double> temp;

  temp = -B / (4.0 * A)
      + 0.5 * (w + sqrt(-(3.0 * alpha + 2.0 * y + 2.0 * beta / w)));
  (*real_roots)[0] = temp.real();
  temp = -B / (4.0 * A)
      + 0.5 * (w - sqrt(-(3.0 * alpha + 2.0 * y + 2.0 * beta / w)));
  (*real_roots)[1] = temp.real();
  temp = -B / (4.0 * A)
      + 0.5 * (-w + sqrt(-(3.0 * alpha + 2.0 * y - 2.0 * beta / w)));
  (*real_roots)[2] = temp.real();
  temp = -B / (4.0 * A)
      + 0.5 * (-w - sqrt(-(3.0 * alpha + 2.0 * y - 2.0 * beta / w)));
  (*real_roots)[3] = temp.real();

  return 0;
}

}  // namespace p3p_kneip


/*
 [auto_generated]
 libs/numeric/odeint/test/adams_moulton.cpp

 [begin_description]
 This file tests the use of the Adams-Moulton stepper.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011-2012 Mario Mulansky

 Distributed under 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_TEST_MODULE odeint_adams_moulton

#include <utility>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/list.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/range_c.hpp>

#include <boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp>
#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>
#include <boost/numeric/odeint/stepper/adams_moulton.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;

struct lorenz
{
    template< class State , class Deriv , class Value >
    void operator()( const State &_x , Deriv &_dxdt , const Value &dt ) const
    {
        const value_type sigma = 10.0;
        const value_type R = 28.0;
        const value_type b = 8.0 / 3.0;

        typename boost::range_iterator< const State >::type x = boost::begin( _x );
        typename boost::range_iterator< Deriv >::type dxdt = boost::begin( _dxdt );

        dxdt[0] = sigma * ( x[1] - x[0] );
        dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = x[0]*x[1] - b * x[2];
    }
};

BOOST_AUTO_TEST_SUITE( adams_moulton_test )

BOOST_AUTO_TEST_CASE( test_adams_moulton_coefficients )
{
    detail::adams_moulton_coefficients< value_type , 1 > c1;
    detail::adams_moulton_coefficients< value_type , 2 > c2;
    detail::adams_moulton_coefficients< value_type , 3 > c3;
    detail::adams_moulton_coefficients< value_type , 4 > c4;
    detail::adams_moulton_coefficients< value_type , 5 > c5;
    detail::adams_moulton_coefficients< value_type , 6 > c6;
    detail::adams_moulton_coefficients< value_type , 7 > c7;
    detail::adams_moulton_coefficients< value_type , 8 > c8;
}

typedef boost::mpl::range_c< size_t , 1 , 6 > vector_of_steps;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_init_and_steps , step_type , vector_of_steps )
{
    const static size_t steps = step_type::value;
    typedef boost::array< value_type , 3 > state_type;

    adams_moulton< steps , state_type > stepper;
//    state_type x = {{ 10.0 , 10.0 , 10.0 }};
//    const value_type dt = 0.01;
//    value_type t = 0.0;

//    stepper.do_step( lorenz() , x , t , dt );
}

BOOST_AUTO_TEST_CASE( test_instantiation )
{
    typedef boost::array< double , 3 > state_type;
    adams_moulton< 1 , state_type > s1;
    adams_moulton< 2 , state_type > s2;
    adams_moulton< 3 , state_type > s3;
    adams_moulton< 4 , state_type > s4;
    adams_moulton< 5 , state_type > s5;
    adams_moulton< 6 , state_type > s6;
    adams_moulton< 7 , state_type > s7;
    adams_moulton< 8 , state_type > s8;

//    state_type x = {{ 10.0 , 10.0 , 10.0 }};
//    value_type t = 0.0 , dt = 0.01;
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Transformation.cpp
 *
 *  Created on: 15.02.2018
 *      Author: thies
 */

#include <base/ComposedFunction.h>
#include <base/Transformation.h>
#include <deal.II/base/function_parser.h>

namespace wavepi {
namespace base {

template <int dim>
DiscretizedFunction<dim> IdentityTransform<dim>::transform(const DiscretizedFunction<dim> &param) {
  return param;
}

template <int dim>
std::shared_ptr<LightFunction<dim>> IdentityTransform<dim>::transform(const std::shared_ptr<LightFunction<dim>> param) {
  return param;
}

template <int dim>
DiscretizedFunction<dim> IdentityTransform<dim>::transform_inverse(const DiscretizedFunction<dim> &param) {
  return param;
}

template <int dim>
DiscretizedFunction<dim> IdentityTransform<dim>::inverse_derivative(const DiscretizedFunction<dim> &param
                                                                    __attribute((unused)),
                                                                    const DiscretizedFunction<dim> &h) {
  return h;
}

template <int dim>
DiscretizedFunction<dim> IdentityTransform<dim>::inverse_derivative_transpose(const DiscretizedFunction<dim> &param
                                                                              __attribute((unused)),
                                                                              const DiscretizedFunction<dim> &g) {
  return g;
}

template <int dim>
void LogTransform<dim>::declare_parameters(ParameterHandler &prm) {
  prm.enter_subsection("LogTransform");
  prm.declare_entry("lower bound", "0.0", Patterns::Double(),
                    "transformation function for log transform is φ(x) = log(x-x₀), where x₀ is the lower bound you "
                    "want to enforce.");
  prm.leave_subsection();
}

template <int dim>
void LogTransform<dim>::get_parameters(ParameterHandler &prm) {
  prm.enter_subsection("LogTransform");
  lower_bound = prm.get_double("lower bound");
  prm.leave_subsection();
}

template <int dim>
DiscretizedFunction<dim> LogTransform<dim>::transform(const DiscretizedFunction<dim> &param) {
  AssertThrow(!param.has_derivative(), ExcMessage("Not transforming derivatives!"));

  DiscretizedFunction<dim> tmp(param.get_mesh(), param.get_norm());

  for (size_t i = 0; i < param.length(); i++)
    for (size_t j = 0; j < param[i].size(); j++) {
      Assert(param[i][j] > lower_bound,
             ExcMessage("LogTransform::transform called on param with entries <= lower bound"));
      tmp[i][j] = std::log(param[i][j] - lower_bound);
    }

  return tmp;
}

template <int dim>
std::shared_ptr<LightFunction<dim>> LogTransform<dim>::transform(const std::shared_ptr<LightFunction<dim>> param) {
  return std::make_shared<ComposedFunction<dim>>(param,
                                                 std::make_shared<LogTransform<dim>::TransformFunction>(lower_bound));
}

template <int dim>
DiscretizedFunction<dim> LogTransform<dim>::transform_inverse(const DiscretizedFunction<dim> &param) {
  AssertThrow(!param.has_derivative(), ExcMessage("Not transforming derivatives!"));

  DiscretizedFunction<dim> tmp(param.get_mesh(), param.get_norm());

  for (size_t i = 0; i < param.length(); i++)
    for (size_t j = 0; j < param[i].size(); j++)
      tmp[i][j] = std::exp(param[i][j]) + lower_bound;

  return tmp;
}

template <int dim>
DiscretizedFunction<dim> LogTransform<dim>::inverse_derivative(const DiscretizedFunction<dim> &param,
                                                               const DiscretizedFunction<dim> &h) {
  AssertThrow(!param.has_derivative() && !h.has_derivative(), ExcMessage("Not transforming derivatives!"));
  AssertThrow(param.get_mesh() == h.get_mesh(), ExcMessage("LogTransform: meshes must match"));

  DiscretizedFunction<dim> tmp(param.get_mesh(), param.get_norm());

  for (size_t i = 0; i < param.length(); i++)
    for (size_t j = 0; j < param[i].size(); j++)
      tmp[i][j] = std::exp(param[i][j]) * h[i][j];

  return tmp;
}
template <int dim>
DiscretizedFunction<dim> LogTransform<dim>::inverse_derivative_transpose(const DiscretizedFunction<dim> &param,
                                                                         const DiscretizedFunction<dim> &g) {
  return inverse_derivative(param, g);
}

template <int dim>
void ArtanhTransform<dim>::declare_parameters(ParameterHandler &prm) {
  prm.enter_subsection("ArtanhTransform");
  prm.declare_entry("lower bound", "0.0", Patterns::Double(),
                    "transformation function for log transform is φ: (a,b) → ℝ,  φ(x) = tanh⁻¹((2x-(a+b))/(b-a)) (pointwise), where a and b are the bounds you would like to enforce");
  prm.declare_entry("upper bound", "1.0", Patterns::Double(),
                    "transformation function for log transform is φ: (a,b) → ℝ,  φ(x) = tanh⁻¹(((2x-(a+b))/(b-a)) (pointwise), where a and b are the bounds you would like to enforce");
  prm.leave_subsection();
}

template <int dim>
void ArtanhTransform<dim>::get_parameters(ParameterHandler &prm) {
  prm.enter_subsection("ArtanhTransform");
  lower_bound = prm.get_double("lower bound");
  upper_bound = prm.get_double("upper bound");
  prm.leave_subsection();
}

template <int dim>
DiscretizedFunction<dim> ArtanhTransform<dim>::transform(const DiscretizedFunction<dim> &param) {
  AssertThrow(!param.has_derivative(), ExcMessage("Not transforming derivatives!"));

  DiscretizedFunction<dim> tmp(param.get_mesh(), param.get_norm());

  for (size_t i = 0; i < param.length(); i++)
    for (size_t j = 0; j < param[i].size(); j++) {
      Assert(param[i][j] > lower_bound,
             ExcMessage("ArtanhTransform::transform called on param with entries <= lower bound"));
      Assert(param[i][j] < upper_bound,
             ExcMessage("ArtanhTransform::transform called on param with entries >= upper bound"));

      tmp[i][j] = std::atanh((2 * param[i][j] - (lower_bound + upper_bound)) / (upper_bound - lower_bound));
    }

  return tmp;
}

template <int dim>
std::shared_ptr<LightFunction<dim>> ArtanhTransform<dim>::transform(const std::shared_ptr<LightFunction<dim>> param) {
  return std::make_shared<ComposedFunction<dim>>(param,
                                                 std::make_shared<ArtanhTransform<dim>::TransformFunction>(lower_bound, upper_bound));
}

template <int dim>
DiscretizedFunction<dim> ArtanhTransform<dim>::transform_inverse(const DiscretizedFunction<dim> &param) {
  AssertThrow(!param.has_derivative(), ExcMessage("Not transforming derivatives!"));

  DiscretizedFunction<dim> tmp(param.get_mesh(), param.get_norm());

  for (size_t i = 0; i < param.length(); i++)
    for (size_t j = 0; j < param[i].size(); j++)
      tmp[i][j] = (upper_bound-lower_bound)/2 * std::tanh(param[i][j]) + (lower_bound+upper_bound)/2;

  return tmp;
}

template <int dim>
DiscretizedFunction<dim> ArtanhTransform<dim>::inverse_derivative(const DiscretizedFunction<dim> &param,
                                                               const DiscretizedFunction<dim> &h) {
  AssertThrow(!param.has_derivative() && !h.has_derivative(), ExcMessage("Not transforming derivatives!"));
  AssertThrow(param.get_mesh() == h.get_mesh(), ExcMessage("ArtanhTransform: meshes must match"));

  DiscretizedFunction<dim> tmp(param.get_mesh(), param.get_norm());

  for (size_t i = 0; i < param.length(); i++)
    for (size_t j = 0; j < param[i].size(); j++) {
      double u = std::cosh(param[i][j]);
      tmp[i][j] = (upper_bound-lower_bound)/2 * 1/(u*u) * h[i][j];
    }

  return tmp;
}

template <int dim>
DiscretizedFunction<dim> ArtanhTransform<dim>::inverse_derivative_transpose(const DiscretizedFunction<dim> &param,
                                                                         const DiscretizedFunction<dim> &g) {
  return inverse_derivative(param, g);
}

template class Transformation<1>;
template class Transformation<2>;
template class Transformation<3>;

template class IdentityTransform<1>;
template class IdentityTransform<2>;
template class IdentityTransform<3>;

template class LogTransform<1>;
template class LogTransform<2>;
template class LogTransform<3>;

template class ArtanhTransform<1>;
template class ArtanhTransform<2>;
template class ArtanhTransform<3>;

}  // namespace base
}  // namespace wavepi


//=======================================================================
// Copyright (c) 2013 Robert Rosolek
//
// Distributed under 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)
//=======================================================================
/**
 * @file fractional_winner_determination_in_MUCA_example.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-06-09
 */

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/fractional_winner_determination_in_MUCA/fractional_winner_determination_in_MUCA.hpp"
#include "paal/auctions/xor_bids.hpp"

#include <boost/function_output_iterator.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <algorithm>
#include <iostream>
#include <iterator>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

//! [Fractional Winner Determination in MUCA Example]
int main()
{
   using Bidder = std::string;
   using Item = std::string;
   using Items = std::unordered_set<Item>;
   using Value = long double;
   using Bid = std::pair<Items, Value>;
   using Bids = std::vector<Bid>;
   using Assignment = std::tuple<Bidder, Items, double>;

   // create auction
   const std::unordered_map<Bidder, Bids> bids {
      {"John", {
         {{"lemon", "orange"}, 1},
         {{"apple", "ball"}, 1},
      }},
      {"Bob", {
         {{"lemon", "apple"}, 1},
         {{"orange", "ball"}, 1},
      }},
   };
   const std::vector<Bidder> bidders {"John", "Bob"};
   const std::vector<Item> items {"apple", "ball", "orange", "lemon"};
   auto get_bids = [&](const Bidder& bidder) -> const Bids& { return bids.at(bidder); };
   auto get_value = [](const Bid& bid) { return bid.second; };
   auto get_items = [](const Bid& bid) -> const Items& { return bid.first; };
   auto auction = paal::auctions::make_xor_bids_to_demand_query_auction(
      bidders, items, get_bids, get_value, get_items
   );

   // determine winners
   Value social_welfare = 0;
   auto valuation = paal::auctions::make_xor_bids_to_value_query_auction(
      std::move(bidders), std::move(items), get_bids, get_value, get_items
   );
   paal::auctions::fractional_determine_winners_in_demand_query_auction(
      auction,
      boost::make_function_output_iterator([&](Assignment a)
      {
         auto bidder = std::get<0>(a);
         auto& cur_items = std::get<1>(a);
         auto fraction = std::get<2>(a);
         social_welfare += fraction * valuation.call<paal::auctions::value_query>(bidder, cur_items);
         std::cout << bidder << " got a fraction " << fraction << " of bundle: ";
         boost::copy(cur_items, std::ostream_iterator<Item>(std::cout, ", "));
         std::cout << std::endl;
      })
   );
   std::cout << "social welfare: " << social_welfare << std::endl;

   return 0;
}
//! [Fractional Winner Determination in MUCA Example]


#ifndef GEOM_H
#define GEOM_H
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include "petsc.h"

namespace bg = boost::geometry;
typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::polygon<point_t> polygon_t;
typedef bg::model::segment<point_t> segment_t;
typedef bg::model::linestring<point_t> linestring_t;

class detector_geometry;

std::vector<double> linspace(double a, double b, size_t N);
void setMatrixElements(int tau_idx, int theta_idx, detector_geometry det,Mat A_mat);
void intersectionSet(linestring_t line, detector_geometry det, double theta,
                     std::vector<int>& index,
                     std::vector<double>& Lvec);

// Detector / sample geometry class
class detector_geometry{

public:
  double omega[4],m[2],dz[2],Tol;
  std::vector<double> thetan,x,y; //Maybe make this point_t
  std::vector<point_t> DetKnot0,SourceKnot0;
  polygon_t rectangle;
  PetscBool synthetic;
  char solFile[255];
  int numThetan,nTau;
  detector_geometry(){
    omega[0] = -2;
    omega[1] =  2;
    omega[2] = -2;
    omega[3] = 2;
    Tol = 1e-2;
    numThetan = 60;
    m[0] = 30; m[1] = 30;
    nTau = 30;
  }

  detector_geometry(PetscReal omega_in[4], PetscReal tol_in,
                    PetscInt numThetan_in, PetscInt m_in[2], PetscInt nTau_in,
                    char solFile_in[255]);
};

#endif


//
//=======================================================================
// Copyright (c) 2004 Kristopher Beevers
//
// Distributed under 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)
//=======================================================================
//

#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <utility>
#include <vector>
#include <list>
#include <iostream>
#include <math.h>    // for sqrt
#include <time.h>

using namespace boost;
using namespace std;

// auxiliary types
struct location
{
  float y, x; // lat, long
};
typedef float cost;

template <class Name, class LocMap>
class city_writer {
public:
  city_writer(Name n, LocMap l, float _minx, float _maxx,
              float _miny, float _maxy,
              unsigned int _ptx, unsigned int _pty)
    : name(n), loc(l), minx(_minx), maxx(_maxx), miny(_miny),
      maxy(_maxy), ptx(_ptx), pty(_pty) {}
  template <class Vertex>
  void operator()(ostream& out, const Vertex& v) const {
    float px = 1 - (loc[v].x - minx) / (maxx - minx);
    float py = (loc[v].y - miny) / (maxy - miny);
    out << "[label=\"" << name[v] << "\", pos=\""
        << static_cast<unsigned int>(ptx * px) << ","
        << static_cast<unsigned int>(pty * py)
        << "\", fontsize=\"11\"]";
  }
private:
  Name name;
  LocMap loc;
  float minx, maxx, miny, maxy;
  unsigned int ptx, pty;
};

template <class WeightMap>
class time_writer {
public:
  time_writer(WeightMap w) : wm(w) {}
  template <class Edge>
  void operator()(ostream &out, const Edge& e) const {
    out << "[label=\"" << wm[e] << "\", fontsize=\"11\"]";
  }
private:
  WeightMap wm;
};

// euclidean distance heuristic
template <class Graph, class CostType, class LocMap>
class distance_heuristic : public astar_heuristic<Graph, CostType>
{
public:
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  distance_heuristic(LocMap l, Vertex goal)
    : m_location(l), m_goal(goal) {}
  CostType operator()(Vertex u)
  {
    CostType dx = m_location[m_goal].x - m_location[u].x;
    CostType dy = m_location[m_goal].y - m_location[u].y;
    return ::sqrt(dx * dx + dy * dy);
  }
private:
  LocMap m_location;
  Vertex m_goal;
};

struct found_goal {}; // exception for termination

// visitor that terminates when we find the goal
template <class Vertex>
class astar_goal_visitor : public boost::default_astar_visitor
{
public:
  astar_goal_visitor(Vertex goal) : m_goal(goal) {}
  template <class Graph>
  void examine_vertex(Vertex u, Graph&) {
    if(u == m_goal)
      throw found_goal();
  }
private:
  Vertex m_goal;
};

int main(int, char **)
{

  // specify some types
  typedef adjacency_list<listS, vecS, undirectedS, no_property,
    property<edge_weight_t, cost> > mygraph_t;
  typedef property_map<mygraph_t, edge_weight_t>::type WeightMap;
  typedef mygraph_t::vertex_descriptor vertex;
  typedef mygraph_t::edge_descriptor edge_descriptor;
  typedef mygraph_t::vertex_iterator vertex_iterator;
  typedef std::pair<int, int> edge;

  // specify data
  enum nodes {
    Troy, LakePlacid, Plattsburgh, Massena, Watertown, Utica,
    Syracuse, Rochester, Buffalo, Ithaca, Binghamton, Woodstock,
    NewYork, N
  };
  const char *name[] = {
    "Troy", "Lake Placid", "Plattsburgh", "Massena",
    "Watertown", "Utica", "Syracuse", "Rochester", "Buffalo",
    "Ithaca", "Binghamton", "Woodstock", "New York"
  };
  location locations[] = { // lat/long
    {42.73, 73.68}, {44.28, 73.99}, {44.70, 73.46},
    {44.93, 74.89}, {43.97, 75.91}, {43.10, 75.23},
    {43.04, 76.14}, {43.17, 77.61}, {42.89, 78.86},
    {42.44, 76.50}, {42.10, 75.91}, {42.04, 74.11},
    {40.67, 73.94}
  };
  edge edge_array[] = {
    edge(Troy,Utica), edge(Troy,LakePlacid),
    edge(Troy,Plattsburgh), edge(LakePlacid,Plattsburgh),
    edge(Plattsburgh,Massena), edge(LakePlacid,Massena),
    edge(Massena,Watertown), edge(Watertown,Utica),
    edge(Watertown,Syracuse), edge(Utica,Syracuse),
    edge(Syracuse,Rochester), edge(Rochester,Buffalo),
    edge(Syracuse,Ithaca), edge(Ithaca,Binghamton),
    edge(Ithaca,Rochester), edge(Binghamton,Troy),
    edge(Binghamton,Woodstock), edge(Binghamton,NewYork),
    edge(Syracuse,Binghamton), edge(Woodstock,Troy),
    edge(Woodstock,NewYork)
  };
  unsigned int num_edges = sizeof(edge_array) / sizeof(edge);
  cost weights[] = { // estimated travel time (mins)
    96, 134, 143, 65, 115, 133, 117, 116, 74, 56,
    84, 73, 69, 70, 116, 147, 173, 183, 74, 71, 124
  };

  // create graph
  mygraph_t g(N);
  WeightMap weightmap = get(edge_weight, g);
  for(std::size_t j = 0; j < num_edges; ++j) {
    edge_descriptor e; bool inserted;
    boost::tie(e, inserted) = add_edge(edge_array[j].first,
                                       edge_array[j].second, g);
    weightmap[e] = weights[j];
  }

  // pick random start/goal
  boost::minstd_rand gen(time(0));
  vertex start = gen() % num_vertices(g);
  vertex goal = gen() % num_vertices(g);

  cout << "Start vertex: " << name[start] << endl;
  cout << "Goal vertex: " << name[goal] << endl;

  vector<mygraph_t::vertex_descriptor> p(num_vertices(g));
  vector<cost> d(num_vertices(g));
  try {
    // call astar named parameter interface
    astar_search
      (g, start,
       distance_heuristic<mygraph_t, cost, location*>
        (locations, goal),
       predecessor_map(&p[0]).distance_map(&d[0]).
       visitor(astar_goal_visitor<vertex>(goal)));

  } catch(found_goal fg) { // found a path to the goal
    list<vertex> shortest_path;
    for(vertex v = goal;; v = p[v]) {
      shortest_path.push_front(v);
      if(p[v] == v)
        break;
    }
    cout << "Shortest path from " << name[start] << " to "
         << name[goal] << ": ";
    list<vertex>::iterator spi = shortest_path.begin();
    cout << name[start];
    for(++spi; spi != shortest_path.end(); ++spi)
      cout << " -> " << name[*spi];
    cout << endl << "Total travel time: " << d[goal] << endl;
    return 0;
  }

  cout << "Didn't find a path from " << name[start] << "to"
       << name[goal] << "!" << endl;
  return 0;

}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2012.
// Distributed under 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)

#include <boost/metaparse/repeated.hpp>
#include <boost/metaparse/sequence.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/space.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/get_result.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/transform.hpp>
#include <boost/metaparse/always.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>

using boost::metaparse::sequence;
using boost::metaparse::lit_c;
using boost::metaparse::last_of;
using boost::metaparse::space;
using boost::metaparse::repeated;
using boost::metaparse::build_parser;
using boost::metaparse::int_;
using boost::metaparse::foldl_reject_incomplete_start_with_parser;
using boost::metaparse::get_result;
using boost::metaparse::one_of;
using boost::metaparse::token;
using boost::metaparse::entire_input;
using boost::metaparse::transform;
using boost::metaparse::always;

using boost::mpl::apply_wrap1;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::if_;
using boost::mpl::bool_;

/*
 * The grammar
 *
 * expression ::= plus_exp
 * plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*
 * prod_exp ::= value_exp ((mult_token | div_token) value_exp)*
 * value_exp ::= int_token | '_'
 */

typedef token<lit_c<'+'> > plus_token;
typedef token<lit_c<'-'> > minus_token;
typedef token<lit_c<'*'> > mult_token;
typedef token<lit_c<'/'> > div_token;

typedef token<int_> int_token;
typedef token<lit_c<'_'> > arg_token;

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct build_plus
{
  template <class A, class B>
  class _plus
  {
  public:
    typedef _plus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) + _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _minus
  {
  public:
    typedef _minus type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) - _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '+'>::type,
      _plus<State, typename back<C>::type>,
      _minus<State, typename back<C>::type>
    >
  {};
};

struct build_mult
{
  template <class A, class B>
  class _mult
  {
  public:
    typedef _mult type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) * _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class A, class B>
  class _div
  {
  public:
    typedef _div type;

    template <class T>
    T operator()(T t) const
    {
      return _left(t) / _right(t);
    }
  private:
    typename A::type _left;
    typename B::type _right;
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '*'>::type,
      _mult<State, typename back<C>::type>,
      _div<State, typename back<C>::type>
    >
  {};
};

struct build_value
{
  typedef build_value type;

  template <class V>
  struct apply
  {
    typedef apply type;

    template <class T>
    int operator()(T) const
    {
      return V::type::value;
    }
  };
};

struct arg
{
  typedef arg type;

  template <class T>
  T operator()(T t) const
  {
    return t;
  }
};

typedef
  one_of<transform<int_token, build_value>, always<arg_token, arg> >
  value_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<mult_token, div_token>, value_exp>,
    value_exp,
    build_mult
  >
  prod_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<plus_token, minus_token>, prod_exp>,
    prod_exp,
    build_plus
  >
  plus_exp;

typedef last_of<repeated<space>, plus_exp> expression;

typedef build_parser<entire_input<expression> > function_parser;

#if BOOST_METAPARSE_STD < 2011

template <class Exp>
struct lambda : apply_wrap1<function_parser, Exp> {};

using boost::metaparse::string;

lambda<string<'1','3'> >::type f1;
lambda<string<'2',' ','+',' ','3'> >::type f2;
lambda<string<'2',' ','*',' ','2'> >::type f3;
lambda<string<' ','1','+',' ','2','*','4','-','6','/','2'> >::type f4;
lambda<string<'2',' ','*',' ','_'> >::type f5;

#else

#ifdef LAMBDA
  #error LAMBDA already defined
#endif
#define LAMBDA(exp) apply_wrap1<function_parser, BOOST_METAPARSE_STRING(#exp)>::type

LAMBDA(13) f1;
LAMBDA(2 + 3) f2;
LAMBDA(2 * 2) f3;
LAMBDA( 1+ 2*4-6/2) f4;
LAMBDA(2 * _) f5;

#endif

int main()
{
  using std::cout;
  using std::endl;

  cout
    << f1(11) << endl
    << f2(11) << endl
    << f3(11) << endl
    << f4(11) << endl
    << f5(11) << endl
    << f5(1.1) << endl
    ;
}


#define GLM_ENABLE_EXPERIMENTAL 1
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Halide.h>
#include <fstream>
#include <glm/ext.hpp>
#include <glm/glm.hpp>
#include <halide_image_io.h>
#include <iostream>
#include <stdio.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>

using std::string;
using std::stringstream;
using std::vector;

using namespace std;
using namespace Halide;
using namespace Halide::Tools;
using namespace Eigen;

using Vector4h = Matrix<Halide::Expr, 4, 1>;
using Matrix4h = Matrix<Halide::Expr, 4, 4>;

Var x, y, c;

struct Point
{
    float x;
    float y;
    float z;
    uint16_t r;
    uint16_t g;
    uint16_t b;
};

Matrix4h read4x4MatFromCSV(string csvFile)
{
    Matrix4h transfMat;
    ifstream in(csvFile);
    vector<float> floatVec;
    if (in)
    {
        string line;
        while (getline(in, line))
        {
            stringstream sep(line);
            string field;
            while (getline(sep, field, ','))
            {
                float val = stod(field);
                floatVec.push_back(val);
            }
        }
    }
    transfMat << floatVec[0], floatVec[1], floatVec[2], floatVec[3],
        floatVec[4], floatVec[5], floatVec[6], floatVec[7],
        floatVec[8], floatVec[9], floatVec[10], floatVec[11],
        floatVec[12], floatVec[13], floatVec[14], floatVec[15];
    return transfMat;
}

ostream &operator<<(ostream &os, const Matrix4h &m)
{
    os << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << " " << m(0, 3) << "\n"
       << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << " " << m(1, 3) << "\n"
       << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << " " << m(2, 3) << "\n"
       << m(3, 0) << " " << m(3, 1) << " " << m(3, 2) << " " << m(3, 3) << endl;
    return os;
}

void saveImage(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(clamp(result, 0.0f, 1.0f) * 255.0f);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename << ".png";
    save_image(output, filename.str());
}

ostream &operator<<(ostream &os, const Vector4h &v)
{
    os << v(0) << "\n"
       << v(1) << "\n"
       << v(2) << "\n"
       << v(3) << endl;
    return os;
}

Matrix4h getInvCameraMat(float focalLen, float pxDim, int width, int height)
{

    focalLen = focalLen * 10e-3;
    float u0 = width / 2.0;
    float v0 = height / 2.0;
    Matrix4h camMat;
    float zero = 0.0;
    float one = 1.0;
    camMat << pxDim / focalLen, 0.0f, -pxDim * u0 / focalLen, 0.0f,
        0.0f, pxDim / focalLen, -pxDim * v0 / focalLen, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f;

    return camMat;
}

Matrix4h getTransMatProjToCam()
{
    Matrix4h transMat;
    transMat << 0.9945219f, 0.0f, -0.10452846f, -0.2f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.10452846f, 0.0f, 0.9945219f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f;
    return transMat;
}

Matrix4h getTransMatWorldToProj()
{
    Matrix4h transMat;
    transMat << -0.99862951f, 0.0f, 0.0523359f, 0.1f,
        0.00908804f, -0.1734101f, 0.9986295f, 3.0f,
        -0.052335f, -1.0f, 0.0f, 1.0f,
        0.0f, 0.0f, 0.0f, 1.0f;
    return transMat;
}

Vector4h makeAxBzCLine(Vector4h a, Vector4h b)
{
    Vector4h result;
    result(0) = a(2) - b(2);
    result(1) = b(0) - a(0);
    result(2) = a(0) * b(2) - b(0) * a(2);
    result(3) = 1.0f;
    return result;
}

Vector4h cross3(Vector4h a, Vector4h b)
{
    Vector4h res;
    res(0) = a(1) * b(2) - a(2) * b(1);
    res(1) = a(2) * b(0) - a(0) * b(2);
    res(2) = a(0) * b(1) - a(1) * b(0);
    res(3) = 0;
    return res;
}

Expr dot3(Vector4h a, Vector4h b)
{
    Expr res;
    res = a(0) * b(0) + a(1) * b(1) + a(2) * b(2);
    return res;
}

tuple<Vector4h, Vector4h> pluckerLine(Vector4h p1, Vector4h p2)
{
    Vector4h l;
    Vector4h l_dash;
    l = p1(3) * p2 - p2(3) * p1;
    l(3) = 0;
    l_dash = cross3(p1, p2);
    return {l, l_dash};
}

Vector4h pluckerPlane(Vector4h l, Vector4h l_dash, Vector4h pluckPt)
{
    Vector4h u;
    u = -pluckPt(3) * l_dash + cross3(pluckPt, l);
    u(3) = dot3(pluckPt, l_dash);
    return u;
}

Vector4h intersectionLinePlane(Vector4h l, Vector4h l_dash, Vector4h plPlane)
{
    Vector4h intersection;
    intersection = -plPlane(3) * l + cross3(plPlane, l_dash);
    intersection(3) = dot3(plPlane, l);
    return intersection;
}

template <typename Function>
void writeBufferToXYZFile(Buffer<float> &buffer, Buffer<uint8_t> &color, string filename, string deliminator, Function condFunc)
{
    std::vector<Point> points;
    for (int j = 0; j < buffer.height(); j++)
    {
        for (int i = 0; i < buffer.width(); i++)
        {
            const auto x = buffer(i, j, 0);
            const auto y = buffer(i, j, 1);
            const auto z = buffer(i, j, 2);
            uint8_t r = color(i, j, 0);
            uint8_t g = color(i, j, 1);
            uint8_t b = color(i, j, 2);
            if (condFunc(x, y, z))
            {
                continue;
            }
            points.push_back({x, y, z, r, g, b});
        }
    }
    std::ofstream outFile;
    outFile.open(filename);
    outFile << points.size() << "\n";
    const auto d = deliminator;
    for (const auto &point : points)
    {
        outFile << point.x << d << point.y << d << point.z << d << point.r << d << point.g << d << point.b << "\n";
    }
}

bool conditionProjector(float x, float y, float z)
{
    return (x < -10.0f || x > 10.0f || y < -10.0f || y > 10.0f || z < -10.0f || z > 10.0f);
}

bool noCondition(float x, float y, float z)
{
    return false;
}

int main(int argc, char **argv)
{
    const float FOCAL_LEN = 36.1;
    const float PX_DIM = 10 * 10e-6;
    const bool POINT_CLOUD_GLOBAL_FRAME = true;
    const bool POINT_CLOUD_PROJ_FRAME = true;
    const string PROJECTOR_X_PNG = "images/x-val-img.png";
    const string TRANSF_MAT_WORLD_TO_PROJ_CSV = "matrices/transf-world-proj.csv";
    const string TRANSF_PROJ_CAM_CSV = "matrices/transf-proj-cam.csv";
    const string INV_CAM_MAT_CSV = "matrices/inv-cam-mat.csv";
    const string SAVE_DEPTH_IMAGE = "images/depth_img.png";
    const string SAVE_XYZ_PROJ = "pointclouds/proj.txt";
    const string SAVE_XYZ_WORLD = "pointclouds/world.txt";
    const string NO_PROJECTOR_PNG = "images/no-projector.png";

    Halide::Buffer<uint8_t> input = load_image(PROJECTOR_X_PNG);
    const int HEIGHT = input.height();
    const int WIDTH = input.width();

    Matrix4h InvK = read4x4MatFromCSV(INV_CAM_MAT_CSV);
    Matrix4h transMatProjToCam = read4x4MatFromCSV(TRANSF_PROJ_CAM_CSV);
    Matrix4h transMatWorldToProj = read4x4MatFromCSV(TRANSF_MAT_WORLD_TO_PROJ_CSV);

    cout << "Tranformation Matrix World to Projector" << endl;
    cout << transMatWorldToProj << endl;

    Vector4h px{x, y, 1.0f, 0.0f};

    Vector4h normCam1 = InvK * px;
    normCam1(3) = 1.0f;
    Vector4h normCam2 = 2 * normCam1;
    normCam2(3) = 1.0f;

    Vector4h pointCam1 = transMatProjToCam * normCam1;
    pointCam1 = pointCam1 / pointCam1(3);
    Vector4h pointCam2 = transMatProjToCam * normCam2;
    pointCam2 /= pointCam2(3);

    const auto [camLine, camLineDash] = pluckerLine(pointCam1, pointCam2);
    //Vector4h cameraLine = makeAxBzCLine(pointCam1, pointCam2);

    Vector4h pxProj{input(x, y) / 255.0f * 1920.0f, 0.0f, 1.0f, 0.0f};
    Vector4h normProj1 = InvK * pxProj;
    normProj1 = normProj1 / normProj1(2);
    normProj1(3) = 1.0f;
    Vector4h normProj2{0.0f, 0.0f, 0.0f, 1.0f};
    Vector4h normProj3{0.0f, 1.0f, 0.0f, 1.0f};

    const auto [projLine, projLineDash] = pluckerLine(normProj2, normProj3);
    Vector4h ProjPlane = pluckerPlane(projLine, projLineDash, normProj1);
    Vector4h intersection = intersectionLinePlane(camLine, camLineDash, ProjPlane);
    intersection = intersection / intersection(3);
    intersection *= (input(x, y) > 0); // does this make sense?

    Expr depth = intersection(3);

    saveImage(depth, input.width(), input.height(), SAVE_DEPTH_IMAGE);

    Halide::Buffer<uint8_t> color = load_image(NO_PROJECTOR_PNG);

    if (POINT_CLOUD_PROJ_FRAME)
    {
        Func result;
        result(x, y, c) = 0.0f;
        result(x, y, 0) = intersection(0);
        result(x, y, 1) = intersection(1);
        result(x, y, 2) = intersection(2);

        Buffer<float> output(input.width(), input.height(), 3);
        result.realize(output);
        writeBufferToXYZFile(output, color, SAVE_XYZ_PROJ, ";", conditionProjector);
    }
    if (POINT_CLOUD_GLOBAL_FRAME)
    {

        Vector4h ptGlobalFrame = transMatWorldToProj * intersection;
        Func result_w;
        result_w(x, y, c) = 0.0f;
        result_w(x, y, 0) = ptGlobalFrame(0);
        result_w(x, y, 1) = ptGlobalFrame(1);
        result_w(x, y, 2) = ptGlobalFrame(2);

        Buffer<float> output_w(input.width(), input.height(), 3);
        result_w.realize(output_w);
        writeBufferToXYZFile(output_w, color, SAVE_XYZ_WORLD, ";", conditionProjector);
    }

    return 0;
}


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file Transform3DMatConvertersTest.cpp
 * @date 20/02/2018
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup CommonTests
 *
 * Testing conversion from cv Matrix to Transform3D and viceversa.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <catch.hpp>
#include <Converters/Transform3DToMatConverter.hpp>
#include <Converters/MatToTransform3DConverter.hpp>
#include <Types/CPP/Pose.hpp>
#include <Errors/Assert.hpp>
#include <boost/smart_ptr.hpp>
#include <Eigen/Geometry>

using namespace Converters;
using namespace PoseWrapper;
const double EPSILON = 0.000000001;

TEST_CASE( "Mat to Transform 3D and Back", "[MatToTransform]" )
	{
	MatToTransform3DConverter firstConverter;
	Transform3DToMatConverter secondConverter;

	Eigen::Matrix4f inputTransform;
	Eigen::Quaternionf inputRotation(std::cos(M_PI/2), std::sin(M_PI/2), std::sin(M_PI/2)/2, std::sin(M_PI/2)*2);
	Eigen::Matrix3f eigenRotationMatrix = inputRotation.normalized().toRotationMatrix();
	inputTransform << 	eigenRotationMatrix(0,0), eigenRotationMatrix(0,1), eigenRotationMatrix(0,2), 0.3,
				eigenRotationMatrix(1,0), eigenRotationMatrix(1,1), eigenRotationMatrix(1,2), 0.4,
				eigenRotationMatrix(2,0), eigenRotationMatrix(2,1), eigenRotationMatrix(2,2), 0.6,
				0, 0, 0, 1;

	cv::Mat cvInputTransform(3, 4, CV_32FC1);
	for(unsigned row = 0; row<3; row++)
		{
		for(unsigned column = 0; column < 4; column++)
			{
			cvInputTransform.at<float>(row, column) = inputTransform(row, column);
			}
		}

	Transform3DSharedConstPtr asnTransform3D = firstConverter.ConvertShared(cvInputTransform);
	REQUIRE( GetXPosition(*asnTransform3D) < inputTransform(0, 3) + EPSILON );
	REQUIRE( GetXPosition(*asnTransform3D) > inputTransform(0, 3) - EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) < inputTransform(1, 3) + EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) > inputTransform(1, 3) - EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) < inputTransform(2, 3) + EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) > inputTransform(2, 3) - EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) < inputRotation.normalized().x() + EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) > inputRotation.normalized().x() - EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) < inputRotation.normalized().y() + EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) > inputRotation.normalized().y() - EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) < inputRotation.normalized().z() + EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) > inputRotation.normalized().z() - EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) < inputRotation.normalized().w() + EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) > inputRotation.normalized().w() - EPSILON );

	cv::Mat outputTransform = secondConverter.ConvertShared(asnTransform3D);
	for(unsigned rowIndex = 0; rowIndex < 3; rowIndex++)
		{
		for(unsigned columnIndex = 0; columnIndex < 4; columnIndex++)
			{
			REQUIRE(outputTransform.at<float>(rowIndex, columnIndex) < inputTransform(rowIndex, columnIndex) + EPSILON);
			REQUIRE(outputTransform.at<float>(rowIndex, columnIndex) > inputTransform(rowIndex, columnIndex) - EPSILON);
			}
		}

	asnTransform3D.reset();
	}

TEST_CASE( "Transform3D to Mat and Back", "[Transform3DToMat]" )
	{
	MatToTransform3DConverter firstConverter;
	Transform3DToMatConverter secondConverter;

	Eigen::Matrix4f inputTransform;
	Eigen::Quaternionf inputRotation(std::cos(M_PI/2), std::sin(M_PI/2), std::sin(M_PI/2)/2, std::sin(M_PI/2)*2);
	Eigen::Matrix3f eigenRotationMatrix = inputRotation.normalized().toRotationMatrix();
	inputTransform << 	eigenRotationMatrix(0,0), eigenRotationMatrix(0,1), eigenRotationMatrix(0,2), 0.3,
				eigenRotationMatrix(1,0), eigenRotationMatrix(1,1), eigenRotationMatrix(1,2), 0.4,
				eigenRotationMatrix(2,0), eigenRotationMatrix(2,1), eigenRotationMatrix(2,2), 0.6,
				0, 0, 0, 1;

	cv::Mat cvInputTransform(3, 4, CV_32FC1);
	for(unsigned row = 0; row<3; row++)
		{
		for(unsigned column = 0; column < 4; column++)
			{
			cvInputTransform.at<float>(row, column) = inputTransform(row, column);
			}
		}

	Transform3DSharedConstPtr asnTransform3D = firstConverter.ConvertShared(cvInputTransform);
	cv::Mat intermediateTransform = secondConverter.ConvertShared(asnTransform3D);
	Transform3DSharedConstPtr outputTransform = firstConverter.ConvertShared(intermediateTransform);

	REQUIRE( GetXPosition(*asnTransform3D) < GetXPosition(*outputTransform) + EPSILON );
	REQUIRE( GetXPosition(*asnTransform3D) > GetXPosition(*outputTransform) - EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) < GetYPosition(*outputTransform) + EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) > GetYPosition(*outputTransform) - EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) < GetZPosition(*outputTransform) + EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) > GetZPosition(*outputTransform) - EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) < GetXOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) > GetXOrientation(*outputTransform) - EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) < GetYOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) > GetYOrientation(*outputTransform) - EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) < GetZOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) > GetZOrientation(*outputTransform) - EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) < GetWOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) > GetWOrientation(*outputTransform) - EPSILON );

	asnTransform3D.reset();
	outputTransform.reset();
	}

TEST_CASE("Bad Transform conversion 1 (Mat)", "[Bad Transform 1]")
	{
	MatToTransform3DConverter firstConverter;

	Eigen::Matrix4f inputTransform;
	inputTransform << 0.3, 0.2, 0.1, 0.4, 0.3, 0.2, 0.1, 0.1, 0.5, 0.3, 0.2, 0.1, 1, 0, 0, 1;

	cv::Mat cvInputTransform(3, 4, CV_32FC1);
	for(unsigned row = 0; row<3; row++)
		{
		for(unsigned column = 0; column < 4; column++)
			{
			cvInputTransform.at<float>(row, column) = inputTransform(row, column);
			}
		}

	REQUIRE_THROWS( firstConverter.ConvertShared(cvInputTransform) );
	}

TEST_CASE("Bad Transform conversion 2 (Mat)", "[Bad Transform 2]")
	{
	MatToTransform3DConverter firstConverter;

	Eigen::Matrix4f inputTransform;
	inputTransform << 0.3, 0.2, 0.1, 0.4, 0.3, 0.2, 0.1, 0.1, 0.5, 0.3, 0.2, 0.1, 0, 0, 0, 1;

	cv::Mat cvInputTransform(3, 4, CV_32FC1);
	for(unsigned row = 0; row<3; row++)
		{
		for(unsigned column = 0; column < 4; column++)
			{
			cvInputTransform.at<float>(row, column) = inputTransform(row, column);
			}
		}

	REQUIRE_THROWS( firstConverter.ConvertShared(cvInputTransform) );
	}


//#include <bits/stdc++.h>

#include <algorithm>
#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <vector>
#include "time.h"

#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>

#include <Spectra/SymEigsSolver.h>

#include "alstructure.h"
#include "config.h"
#include "storage.h"

struct timespec t0;

// https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix
template<typename M>
M load_tsv (const std::string & path) {
	std::ifstream indata;
	indata.open(path);
	std::string line;
	std::vector<double> values;
	int rows = 0;
	while (std::getline(indata, line)) {
		std::stringstream lineStream(line);
		std::string cell;
		while (std::getline(lineStream, cell, '\t')) {
			values.push_back(std::stod(cell));
		}
		++rows;
	}
	return Eigen::Map<const Eigen::Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, Eigen::RowMajor>>(values.data(), rows, values.size()/rows);
}

template<typename M>
M read_plink_freq_file (const std::string &path, int k) {
	/*
	Reads from plink.frq.strat file to initialize P matrix
	Returns rows/k x k matrix, where rows in number of rows
		in file
	*/
        std::ifstream indata;
        indata.open(path);
        std::string line;
        std::vector<double> values;
        int rows = 1;
        std::getline(indata, line);
        while (std::getline(indata, line)) {
                std::stringstream lineStream(line);
                std::string cell;
                std::vector<std::string> seglist;
                while (lineStream >> cell){
                        seglist.push_back(cell);
                }
                values.push_back(1-std::stod(seglist[5]));
                ++rows;
        }
        return Eigen::Map<const Eigen::Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, Eigen::RowMajor>>(values.data(), rows/k, k);
}

double fix_interval(double x) {
	/* Random function in Eigen generates numbers from a
	 * uniform distribution on the interval [-1, 1].
	 * We need to map these values to the interval [0, 1].
	 * To map x in [a, b] to [c, d]
	 * f(x) = c + ((d-c)/(b-a)) * (x-a)
	 */
	return (0.5 * (x + 1.0));
}

double divide_by_two(double x) {
	return x / 2.0;
}

double truncate_with_epsilon(double x) {
	double epsilon = 0.0000000001;
	if (x <= 0.0)
		return epsilon;
	if (x >= 1.0)
		return 1.0 - epsilon;
	return x;
}

double truncate_xxx(double x) {
	if (x < 0.0)
		return 0.0;
	if (x > 1.0)
		return 1.0;
	return x;
}

void project_onto_simplex(std::vector<double> &data) {
	std::vector<size_t> inds(data.size());

	std::iota(inds.begin(), inds.end(), 0);

	std::stable_sort(inds.begin(), inds.end(), [&data](size_t i, size_t j) { return data[i] > data[j]; });

	double tmpsum = 0;
	double tmax;
	bool bget = false;

	for (int i = 1; i < data.size(); i++) {
		tmpsum = tmpsum + data[inds[i - 1]];
		tmax = (tmpsum - 1.0) / i;
		if (tmax >= data[inds[i]]) {
			bget = true;
			break;
		}
	}

	if (!(bget)) {
		tmpsum = tmpsum + data[inds[data.size() - 1]];
		tmax = (tmpsum - 1) / data.size();
	}

	for (int i = 0; i < data.size(); i++) {
		data[i] = data[i] - tmax;
		if (data[i] < 0.0) {
			data[i] = 0.0;
		}
	}
}

ALStructure::ALStructure(int argc, char const *argv[]) {
	// Set default values
	command_line_opts.num_of_evec = 5;
	command_line_opts.debugmode = false;
	command_line_opts.OUTPUT_PATH = "scope_";
	bool got_genotype_file = false;
	bool got_rowspace_file = false;
	bool got_freq_file = false;
	command_line_opts.convergence_limit = 0.00001;  // Used by R code
	command_line_opts.max_iterations = 1000;        // Used by R code
	command_line_opts.memory_efficient = false;
	command_line_opts.fast_mode = true;
	command_line_opts.missing = false;
	command_line_opts.text_version = false;
	command_line_opts.fhat_version = false;
	command_line_opts.fhattrunc_version = false;
	command_line_opts.nthreads = 1;
	command_line_opts.seed = -1;
	command_line_opts.given_seed = false;
	nops = 1;

	if (argc < 3) {
		printCorrectUsage();
		//std::cout << "Correct Usage is " << argv[0] << " -p <parameter file>" << std::endl;
		exit(-1);
	}

	if (strcmp(argv[1], "-p") == 0) {
		// Read arguments from configuration file
		std::string cfg_filename = std::string(argv[2]);
		ConfigFile cfg(cfg_filename);
		got_genotype_file = cfg.keyExists("genotype");
		command_line_opts.num_of_evec = cfg.getValueOfKey<int>("num_evec", 5);
		command_line_opts.max_iterations = cfg.getValueOfKey<int>("max_iterations", command_line_opts.num_of_evec + 2);
		command_line_opts.debugmode = cfg.getValueOfKey<bool>("debug", false);
		command_line_opts.OUTPUT_PATH = cfg.getValueOfKey<std::string>("output_path", std::string("fastppca_"));
		command_line_opts.GENOTYPE_FILE_PATH = cfg.getValueOfKey<std::string>("genotype", std::string(""));
		command_line_opts.convergence_limit = cfg.getValueOfKey<double>("convergence_limit", -1.0);
		command_line_opts.memory_efficient = cfg.getValueOfKey<bool>("memory_efficient", false);
		command_line_opts.fast_mode = cfg.getValueOfKey<bool>("fast_mode", true);
		command_line_opts.missing = cfg.getValueOfKey<bool>("missing", false);
		command_line_opts.text_version = cfg.getValueOfKey<bool>("text_version", false);
		command_line_opts.nthreads = cfg.getValueOfKey<int>("nthreads", 1);
		command_line_opts.seed = cfg.getValueOfKey<int>("seed", -1);
		command_line_opts.given_seed = command_line_opts.seed >= 0 ? true: false;
	} else {
		// Read arguments from standard input
		bool got_max_iter = false;
		for (int i = 1; i < argc; i++) {
			if (i + 1 != argc) {
				if (strcmp(argv[i], "-g") == 0) {
					command_line_opts.GENOTYPE_FILE_PATH = std::string(argv[i+1]);
					got_genotype_file = true;
					i++;
				} else if (strcmp(argv[i], "-r") == 0) {
					command_line_opts.ROWSPACE_FILE_PATH = std::string(argv[i+1]);
					got_rowspace_file = true;
					i++;
				} else if (strcmp(argv[i], "-freq") == 0) {
					command_line_opts.FREQ_FILE_PATH = std::string(argv[i+1]);
					got_freq_file = true;
					i++;
				} else if (strcmp(argv[i], "-i") == 0) {
					command_line_opts.INITIAL_FILE_PATH = std::string(argv[i+1]);
					i++;
				} else if (strcmp(argv[i], "-o") == 0) {
					command_line_opts.OUTPUT_PATH = std::string(argv[i+1]);
					i++;
				} else if (strcmp(argv[i], "-k") == 0) {
					command_line_opts.num_of_evec = atoi(argv[i+1]);
					i++;
				} else if (strcmp(argv[i], "-m") == 0) {
					command_line_opts.max_iterations = atoi(argv[i+1]);
					got_max_iter = true;
					i++;
				} else if (strcmp(argv[i], "-nt") == 0) {
					command_line_opts.nthreads = atoi(argv[i+1]);
					i++;
				} else if (strcmp(argv[i], "-seed") == 0) {
					command_line_opts.seed = atoi(argv[i+1]);
					command_line_opts.given_seed = command_line_opts.seed >= 0 ? true: false;
					i++;
				} else if (strcmp(argv[i], "-cl") == 0) {
					command_line_opts.convergence_limit = atof(argv[i+1]);
					i++;
				} else if (strcmp(argv[i], "-v") == 0) {
					command_line_opts.debugmode = true;
				} else if (strcmp(argv[i], "-mem") == 0) {
					command_line_opts.memory_efficient = true;
				} else if (strcmp(argv[i], "-miss") == 0) {
					command_line_opts.missing = true;
				} else if (strcmp(argv[i], "-nfm") == 0) {
					command_line_opts.fast_mode = false;
				} else if (strcmp(argv[i], "-txt") == 0) {
					command_line_opts.text_version = true;
				} else if (strcmp(argv[i], "-fhat") == 0) {
					command_line_opts.fhat_version = true;
				} else if (strcmp(argv[i], "-fhattrunc") == 0) {
					command_line_opts.fhattrunc_version = true;
				} else {
					std::cout << "Not Enough or Invalid arguments" << std::endl;
					printCorrectUsage();
					exit(-1);
				}
			} else if (strcmp(argv[i], "-v") == 0) {
				command_line_opts.debugmode = true;
			} else if (strcmp(argv[i], "-mem") == 0) {
				command_line_opts.memory_efficient = true;
			} else if (strcmp(argv[i], "-nfm") == 0) {
				command_line_opts.fast_mode = false;
			} else if (strcmp(argv[i], "-miss") == 0) {
				command_line_opts.missing = true;
			} else if (strcmp(argv[i], "-txt") == 0) {
				command_line_opts.text_version = true;
			} else if (strcmp(argv[i], "-fhat") == 0) {
				command_line_opts.fhat_version = true;
			} else if (strcmp(argv[i], "-fhattrunc") == 0) {
				command_line_opts.fhattrunc_version = true;
			}
		}
	}

	if (got_genotype_file == false) {
		std::cout << "Genotype file missing" << std::endl;
		printCorrectUsage();
		exit(-1);
	}
}

void ALStructure::printCorrectUsage(void) {
	std::cout << "SCOPE Options:\n"
			  << "-g <genotype file> Path to PLINK binary prefix\n"
			  << "-k <latent dimension> Number of latent population (default: 5)\n"
			  << "-m <max iterations> Maximum number of iterations for ALS (default: 1000)\n"
			  << "-cl <convergence limit> Convergence threshold for LSE and ALS (default: 0.00001)\n"
			  << "-nt <number of threads> Number of threads to use (default: 1)\n"
       			  << "-seed <seed> Seed to use (default: system time)\n"
			  << "-freq <frequency file> Path to PLINK frequency file for supervision (default: none)\n"
			  << "-o <output_path> Output prefix (default: scope_)\n";
}

void ALStructure::solve_for_Qhat() {
	if (fhat_version || fhattrunc_version) {
		Qhat = (((Phat.transpose() * Phat).inverse() * Phat.transpose()) * Fhat);
	}

	MatrixXdr temp_kxp(k, p);
	temp_kxp = ((Phat.transpose() * Phat).inverse()) * Phat.transpose();

	// temp_kxn = temp_kxp * X, where X is the p X n genotype matrix
	MatrixXdr temp_kxn(k, n);
	mm.multiply_y_post(temp_kxp, k, temp_kxn, false);

	temp_kxn = temp_kxn.unaryExpr(&divide_by_two);

	Qhat = (temp_kxn * V) * V.transpose();
}

void ALStructure::solve_for_Phat() {
	if (fhat_version || fhattrunc_version) {
		Phat = (Fhat * (Qhat.transpose() * (( Qhat * Qhat.transpose() ).inverse()) ));
	}

	MatrixXdr temp_nxk(n, k);
	temp_nxk = V * (V.transpose() * ((Qhat.transpose() * (Qhat * Qhat.transpose()).inverse())));

	mm.multiply_y_pre(temp_nxk, k, Phat, false);

	Phat = Phat.unaryExpr(&divide_by_two);
}

void ALStructure::initialize(std::default_random_engine &prng_eng) {
	if (!command_line_opts.given_seed) {
		seed = static_cast<unsigned int>(time(NULL));
	}

	std::cout << "Initializing Phat using seed " << seed << std::endl;

	// srand(seed);
	prng_eng.seed(seed);

	// Phat = MatrixXdr::Random(p, k);
	// Phat = Phat.unaryExpr(&fix_interval);
	std::uniform_real_distribution<double> dis(0, 1);
	Phat = MatrixXdr::Zero(p, k).unaryExpr([&](float dummy){return dis(prng_eng);});

	if (debug) write_matrix(Phat, std::string("Phat_0_") + std::to_string(seed) +  std::string(".txt"));
}

void ALStructure::truncated_alternating_least_squares(bool projection_mode) {

	if (projection_mode){
		std::cout << "Solving for Q using provided frequencies" << std::endl;
		solve_for_Qhat();
		std::vector<double> col;
		col.resize(k);
		for (int c_iter = 0; c_iter < n; c_iter++) {
 			//VectorXd::Map(&col[0], d) = Qhat.col(c_iter);
			for (int r_iter = 0; r_iter < k; r_iter++) {
				col[r_iter] = Qhat(r_iter, c_iter);
			}

			project_onto_simplex(col);

			for (int r_iter = 0; r_iter < k; r_iter++) {
				Qhat(r_iter, c_iter) = col[r_iter];
			}
		}
		return;
	}

	solve_for_Qhat();
	if (debug) write_matrix(Qhat, std::string("Qhat_0.txt"));

	solve_for_Phat();
	if (debug) write_matrix(Phat, std::string("Phat_1.txt"));

	for (niter = 1; niter < MAX_ITER; niter++) {
		Qhat_old = Qhat;

		solve_for_Qhat();
		if (debug) write_matrix(Qhat, std::string("Qhat_" + std::to_string(niter) + ".txt"));

		std::vector<double> col;
		col.resize(k);
		for (int c_iter = 0; c_iter < n; c_iter++) {
 			//VectorXd::Map(&col[0], d) = Qhat.col(c_iter);
			for (int r_iter = 0; r_iter < k; r_iter++) {
				col[r_iter] = Qhat(r_iter, c_iter);
			}

			project_onto_simplex(col);

			for (int r_iter = 0; r_iter < k; r_iter++) {
				Qhat(r_iter, c_iter) = col[r_iter];
			}
		}
		if (debug) write_matrix(Qhat, std::string("Qhat_" + std::to_string(niter) + "_w_constraints.txt"));

		solve_for_Phat();
		if (debug) write_matrix(Phat, std::string("Phat_" + std::to_string(niter+1) + ".txt"));

		Phat = Phat.unaryExpr(&truncate_with_epsilon);
		if (debug) write_matrix(Phat, std::string("Phat_" + std::to_string(niter+1) + "_w_constraints.txt"));

		diff = Qhat - Qhat_old;
 		rmse = diff.norm() / sqrt(n * k);
		std::cout << "Iteration " << niter+1 << "  -- RMSE " << std::setprecision(15) << rmse << std::endl;
		if ((rmse <= convergence_limit) || std::isnan(rmse)) {
			std::cout << "Breaking after " << niter+1 << " iterations" << std::endl;
			break;
		}
	}
}

void ALStructure::write_matrix(MatrixXdr &mat, const std::string file_name) {
	std::ofstream fp;
	fp.open((command_line_opts.OUTPUT_PATH + file_name).c_str());
	fp << std::setprecision(15) << mat << std::endl;
	fp.close();
}

void ALStructure::write_matrix_maf(MatrixXdr &mat, const std::string file_name) {
        std::ofstream fp;
        fp.open((command_line_opts.OUTPUT_PATH + file_name).c_str());
        fp << std::setprecision(15) << 1 - mat.array() << std::endl;
        fp.close();
}

void ALStructure::write_vector(Eigen::VectorXd &vec, const std::string file_name) {
	std::ofstream fp;
	fp.open((command_line_opts.OUTPUT_PATH + file_name).c_str());
	fp << std::setprecision(15) << vec << std::endl;
	fp.close();
}

int ALStructure::run() {

	total_begin = clock();

	memory_efficient = command_line_opts.memory_efficient;
	text_version = command_line_opts.text_version;
	fhat_version = command_line_opts.fhat_version;
	fhattrunc_version = command_line_opts.fhattrunc_version;
	fast_mode = command_line_opts.fast_mode;
	missing = command_line_opts.missing;
	MAX_ITER =  command_line_opts.max_iterations;
	convergence_limit = command_line_opts.convergence_limit;
	debug = command_line_opts.debugmode;
	nthreads = command_line_opts.nthreads;
	output_path = std::string(command_line_opts.OUTPUT_PATH);
	seed = command_line_opts.seed;

	auto start = std::chrono::system_clock::now();

	clock_t io_begin = clock();
	clock_gettime(CLOCK_REALTIME, &t0);

	// Read genotype matrix X
	if (text_version) {
		if (fast_mode) {
			g.read_txt_mailman(command_line_opts.GENOTYPE_FILE_PATH, missing);
		} else {
			g.read_txt_naive(command_line_opts.GENOTYPE_FILE_PATH, missing);
		}
	} else {
		g.read_plink(command_line_opts.GENOTYPE_FILE_PATH, missing, fast_mode);
	}

	p = g.Nsnp;
	n = g.Nindv;
	k = command_line_opts.num_of_evec;

	// TODO: Implement these codes.
	if (missing) {
		std::cout << "Missing version not yet implemented!" << std::endl;
		exit(-1);
	}

	if (fast_mode && memory_efficient) {
		std::cout << "Memory effecient version for mailman EM not yet implemented" << std::endl;
		std::cout << "Ignoring Memory effecient Flag" << std::endl;
	}

	if (!fast_mode && !memory_efficient) {
		geno_matrix.resize(p, n);
		g.generate_eigen_geno(geno_matrix, false, false);
		if (debug) write_matrix(geno_matrix, "X.txt");
	}

	clock_t io_end = clock();

	std::cout << "Running on Dataset of " << p << " SNPs and " << n << " Individuals" << std::endl;

	#if SSE_SUPPORT == 1
		if (fast_mode)
			std::cout << "Using Optimized SSE FastMultiply" << std::endl;
	#endif

	clock_t it_begin = clock();

	mm = MatMult(g, geno_matrix, debug, false, memory_efficient, missing, fast_mode, nthreads, k);

	if (std::string(command_line_opts.ROWSPACE_FILE_PATH) != "") {
		std::cout << "Using provided V" << std::endl;

	// Read eigenvectors of the n x n matrix: G = (1/m) * (X^T X - D)
		V = load_tsv<MatrixXdr>(command_line_opts.ROWSPACE_FILE_PATH);
		if (k != V.cols()) {
			k = V.cols();
			std::cout << "Mismatch between column number of provided V and provided k!" << std::endl;
			std::cout << "Changing k to number of columns in V" << std::endl;
		}

		if (V.rows() != n) {
			std::cout << "Dimensions of genotype matrix and rowspace matrix do not agree!" << std::endl;
			exit(-1);
		}
	} else {
		std::cout << "Performing latent subspace estimation" << std::endl;

		// Calculate D matrix
		D.resize(g.Nindv);
		for (int i = 0; i < g.Nindv; ++i) {
			D[i] =  2 * g.rowsum[i] - g.rowsqsum[i];
		}
		if (debug) write_vector(D, "D.txt");

		// Calculate V
		Spectra::SymEigsSolver<double, Spectra::LARGEST_ALGE, ALStructure> eigs(this, k, k * 2 + 1);
		eigs.init();
		eigs.compute(MAX_ITER, convergence_limit);

		if (eigs.info() == Spectra::SUCCESSFUL) {
			V = eigs.eigenvectors();
			write_matrix(V, "V.txt");
			if (debug) {
				Eigen::VectorXd evals = eigs.eigenvalues().array() / (n-1);
				write_vector(evals, "evals.txt");
			}
			std::cout << "Latent subspace esimation completed after " << nops << " iterations" << std::endl;
		}
		else {
			throw new std::runtime_error(
				std::string("Spectra eigendecomposition unsucessful") + ", status" + std::to_string(eigs.info())
			);
		}
	}

	// Compute Fhat = (1/2) * (X V V^T)
	if (fhat_version || fhattrunc_version) {
		std::cout << "Explicitly computing Fhat";
		MatrixXdr temp_pxk(p, k);
		mm.multiply_y_pre(V, k, temp_pxk, false);

		// if (debug) write_matrix(temp_pxk, "XV.txt");

		Fhat = temp_pxk * V.transpose();
		Fhat = Fhat.unaryExpr(&divide_by_two);
		if (debug) write_matrix(Fhat, "Fhat.txt");

		if (command_line_opts.fhattrunc_version) {
			std::cout << " -- truncated";
			Fhat = Fhat.unaryExpr(&truncate_xxx);
			if (debug) write_matrix(Fhat, "Fhat_truncated.txt");
		}
		std::cout << std::endl;
	}

	// Create pseudo random number generator (PRNG) engine
	std::default_random_engine prng_eng{};

	if (std::string(command_line_opts.INITIAL_FILE_PATH) != "") {
		std::cout << "Using initial Phat provided" << std::endl;
		Phat = load_tsv<MatrixXdr>(command_line_opts.INITIAL_FILE_PATH);
	}
	else if (std::string(command_line_opts.FREQ_FILE_PATH) != ""){
		Phat = read_plink_freq_file<MatrixXdr>(command_line_opts.FREQ_FILE_PATH,k);
		MAX_ITER = 1;
	}
	else {
		initialize(prng_eng);
	}

	if (std::string(command_line_opts.FREQ_FILE_PATH) != ""){
		truncated_alternating_least_squares(true);
	}
	else {
		truncated_alternating_least_squares();

		// Try restarting with new seed!
		if (std::isnan(rmse) && (std::string(command_line_opts.INITIAL_FILE_PATH) == "")) {
			command_line_opts.given_seed = false;
			for (int xx = 0; xx < 5; xx++) {
				initialize(prng_eng);
				truncated_alternating_least_squares();
				if (!std::isnan(rmse)) {
					break;
				}
			}
		}
	}

	clock_t it_end = clock();

	write_matrix_maf(Phat, "Phat.txt");
	write_matrix(Qhat, "Qhat.txt");

	mm.clean_up();

	clock_t total_end = clock();
	double io_time = static_cast<double>(io_end - io_begin) / CLOCKS_PER_SEC;
	//double avg_it_time = static_cast<double>(it_end - it_begin) / (MAX_ITER * 1.0 * CLOCKS_PER_SEC);
	double total_time = static_cast<double>(total_end - total_begin) / CLOCKS_PER_SEC;
	std::cout << "Completed!" << std::endl;
	std::cout << "IO Time:  " << io_time << std::endl;
	std::cout << "Total runtime:   " << total_time << std::endl;

	std::chrono::duration<double> wctduration = std::chrono::system_clock::now() - start;
	std::cout << "Wall clock time = " <<  wctduration.count() << std::endl;

	return 0;
}

unsigned int ALStructure::cols() {
	return n;
}

unsigned int ALStructure::rows() {
	return n;
}

void ALStructure::perform_op(const double* x_in, double* y_out) {
	// Performs ((Xv)^T X)^T - Dv
	MatrixXdr x = Eigen::Map<const Eigen::VectorXd> (x_in, n);
	Eigen::Map<Eigen::VectorXd> y(y_out, n);

	MatrixXdr temp_px1(p,1);
	mm.multiply_y_pre(x,1,temp_px1,false);
	temp_px1.transposeInPlace(); // 1xp

	MatrixXdr temp_1xn(1,n);
	mm.multiply_y_post(temp_px1,1,temp_1xn,false);

	y.noalias() = temp_1xn.transpose() - D.cwiseProduct(x);
	nops++;
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * 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.
 */

#include <cmath>

#include <string>
#include <iostream>
#include <random>

#include <boost/math/constants/constants.hpp>

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/mathtools/solveclyap.h>
#include <tomographer/tools/eigenutil.h> // denseRandom
#include <tomographer/tools/loggers.h>
#include <tomographer/mathtools/random_unitary.h>

#include <tomographer/tools/boost_test_logger.h>

// -----------------------------------------------------------------------------
// fixture(s)

// template<typename MatrixType_, typename Rng_>
// class RandomPosSemiDef
// {
// public:
//   typedef MatrixType_ MatrixType;
//   typedef typename MatrixType::Scalar Scalar;
//   typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
//   typedef typename MatrixType::Index IndexType;
//   typedef Eigen::Matrix<RealScalar, Eigen::Dynamic, 1> RealVectorType;

//   typedef Rng_ Rng;

// private:
//   Rng & _rng;
//   IndexType _dim;

//   MatrixType _U;
//   RealVectorType _diag;

// public:
//   RandomPosSemiDef(Rng & rng, IndexType dim)
//     : _rng(rng), _dim(dim), _U(dim, dim), _diag(dim)
//   {
//   }

//   MatrixType withUnifEig(RealScalar max_eigenval = RealScalar(1),
// 			 IndexType max_rank = std::numeric_limits<IndexType>::max())
//   {
//     std::uniform_real_distribution<RealScalar> dist(0, max_eigenval);

//     Tomographer::MathTools::randomUnitary<MatrixType>(_U, _rng);

//     _diag = Tomographer::Tools::denseRandom<Eigen::VectorXd>(_rng, dist, _dim);

//     for (int i = max_rank; i < _dim; ++i) {
//       _diag(i) = 0;
//     }

//     // this is positive semidefinite.
//     return _U*_diag.asDiagonal()*_U.adjoint();
//   }

//   inline const MatrixType & lastU() const { return _U; }
//   inline const RealVectorType & lastDiag() const { return _diag; }

// };

struct test_solveclyap_fixture
{
  template<typename Rng>
  void do_test(Rng & rng, int d, int A_rank)
  {
    typedef Eigen::MatrixXcd MatType;

    std::uniform_real_distribution<double> dist(0.0, 1.0);

    MatType U(d,d);
    Tomographer::MathTools::randomUnitary<MatType>(U, rng);
    MatType W(U.block(0,0,d,A_rank));

    Eigen::VectorXd eigvals(Tomographer::Tools::denseRandom<Eigen::VectorXd>(rng, dist, A_rank));
    // this is positive semidefinite.
    MatType A(W * eigvals.asDiagonal() * W.adjoint());

    BOOST_MESSAGE("A = " << A) ;

    // create a random X in the support of A
    // MatType X(W * RandomPosSemiDef<MatType, std::mt19937>(rng, A_rank).withUnifEig(1.0, A_rank) * W.adjoint());
    MatType X(W * Tomographer::Tools::denseRandom<MatType>(rng, dist, A_rank, A_rank) * W.adjoint());

    BOOST_MESSAGE("X = " << X) ;

    const MatType C(A.adjoint()*X + X*A);

    BOOST_MESSAGE("--> C = " << C) ;

    MatType X2(d,d);

    Tomographer::Logger::BoostTestLogger logger(Tomographer::Logger::DEBUG);

    Tomographer::MathTools::SolveCLyap::solve<true>(X2, A, C, logger, 1e-8);

    MY_BOOST_CHECK_EIGEN_EQUAL(X, X2, 1e-8);
  }
};

// -----------------------------------------------------------------------------
// test suites

BOOST_FIXTURE_TEST_SUITE(test_mathtools_solveclyap, test_solveclyap_fixture)

BOOST_AUTO_TEST_CASE(random_test_7_4)
{
  std::mt19937 rng(4938221);

  const int d = 7; // dimension of problem
  const int A_rank = 4; // rank of A
  for (int repeat = 0; repeat < 1000; ++repeat) {
    BOOST_MESSAGE("Repeat : iteration #" << repeat) ;
    do_test(rng, d, A_rank);
  }
}

BOOST_AUTO_TEST_CASE(random_test_15_15)
{
  std::mt19937 rng(89120);

  const int d = 15; // dimension of problem
  const int A_rank = 15; // rank of A

  for (int repeat = 0; repeat < 100; ++repeat) {
    BOOST_MESSAGE("Repeat : iteration #" << repeat) ;
    do_test(rng, d, A_rank);
  }
}

BOOST_AUTO_TEST_SUITE_END()


//-----------------------------------------------------------------------------
// Copyright (c) 2016 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _mitrax__Eigen__convert__hpp_INCLUDED_
#define _mitrax__Eigen__convert__hpp_INCLUDED_

#include <Eigen/Core>

#include <mitrax/matrix.hpp>

namespace mitrax{

	template < typename Derived >
	constexpr auto convert(Eigen::MatrixBase< Derived > const& m){
		return make_matrix_fn(dim_pair(m.cols(), m.rows()), [&m](auto x, auto y){
			return m(y, x);
		});
	}

	template < typename M, col_t C, row_t R >
	constexpr auto convert(matrix< M, C, R > const& m){
		Eigen::Matrix<
			value_type_t< M >,
			R == 0 ? Eigen::Dynamic : int(R),
			C == 0 ? Eigen::Dynamic : int(C)
		> res(int(m.rows()), int(m.cols()));

		for(size_t y = 0; y < m.rows(); ++y){
			for(size_t x = 0; x < m.cols(); ++x){
				res.coeffRef(y, x) = m(x, y);
			}
		}

		return res;
	}

}

#endif


#include <iostream>
#include <fstream>
#include <chrono>
#include <string>
#include <Eigen/Cholesky>
#include <enoki/array.h>
#include <enoki/autodiff.h>
#include <enoki/autodiff.cpp>

/* Don't forget to include the 'enoki' namespace */
using namespace enoki;
using namespace std;
using namespace std::chrono;

/* Static float array (the suffix "P" indicates that this is a fixed-size packet) */
using FloatP = Packet<float, 4>;

/* Dynamic float array (vectorized via FloatP, the suffix "X" indicates arbitrary length) */
using FloatX = DynamicArray<FloatP>;

using FloatD = DiffArray<FloatX>;

int main(int argc, char **argv)
{
    int num_params = 90010;
    int num_vars = 1;
    Eigen::VectorXd args(num_params * num_vars);
	FloatX init_k = zero<FloatX>(num_params);

    string output_filename = argv[1];
    ofstream outfile;
    outfile.open(output_filename);

    std::ifstream file("./tests/params.txt");

    int i = 0;
    for (std::string line; std::getline(file, line);)
    {
        args(i) = stod(line.c_str());
        i++;
    }
    file.close();
    for (int i = 0; i < num_params; i++)
    {
		init_k[i] = args[i * num_vars + 0];

    }

	FloatD k(init_k);

	set_requires_gradient(k);

    FloatD function = sin(k) + cos(k) + pow(k, 2); // derivative

    auto start = high_resolution_clock::now();
    backward(function);
	FloatX grad_k = gradient(k);

    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    outfile << (double)duration.count() / 1000000.0 << " ";
    for (int i = 0; i < num_params; i++)
    {
		outfile << grad_k[i] << " ";

    }
    outfile.close();
    return 0;
}


/* Copyright 2017 Ramakrishnan Kannan */

#include <armadillo>
#include "ncpfactors.hpp"
#include "ntf_utils.hpp"
#include "tensor.hpp"
#include "utils.h"

int main(int argc, char* argv[]) {
  int test_order = 5;
  int low_rank = 2;
  UVEC dimensions(test_order);
  FMAT* mttkrps = new FMAT[test_order];
  // UVEC dimensions(4);
  for (int i = 0; i < test_order; i++) {
    dimensions(i) = i + 2;
  }
  // dimensions(3) = 6;
  PLANC::NCPFactors cpfactors(dimensions, low_rank);
  cpfactors.print();
  FMAT krp_2 = cpfactors.krp_leave_out_one(2);
  cout << "krp" << endl << "------" << endl << krp_2;
  PLANC::Tensor my_tensor = cpfactors.rankk_tensor();
  // cout << "input tensor" << endl << "--------" << endl;
  // my_tensor.print();
  for (int i = 0; i < test_order; i++) {
    mttkrps[i] = arma::zeros<FMAT>(dimensions(i), low_rank);
  }
  for (int i = 0; i < test_order; i++) {
    mttkrp(i, my_tensor, cpfactors, &mttkrps[i]);
    cout << "mttkrp " << i << endl << "---------" << endl << mttkrps[i];
  }
}


#pragma once
#include <Eigen/Core>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>

namespace gtsam
{
    class LidarNormalFactor : public NoiseModelFactor2<Pose3, Pose3>
    {
        using X = Pose3;
        using Base = NoiseModelFactor2<Pose3, Pose3>;
        using This = LidarNormalFactor;

    public:
        LidarNormalFactor(Key key1, Key key2, const Point3 &unit1, const Point3 &unit2, const SharedNoiseModel &model)
            : Base(model, key1, key2), u1_(unit1.normalized()), u2_(unit2.normalized())
        {
        }

        virtual ~LidarNormalFactor() {}

        /*
            d(R*u)/dT =  R*[[-u]x 03x3]

            err = R1*u1 x R2*u2
            derr/dT1 =  [R2*u2]x * R1*[[u1]x 03x3]
            derr/dT2 = -[R1*u1]x * R2*[[u2]x 03x3]
        */
        Vector evaluateError(const X &pose1, const X &pose2,
                             boost::optional<Matrix &> H1 = boost::none,
                             boost::optional<Matrix &> H2 = boost::none) const
        {
            const auto &rot_u1 = pose1.rotation().matrix() * u1_;
            const auto &rot_u2 = pose2.rotation().matrix() * u2_;
            if (H1)
                *H1 = skewSymmetric(rot_u2[0], rot_u2[1], rot_u2[2]) * (Matrix36() << pose1.rotation().matrix() * skewSymmetric(u1_[0], u1_[1], u1_[2]), Z_3x3).finished();

            if(H2)
                *H2 = skewSymmetric(rot_u1[0], rot_u1[1], rot_u1[2]) * (Matrix36() << pose2.rotation().matrix() * skewSymmetric(u2_[0], u2_[1], u2_[2]), Z_3x3).finished();

            return rot_u1.cross(rot_u2);
        }

        virtual NonlinearFactor::shared_ptr clone() const
        {
            return boost::static_pointer_cast<NonlinearFactor>(
                NonlinearFactor::shared_ptr(new This(*this)));
        }

        // virtual bool equals(const NonlinearFactor &expected, double tol = 1e-9) const
        // {
        //   const This *e = dynamic_cast<const This *>(&expected);
        //   return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(p1_, e->p1_, tol) &&
        //          traits<Point3>::Equals(p2_, e->p2_, tol) && traits<Point3>::Equals(u_, e->u_, tol);
        // }

        // virtual void print(const std::string &s = "",
        //                    const KeyFormatter &keyFormatter = DefaultKeyFormatter) const
        // {
        //   cout << s << ":\nLidarPlaneFactor1 on (" << keyFormatter(key()) <<  ")\n"
        //        << "  Plane Point: " << p1_.transpose() << "\n"
        //        << "  Plane norm Axis: " << u_.transpose() << "\n"
        //        << "  Match Point: " << p2_.transpose() << "\n";
        //   noiseModel_->print("  noise model: ");
        // }

    private:
        Point3 u1_, u2_;
    }; // class LidarNormalFactor
}