#ifndef RANDOM_UTIL_HPP
#define RANDOM_UTIL_HPP
/**
 * @file random_util.hpp
 *
 * @brief some functions commonly used in MTToolBox.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2011 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <inttypes.h>
#include <stdint.h>
#include <stdexcept>
#include <openssl/sha.h>
#include <tr1/memory>

#include <NTL/GF2X.h>

#define bit_size(tp) (static_cast<int>(sizeof(tp) * 8))

namespace MTToolBox {
    inline static int count_bit(uint16_t x);
    inline static int count_bit(uint32_t x);
    inline static int count_bit(uint64_t x);
    inline static uint32_t reverse_bit(uint32_t x);
    inline static uint64_t reverse_bit(uint64_t x);

    /**
     * calculate the largest number which is 2^m and does not exceed n.
     * @tparam T type of integer
     * @param n number
     * @returns the largest number which is 2^m and does not exceed n.
     */
    template<typename T>
    T floor2p(T n) {
	if (n == 1) {
	    return 1;
	} else {
	    return 2 * floor2p<T>(n / 2);
	}
    }

    /**
     * print polynomial in binary form. The coefficient of the smallest degree
     * is printed first.
     * @param os output-stream
     * @param poly polynomial to be printed
     * @param breakline if true, break line every 32 outputs.
     */
    inline static void print_binary(std::ostream& os,
				    NTL::GF2X& poly,
				    bool breakline = true) {
	using namespace NTL;
	if (deg(poly) < 0) {
	    os << "0deg=-1" << std::endl;
	    return;
	}
	for(int i = 0; i <= deg(poly); i++) {
	    if(rep(coeff(poly, i)) == 1) {
		os << '1';
	    } else {
		os << '0';
	    }
	    if (breakline && ((i % 32) == 31)) {
		os << std::endl;
	    }
	}
	os << "deg=" << deg(poly) << std::endl;
    }

    /**
     * change \b input to the number between \b start and \b end
     * @param input input number
     * @param start start of the range
     * @param end end of the range
     * @return the number r such that \b start <= \b r <= \b end.
     */
    inline static uint32_t get_range(uint32_t input, int start, int end) {
	if (end < start) {
	    printf("get_range:%d, %d\n", start, end);
	    exit(0);
	}
	return input % (end - start + 1) + start;
    }

    /**
     * change \b input to the number between \b start and \b end
     * @param input input number
     * @param start start of the range
     * @param end end of the range
     * @return the number r such that \b start <= \b r <= \b end.
     */
    inline static uint64_t get_range(uint64_t input, int start, int end) {
	if (end < start) {
	    printf("get_range:%u, %u\n", start, end);
	    exit(0);
	}
	return input % (end - start + 1) + start;
    }

    /**
     * change the small F2 table to the fast and redundant table.
     * @tparam T type of table member.
     * @param dist_tbl new redundant table
     * @param src_tbl source table
     * @param size size of \b dist_table
     */
    template<typename T>
    void fill_table(T dist_tbl[], T src_tbl[], int size) {
    	for(int i = 1; i < size; i++) {
	    for(int j = 1, k = 0; j <= i; j <<= 1, k++) {
		if (i & j) {
		    dist_tbl[i] ^= src_tbl[k];
		}
	    }
	}
    }

    /**
     * calculate the SHA1 digest of F2 polynomial. The coefficients of
     * the polynomial are changed to the string of "0" and "1", which
     * starts with the coefficient of the lowest degree, and the SHA1
     * hash of the string is calculated. The result hash is returned
     * by hexadecimal string.
     *
     * @param str output string
     * @param poly F2 polynomial
     */
    inline static void poly_sha1(std::string& str, const NTL::GF2X& poly) {
	using namespace NTL;
	using namespace std;
	SHA_CTX ctx;
	SHA1_Init(&ctx);
	if (deg(poly) < 0) {
	    SHA1_Update(&ctx, "-1", 2);
	}
	for(int i = 0; i <= deg(poly); i++) {
	    if(rep(coeff(poly, i)) == 1) {
		SHA1_Update(&ctx, "1", 1);
	    } else {
		SHA1_Update(&ctx, "0", 1);
	    }
	}
	unsigned char md[SHA_DIGEST_LENGTH];
	SHA1_Final(md, &ctx);
	stringstream ss;
	for (int i = 0; i < SHA_DIGEST_LENGTH; i++) {
	    ss << setfill('0') << setw(2) << hex
	       << static_cast<int>(md[i]);
	}
	ss >> str;
    }

    /**
     * calculate the position of most right 1, or
     * least significant 1. The position of the MSB is
     * 0. returns -1 when \b v is zero.
     * citing from a website http://aggregate.org/MAGIC/#Trailing Zero Count
     *
     * @param x input
     * @return the position of most right 1.
     */
    int calc_1pos(uint16_t x)
    {
	if (x == 0) {
	    return -1;
	}
	int16_t y = (int16_t)x;
	y = count_bit((uint16_t)((y & -y) - 1));
	return 15 - y;
    }

    /**
     * calculate the position of most right 1, or
     * least significant 1. The position of the MSB is
     * 0. returns -1 when \b v is zero.
     * citing from a website http://aggregate.org/MAGIC/#Trailing Zero Count
     *
     * @param x input
     * @return the position of most right 1.
     */
    int calc_1pos(uint32_t x)
    {
	if (x == 0) {
	    return -1;
	}
	int32_t y = (int32_t)x;
	y = count_bit((uint32_t)(y & -y) - 1);
	return 31 - y;
    }

    /**
     * calculate the position of most right 1, or
     * least significant 1. The position of the MSB is
     * 0. returns -1 when \b v is zero.
     * citing from a website http://aggregate.org/MAGIC/#Trailing Zero Count
     *
     * @param x input
     * @return the position of most right 1.
     */
    int calc_1pos(uint64_t x)
    {
	if (x == 0) {
	    return -1;
	}
	int64_t y = (int64_t)x;
	y = count_bit((uint64_t)(y & -y) - 1);
	return 63 - y;
    }

    /**
     * check if \b array is all zero or not.
     * @tparam type of members of \b array
     * @param array checked array
     * @param size size of \b array
     * @return true if all elements of \b array are zero.
     */
    template<typename T>
    bool is_zero_array(T *array, int size) {
	if (array[0] != 0) {
	    return false;
	} else {
	    return (memcmp(array, array + 1, sizeof(T) * (size - 1)) == 0);
	}
    }

    /**
     * calculate the minimal polynomial of the generated sequence.
     * @returns the minimal polynomial
     */
    template<typename G>
    std::tr1::shared_ptr<NTL::GF2X> get_minpoly(G& generator, int length) {
	using namespace std;
	using namespace NTL;
	using namespace std::tr1;

	vec_GF2 vec;
	vec.SetLength(length * 2);
	for (int i = 0; i < length * 2; i++) {
	    vec[i] = generator() & 1;
	}
	shared_ptr<GF2X> minpoly(new GF2X());
	MinPolySeq(*minpoly, vec, length);
	return minpoly;
    }

    /**
     * count the number of 1
     * SIMD within a Register algorithm
     * citing from a website http://aggregate.org/MAGIC/
     */
    inline static int count_bit(uint16_t x) {
        x -= (x >> 1) & UINT16_C(0x5555);
        x = ((x >> 2) & UINT16_C(0x3333)) + (x & UINT16_C(0x3333));
        x = ((x >> 4) + x) & UINT16_C(0x0f0f);
        x += (x >> 8);
        return (int)(x & 0x1f);
    }

    inline static int count_bit(uint32_t x) {
        x -= (x >> 1) & UINT32_C(0x55555555);
        x = ((x >> 2) & UINT32_C(0x33333333)) + (x & UINT32_C(0x33333333));
        x = ((x >> 4) + x) & UINT32_C(0x0f0f0f0f);
        x += (x >> 8);
        x += (x >> 16);
        return (int)(x & 0x3f);
    }

    inline static int count_bit(uint64_t x) {
	x -= (x >> 1) & UINT64_C(0x5555555555555555);
	x = ((x >> 2) & UINT64_C(0x3333333333333333))
	    + (x & UINT64_C(0x3333333333333333));
	x = ((x >> 4) + x) & UINT64_C(0x0f0f0f0f0f0f0f0f);
	x += (x >> 8);
	x += (x >> 16);
	x += (x >> 32);
	return (int)(x & 0x7f);
    }

    inline static uint32_t reverse_bit(uint32_t x)
    {
	uint32_t y = 0x55555555;
	x = (((x >> 1) & y) | ((x & y) << 1));
	y = 0x33333333;
	x = (((x >> 2) & y) | ((x & y) << 2));
	y = 0x0f0f0f0f;
	x = (((x >> 4) & y) | ((x & y) << 4));
	y = 0x00ff00ff;
	x = (((x >> 8) & y) | ((x & y) << 8));
	return((x >> 16) | (x << 16));
    }

    inline static uint64_t reverse_bit(uint64_t x)
    {
	uint64_t y = UINT64_C(0x5555555555555555);
	x = (((x >> 1) & y) | ((x & y) << 1));
	y = UINT64_C(0x3333333333333333);
	x = (((x >> 2) & y) | ((x & y) << 2));
	y = UINT64_C(0x0f0f0f0f0f0f0f0f);
	x = (((x >> 4) & y) | ((x & y) << 4));
	y = UINT64_C(0x00ff00ff00ff00ff);
	x = (((x >> 8) & y) | ((x & y) << 8));
	y = UINT64_C(0x0000ffff0000ffff);
	x = (((x >> 16) & y) | ((x & y) << 16));
	return((x >> 32) | (x << 32));
    }

    /**
     * divide and ceil
     */
    inline static int div_ceil(int x, int y) {
	if (x % y == 0) {
	    return x / y;
	} else {
	    return x / y + 1;
	}
    }

    /**
     * polynomial to string
     */
    inline static void to_str(uint8_t * str, int size, NTL::GF2X& poly) {
	if (deg(poly) >= size * 8) {
	    std::cerr << "str size too small" << std::endl;
	    throw new std::out_of_range("str size too small");
	}
	for (int i = 0; i < size; i++) {
	    str[i] = 0;
	}
	int idx = 0;
	for (int i = 0; i < size; i++) {
	    uint8_t mask = 1;
	    for (int j = 0; j < 8; j++) {
		if (IsOne(coeff(poly, idx++))) {
		    str[i] |= mask;
		}
		mask <<= 1;
	    }
	}
    }
}
#endif


#pragma once

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <array>
#include <boost/assert.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multi_array.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

#include "base/array_buffer.hpp"
#include "base/exceptions.hpp"
#include "base/timer.hpp"
#include "filtered_range.hpp"
#ifndef NOHDF5
#include "base/eigen2hdf.hpp"
#endif
#include "quadrature/qhermitew.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/laguerren_ks.hpp"
#include "spectral/eval_handlers.hpp"
#include "spectral/pl_radial_eval.hpp"

namespace boltzmann {
namespace detail {

/**
 * @brief sort *polar* elements by `k`
 */
struct CMP
{
  template <typename E>
  bool operator()(const E &e1, const E &e2) const
  {
    // typedef typename boost::mpl::at_c<typename E::types_t,0>::type fa_type;
    typedef typename boost::mpl::at_c<typename E::types_t, 1>::type fr_type;

    typename E::Acc::template get<fr_type> fr_accessor;
    // typename E::Acc::template get<fa_type> fa_accessor;

    const auto &idR1 = fr_accessor(e1).get_id();
    const auto &idR2 = fr_accessor(e2).get_id();

    return std::tie(idR1.k, idR1.j) < std::tie(idR2.k, idR2.j);
  }
};

}  // end namespace detail

template <typename PolarBasis, typename HermiteBasis>
class Polar2Hermite
{
 public:
  Polar2Hermite(const PolarBasis &polar_basis, const HermiteBasis &hermite_basis);
#ifndef NOHDF5
  void exportmat(const std::string &fname) const;
#endif

  void to_hermite(std::vector<double> &dst, const std::vector<double> &src) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_hermite(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_hermite_T(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const;

  void to_polar(std::vector<double> &dst, const std::vector<double> &src) const;

  template <typename DERIVED1, typename DERIVED2>
  void to_polar(Eigen::DenseBase<DERIVED1> &dst, const Eigen::DenseBase<DERIVED2> &src) const;

  void to_polar(double *dst, const double *src) const;

  const Eigen::MatrixXd &get_mat(int k) const;

 private:
  /// Transformation matrices for each polynomial degree k
  std::vector<Eigen::MatrixXd> tmatrices_;
  std::vector<unsigned int> offsets_;
  /// Permutation matrix for polar degrees of freedom
  Eigen::SparseMatrix<double> P_;

  Eigen::VectorXd polar_mass_m_;
  thread_local static ::ArrayBuffer<> buf_;
};

template <typename PolarBasis, typename HermiteBasis>
thread_local ::ArrayBuffer<> Polar2Hermite<PolarBasis, HermiteBasis>::buf_;

template <typename PolarBasis, typename HermiteBasis>
Polar2Hermite<PolarBasis, HermiteBasis>::Polar2Hermite(const PolarBasis &polar_basis,
                                                       const HermiteBasis &hermite_basis)
{
  BOOST_VERIFY(polar_basis.n_dofs() == hermite_basis.n_dofs());
  int K = spectral::get_max_k(polar_basis);
  const unsigned int qpts1d = std::max(40, 2 * (K + 1));
  // initialize quadrature
  QHermiteW quad(1.0, qpts1d);

  // typedef std::array<double, 2> point_t;
  typedef boost::multi_array<double, 2> array2d;
  // typedef boost::multi_array<point_t, 2> array2dpts;
  array2d W(boost::extents[qpts1d][qpts1d]);
  // quad. nodes in cartesian coordinates
  array2d X(boost::extents[qpts1d][qpts1d]);
  array2d Y(boost::extents[qpts1d][qpts1d]);
  array2d R(boost::extents[qpts1d][qpts1d]);
  array2d PHI(boost::extents[qpts1d][qpts1d]);

  for (unsigned int i = 0; i < qpts1d; ++i) {
    const double x = quad.pts(i);
    for (unsigned int j = 0; j < qpts1d; ++j) {
      const double y = quad.pts(j);
      X[i][j] = x;
      Y[i][j] = y;
      W[i][j] = quad.wts(i) * quad.wts(j);
      PHI[i][j] = std::atan2(y, x);
      R[i][j] = std::sqrt(y * y + x * x);
    }
  }
  const unsigned int nq = qpts1d * qpts1d;

  //  ...
  PolarBasis basis_k(polar_basis);
  basis_k.sort(detail::CMP());

  tmatrices_.resize(K + 1);
  P_.resize(polar_basis.n_dofs(), polar_basis.n_dofs());

  // evaluate basis functions at quadrature points
  RDTSCTimer timer;
  timer.start();
  LaguerreNKS<double> L(K);
  unsigned int Rlen = R.shape()[0] * R.shape()[1];
  L.compute(R.data(), Rlen, 0.5);

  typedef typename PolarBasis::elem_t polar_elem_t;
  typedef typename boost::mpl::at_c<typename polar_elem_t::types_t, 1>::type radial_elem_t;
  typedef typename boost::mpl::at_c<typename polar_elem_t::types_t, 0>::type ang_elem_t;
  typename polar_elem_t::Acc::template get<radial_elem_t> get_rad;
  typename polar_elem_t::Acc::template get<ang_elem_t> get_ang;

  typedef typename HermiteBasis::elem_t herm_elem_t;
  typedef typename boost::mpl::at_c<typename herm_elem_t::types_t, 0>::type hx_t;
  typedef typename boost::mpl::at_c<typename herm_elem_t::types_t, 1>::type hy_t;

  typename herm_elem_t::Acc::template get<hx_t> get_hx;
  typename herm_elem_t::Acc::template get<hy_t> get_hy;

  // HermiteEvalHandler2d<HermiteNW<double>, herm_elem_t> heval(H);
  auto heval = hermite_evaluator2d<>::make(hermite_basis, quad.pts());
  PLRadialEval<LaguerreNKS<double>, polar_elem_t> leval(L);
  // permutation matrix entries
  std::vector<unsigned int> p_loc(polar_basis.n_dofs());
  // update offset vector
  unsigned int offset = 0;
  offsets_.resize(K + 2);
  // ----------------------------------------------------
  // loop over block-diagonal matrix and compute offsets
  // ----------------------------------------------------
  offsets_[0] = 0;
  for (int k = 0; k <= K; ++k) {
    std::function<bool(const polar_elem_t &)> pred = [&](const polar_elem_t &e) {
      return (get_rad(e).get_id().k == k);
    };
    auto range = filtered_range(basis_k.begin(), basis_k.end(), pred);

    // collect polar elements of polynomial degree k
    std::vector<polar_elem_t> polar_elems;
    for (auto it = std::get<0>(range); it != std::get<1>(range); ++it) {
      polar_elems.push_back(*it);
    }
    offset += polar_elems.size();
    offsets_[k + 1] = offset;
  }

  // --------------------------------------
  // Compute transformation matrix entries
  // --------------------------------------
  timer.start();
#pragma omp parallel for
  // (* load-balancing will be rather poor *)
  for (int k = 0; k <= K; ++k) {
    std::function<bool(const polar_elem_t &)> pred = [&](const polar_elem_t &e) {
      return (get_rad(e).get_id().k == k);
    };
    auto range = filtered_range(basis_k.begin(), basis_k.end(), pred);

    // collect polar elements of polynomial degree k
    std::vector<polar_elem_t> polar_elems;
    for (auto it = std::get<0>(range); it != std::get<1>(range); ++it) {
      polar_elems.push_back(*it);
    }

    // collect hermite elements of polynomial degree k
    std::vector<herm_elem_t> herm_elems;
    std::function<bool(const herm_elem_t &)> pred2 = [&](const herm_elem_t &e) {
      return (get_hx(e).get_id().k + get_hy(e).get_id().k == k);
    };
    auto range_herm = filtered_range(hermite_basis.begin(), hermite_basis.end(), pred2);
    for (auto it = std::get<0>(range_herm); it != std::get<1>(range_herm); ++it) {
      herm_elems.push_back(*it);
    }

    assert(herm_elems.size() == polar_elems.size());

    // init matrix
    auto &Mk = tmatrices_[k];
    unsigned int nk = herm_elems.size();
    Mk.resize(nk, nk);

    // quadrature nodes and weights
    const double *w = W.origin();
    // const double* x = X.origin();
    // const double* y = Y.origin();
    // const double* r = R.origin();
    const double *phi = PHI.origin();

    for (auto itp = polar_elems.begin(); itp != polar_elems.end(); ++itp) {
      // create permutation matrix
      unsigned int dofx = polar_basis.get_dof_index(itp->get_id());
      unsigned int dofy = basis_k.get_dof_index(itp->get_id());
      p_loc[dofy] = dofx;  // permutation
      //      const unsigned int j = get_rad(*itp).get_id().j;
      const unsigned int k = get_rad(*itp).get_id().k;

      for (auto ith = herm_elems.begin(); ith != herm_elems.end(); ++ith) {
        unsigned int dofy2 = hermite_basis.get_dof_index(ith->get_id());  // index
        // quadrature
        double val = 0;
        for (unsigned int q = 0; q < nq; ++q) {
          // const double rloc = r[q];
          val += (w[q] * leval(*itp, q) * get_ang(*itp).evaluate(phi[q]) *
                  heval(*ith, q / qpts1d, q % qpts1d));
        }
        Mk(dofy2 - offsets_[k], dofy - offsets_[k]) = val;
      }
    }
  }

  for (unsigned int dofy = 0; dofy < p_loc.size(); ++dofy) {
    P_.insert(dofy, p_loc[dofy]) = 1;
  }
  P_.makeCompressed();

  // prepare diagonal entries of mass matrix
  const double PI = boost::math::constants::pi<double>();
  // initialize polar mass m
  const unsigned int N = basis_k.n_dofs();
  polar_mass_m_ = Eigen::VectorXd(N);
  for (unsigned int i = 0; i < N; ++i) {
    const auto &elem = basis_k.get_elem(i);
    if (get_ang(elem).get_id().l == 0)
      polar_mass_m_[i] = PI;
    else
      polar_mass_m_[i] = PI / 2;
  }
}

// ---------------------------------------------------------------------
template <typename PolarBasis, typename HermiteBasis>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_hermite(std::vector<double> &dst,
                                                    const std::vector<double> &src) const
{
  assert(dst.size() == src.size());
  assert(src.size() == (unsigned int)P_.cols());

  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> cvec_t;

  cvec_t vsrc(src.data(), src.size());
  vec_t vdst(dst.data(), dst.size());

  this->to_hermite(vdst, vsrc);
}

// ----------------------------------------------------------------------
template <typename PolarBasis, typename HermiteBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_hermite(Eigen::DenseBase<DERIVED1> &dst,
                                                    const Eigen::DenseBase<DERIVED2> &src) const
{
  const int K = tmatrices_.size();
  const int N = offsets_[K];

  BOOST_ASSERT(dst.size() == N);
  BOOST_ASSERT(src.size() == N);
  auto vpsrc = buf_.get<Eigen::VectorXd>(N);

  // TODO, this is slow
  vpsrc = P_ * src.derived();

  for (int k = 0; k < tmatrices_.size(); ++k) {
    const unsigned int blocksize = offsets_[k + 1] - offsets_[k];
    dst.segment(offsets_[k], blocksize) = tmatrices_[k] * vpsrc.segment(offsets_[k], blocksize);
  }
}

template <typename PolarBasis, typename HermiteBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_hermite_T(Eigen::DenseBase<DERIVED1> &dst,
                                                      const Eigen::DenseBase<DERIVED2> &src) const
{
  assert(dst.size() == src.size());
  assert(src.size() == (unsigned int)P_.cols());
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> cvec_t;

  const int K = tmatrices_.size();
  const int N = offsets_[K];

  auto vpsrc = buf_.get<Eigen::VectorXd>(N);

  for (int k = 0; k < tmatrices_.size(); ++k) {
    const unsigned int blocksize = offsets_[k + 1] - offsets_[k];
    vpsrc.segment(offsets_[k], blocksize) =
        tmatrices_[k].transpose() * src.segment(offsets_[k], blocksize);
  }

  dst = P_.transpose() * vpsrc;
}

template <typename PolarBasis, typename HermiteBasis>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_polar(std::vector<double> &dst,
                                                  const std::vector<double> &src) const
{
  assert(dst.size() == src.size());
  assert(src.size() == (unsigned int)P_.cols());
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> cvec_t;
  vec_t vdst(dst.data(), dst.size());
  cvec_t vsrc(src.data(), src.size());

  this->to_polar(vdst, vsrc);
}

template <typename PolarBasis, typename HermiteBasis>
template <typename DERIVED1, typename DERIVED2>
void
Polar2Hermite<PolarBasis, HermiteBasis>::to_polar(Eigen::DenseBase<DERIVED1> &dst,
                                                  const Eigen::DenseBase<DERIVED2> &src) const
{
  const int K = tmatrices_.size();
  const int N = offsets_[K];

  BOOST_ASSERT(dst.size() == N);
  BOOST_ASSERT(src.size() == N);

  // TODO: use buffer memory instead
  auto vpsrc = buf_.get<Eigen::VectorXd>(N);
  for (int k = 0; k < tmatrices_.size(); ++k) {
    const unsigned int blocksize = offsets_[k + 1] - offsets_[k];
    vpsrc.segment(offsets_[k], blocksize) =
        tmatrices_[k].transpose() * src.segment(offsets_[k], blocksize);
  }
  vpsrc = vpsrc.array() / polar_mass_m_.array();

  dst = P_.transpose() * vpsrc;
}

template <typename PolarBasis, typename HermiteBasis>
const Eigen::MatrixXd &
Polar2Hermite<PolarBasis, HermiteBasis>::get_mat(int k) const
{
  BOOST_ASSERT(k < tmatrices_.size());
  return tmatrices_[k];
}

#ifndef NOHDF5
template <typename PolarBasis, typename HermiteBasis>
void
Polar2Hermite<PolarBasis, HermiteBasis>::exportmat(const std::string &fname) const
{
  hid_t h5f = H5Fcreate(fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  eigen2hdf::save_sparse(h5f, "P", P_);
  for (unsigned int k = 0; k < tmatrices_.size(); ++k) {
    eigen2hdf::save(h5f, std::to_string(k), tmatrices_[k]);
  }
  H5Fclose(h5f);
}
#endif

}  // end namespace boltzmann


//==================================================================================================
/*!
  @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 <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


/*
 * 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) 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;
}


//#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++;
}


#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;
}


#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
}