#ifndef HMM_HPP
#define HMM_HPP

#include <Eigen/Dense>

using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using MatrixType = Eigen::Ref<const Eigen::MatrixXd>;
using VectorType = Eigen::Ref<const Eigen::VectorXd>;

namespace hmm {

  class HMM {

  public:

    HMM() = default;

    /**
     * @brief Construct an HMM object
     * given the sizes of hidden and observed states.
     *
     * Matrices of probabilities are initialized with the inverse of the
     * matrix size so that all states and emissions have equal probability.
     *
     * @param num_hidden Number of hidden states.
     * @param num_observed Number of observed states.
     */
    HMM(std::size_t num_hidden, std::size_t num_observed) noexcept;

    /**
     * @brief Construct an HMM object with given probability matrices.
     *
     * Invokes copy constructors for the Eigen data types.
     *
     * @param transition_matrix Matrix of transition probabilities of
     * dimensions [num_hidden, num_hidden].
     *
     * @param emission_matrix Matrix of emission probabilities of
     * dimensions [num_hidden, num_observed].
     *
     * @param initial_probs Vector of initial probabilities.
     */
    HMM(const MatrixType& transition_probs,
        const MatrixType& emission_probs,
        const VectorType& initial_probs);

    HMM(const MatrixType& transition_probs,
        const MatrixType& emission_probs);

    /**
     * @brief Calculate forward probabilities for a sequence of observed states.
     *
     * @param observations Vector of observed states as `int` values.
     * @returns Matrix of forward probabilities.
     */
    Matrix forward(const VectorType& observations) noexcept;

    Matrix backward(const VectorType&) noexcept;

    /**
     * Calculate the Viterbi path for a sequence of observed states.
     *
     * @param Vector of observed states as `int` values.
     *
     * @return Vector of the most likely sequence of hidden states
     * that gave rise to the sequence of observed states provided as argument.
     */
    Vector viterbi(const VectorType&) noexcept;

    // getters
    Matrix transition_matrix() const noexcept;
    Matrix emission_matrix() const noexcept;

    // setters
    void transition_matrix(const MatrixType&);
    void emission_matrix(const MatrixType&);

  private:
    std::size_t m_num_hidden = 1;
    std::size_t m_num_observed = 1;

    Matrix m_transition_probs =
      Matrix::Constant(m_num_hidden,
                       m_num_hidden,
                       1.0 / m_num_hidden);

    Matrix m_emission_probs =
      Matrix::Constant(m_num_hidden,
                       m_num_observed,
                       1.0 / m_num_observed);

    Vector m_initial_probs =
      Vector::Constant(m_num_hidden,
                       1.0 / m_num_observed);
  };

} /* class HMM */

#endif /* HMM_HPP */


/**********************************************************************
*  Copyright (c) 2008-2013, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include <utilities/geometry/Geometry.hpp>
#include <utilities/geometry/Transformation.hpp>

#include <utilities/core/Assert.hpp>

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

namespace openstudio{
  /// convert degrees to radians
  double degToRad(double degrees)
  {
    return degrees*boost::math::constants::pi<double>()/180.0;
  }

  /// convert radians to degrees
  double radToDeg(double radians)
  {
    return radians*180.0/boost::math::constants::pi<double>();
  }

  /// compute area from surface as Point3dVector
  boost::optional<double> getArea(const Point3dVector& points)
  {
    boost::optional<double> result;
    OptionalVector3d newall = getNewallVector(points);
    if (newall){
      result = newall->length() / 2.0;
    }
    return result;
  }

  // compute Newall vector from Point3dVector, direction is same as outward normal
  // magnitude is twice the area
  OptionalVector3d getNewallVector(const Point3dVector& points)
  {
    OptionalVector3d result;
    unsigned N = points.size();
    if (N >= 3){
      Vector3d vec;
      for (unsigned i = 1; i < N-1; ++i){
        Vector3d v1 = points[i] - points[0];
        Vector3d v2 = points[i+1] - points[0];
        vec += v1.cross(v2);
      }
     result = vec;
   }
   return result;
  }

  // compute outward normal from Point3dVector
  OptionalVector3d getOutwardNormal(const Point3dVector& points)
  {
    OptionalVector3d result = getNewallVector(points);
    if (result){
      if (!result->normalize()){
        result.reset();
      }
    }
    return result;
  }

  /// compute centroid from surface as Point3dVector
  OptionalPoint3d getCentroid(const Point3dVector& points)
  {
    OptionalPoint3d result;

    if (points.size() >= 3){
      // convert to face coordinates
      Transformation alignFace = Transformation::alignFace(points);
      Point3dVector surfacePoints = alignFace.inverse()*points;

      unsigned N = surfacePoints.size();
      double A = 0;
      double cx = 0;
      double cy = 0;
      for (unsigned i = 0; i < N; ++i){
        double x1, x2, y1, y2;
        if (i == N-1){
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[0].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[0].y();
        }else{
          x1 = surfacePoints[i].x();
          x2 = surfacePoints[i+1].x();
          y1 = surfacePoints[i].y();
          y2 = surfacePoints[i+1].y();
        }

        double dA = (x1*y2-x2*y1);
        A += 0.5*dA;
        cx += (x1+x2)*dA;
        cy += (y1+y2)*dA;
      }

      if (A > 0){
        // centroid in face coordinates
        Point3d surfaceCentroid(cx/(6.0*A), cy/(6.0*A), 0.0);

        // centroid
        result = alignFace*surfaceCentroid;
      }
    }
    return result;
  }

  /// reorder points to upper-left-corner convention
  Point3dVector reorderULC(const Point3dVector& points)
  {
    unsigned N = points.size();
    if (N < 3){
      return Point3dVector();
    }

    // transformation to align face
    Transformation t = Transformation::alignFace(points);
    Point3dVector facePoints = t.inverse()*points;

    // find ulc index in face coordinates
    double maxY = std::numeric_limits<double>::min();
    double minX = std::numeric_limits<double>::max();
    unsigned ulcIndex = 0;
    for(unsigned i = 0; i < N; ++i){
      OS_ASSERT(std::abs(facePoints[i].z()) < 0.001);
      if ((maxY < facePoints[i].y()) || ((maxY < facePoints[i].y() + 0.00001) && (minX > facePoints[i].x()))){
        ulcIndex = i;
        maxY = facePoints[i].y();
        minX = facePoints[i].x();
      }
    }

    // no-op
    if (ulcIndex == 0){
      return points;
    }

    // create result
    Point3dVector result;
    std::copy (points.begin() + ulcIndex, points.end(), std::back_inserter(result));
    std::copy (points.begin(), points.begin() + ulcIndex, std::back_inserter(result));
    OS_ASSERT(result.size() == N);
    return result;
  }

  std::vector<Point3d> removeColinear(const Point3dVector& points, double tol)
  {
    unsigned N = points.size();
    if (N < 3){
      return points;
    }

    std::vector<Point3d> result;
    Point3d lastPoint = points[0];
    result.push_back(lastPoint);

    for (unsigned i = 1; i < N; ++i){
      Point3d currentPoint = points[i];
      Point3d nextPoint = points[0];
      if (i < N-1){
        nextPoint = points[i+1];
      }

      Vector3d a = (currentPoint - lastPoint);
      Vector3d b = (nextPoint - currentPoint);

      // if these fail to normalize we have zero length vectors (e.g. adjacent points)
      if (a.normalize()){
        if (b.normalize()){

          Vector3d c = a.cross(b);
          if (c.length() >= tol){
            // cross product is significant
            result.push_back(currentPoint);
            lastPoint = currentPoint;
          }else{
            // see if dot product is near -1
            double d = a.dot(b);
            if (d <= -1.0 + tol){
              // this is a line reversal
              result.push_back(currentPoint);
              lastPoint = currentPoint;
            }
          }
        }
      }
    }

    return result;
  }

  double getDistance(const Point3d& point1, const Point3d& point2) {
    double dx = point1.x() - point2.x();
    double dy = point1.y() - point2.y();
    double dz = point1.z() - point2.z();
    double result = std::sqrt(dx*dx + dy*dy + dz*dz);
    return result;
  }

  double getAngle(const Vector3d& vector1, const Vector3d& vector2) {
    Vector3d working1(vector1);
    working1.normalize();
    Vector3d working2(vector2);
    working2.normalize();
    return acos(working1.dot(working2));
  }

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2)
  {

    // for more accuracy would want to use WGS-84 ellipsoid params and Vincenty formula

    // Haversine formula
    double R = 6371000; // Earth radius meters
    double deltaLat = degToRad(lat2-lat1);
    double deltaLon = degToRad(lon2-lon1);
    double a = sin(deltaLat/2) * sin(deltaLat/2) +
               cos(degToRad(lat1)) * cos(degToRad(lat2)) *
               sin(deltaLon/2) * sin(deltaLon/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = R * c;

    return d;
  }

  bool circularEqual(const Point3dVector& points1, const Point3dVector& points2, double tol)
  {
    unsigned N = points1.size();
    if (N != points2.size()){
      return false;
    }

    if (N == 0){
      return true;
    }

    bool result = false;

    // look for a common starting point
    for (unsigned i = 0; i < N; ++i){
      if (getDistance(points1[0], points2[i]) <= tol){

        result = true;

        // check all other points
        for (unsigned j = 0; j < N; ++j){
          if (getDistance(points1[j], points2[(i + j) % N]) > tol){
            result = false;
            break;
          }
        }
      }

      if (result){
        return result;
      }
    }

    return result;
  }

} // openstudio


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file staticallycorrectedyieldtermstructure.hpp
    \brief Statically corrected yield term structure
    \ingroup termstructures
*/

#ifndef quantext_statically_corrected_yts_hpp
#define quantext_statically_corrected_yts_hpp

#include <qle/termstructures/dynamicstype.hpp>

#include <ql/termstructures/yieldtermstructure.hpp>

#include <boost/unordered_map.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Statically Corrected Yield Term Structure
/*! This termstructure takes a floating reference date term structure
    and two fixed reference date term structures, applying a static
    correction to the floating ts implied by the two fixed ones.
    Usually the floating term structure will coincide with
    the first fixed at construction time. Also, the two fixed
    termstructures should have the same reference date and all three
    termstructures should have the same day counter.

    \ingroup termstructures
 */
class StaticallyCorrectedYieldTermStructure : public YieldTermStructure {
public:
    StaticallyCorrectedYieldTermStructure(const Handle<YieldTermStructure>& floatingTermStructure,
                                          const Handle<YieldTermStructure>& fixedSourceTermStructure,
                                          const Handle<YieldTermStructure>& fixedTargetTermStructure,
                                          const YieldCurveRollDown& rollDown = ForwardForward)
        : YieldTermStructure(floatingTermStructure->dayCounter()), x_(floatingTermStructure),
          source_(fixedSourceTermStructure), target_(fixedTargetTermStructure), rollDown_(rollDown) {
        registerWith(floatingTermStructure);
        registerWith(fixedSourceTermStructure);
        registerWith(fixedTargetTermStructure);
    }

    Date maxDate() const { return x_->maxDate(); }
    void update() {}
    const Date& referenceDate() const { return x_->referenceDate(); }

    Calendar calendar() const { return x_->calendar(); }
    Natural settlementDays() const { return x_->settlementDays(); }

    void flushCache() { cache_c_.clear(); }

protected:
    Real discountImpl(Time t) const;

private:
    // FIXME: remove cache
    // cache for source and target forwards
    struct cache_key {
        double t0, t;
        bool operator==(const cache_key& o) const { return (t0 == o.t0) && (t == o.t); }
    };
    struct cache_hasher : std::unary_function<cache_key, std::size_t> {
        std::size_t operator()(cache_key const& x) const {
            std::size_t seed = 0;
            boost::hash_combine(seed, x.t0);
            boost::hash_combine(seed, x.t);
            return seed;
        }
    };
    mutable boost::unordered_map<cache_key, Real, cache_hasher> cache_c_;
    // end cache
    const Handle<YieldTermStructure> x_, source_, target_;
    const YieldCurveRollDown rollDown_;
};

// inline

inline Real StaticallyCorrectedYieldTermStructure::discountImpl(Time t) const {
    Real c = 1.0;
    if (rollDown_ == ForwardForward) {
        Real t0 = source_->timeFromReference(referenceDate());
        // roll down = ForwardForward
        // cache lookup
        cache_key k = { t0, t };
        boost::unordered_map<cache_key, Real>::const_iterator i = cache_c_.find(k);
        if (i == cache_c_.end()) {
            c = source_->discount(t0) / source_->discount(t0 + t) * target_->discount(t0 + t) / target_->discount(t0);
            cache_c_.insert(std::make_pair(k, c));
        } else {
            c = i->second;
        }
    } else {
        // roll down = ConstantDiscount
        // cache lookup
        cache_key k = { 0.0, t };
        boost::unordered_map<cache_key, Real>::const_iterator i = cache_c_.find(k);
        if (i == cache_c_.end()) {
            c = target_->discount(t) / source_->discount(t);
            cache_c_.insert(std::make_pair(k, c));
        } else {
            c = i->second;
        }
    }
    return x_->discount(t) * c;
}

} // namespace QuantExt

#endif


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "Element.hpp"

#include <cmath>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include <boost/math/special_functions/sign.hpp>

#include "utils/Sphere.hpp"

void Element::spherical_polygon(Eigen::Vector3d & t_, Eigen::Vector3d & b_,
    std::vector<double> & theta, std::vector<double> & phi,
    std::vector<double> & phinumb, std::vector<int> & numb) const
{
  Eigen::Vector3d sph_center = sphere_.center;
  double radius = sphere_.radius;
  // Calculate the azimuthal and polar angles for the tessera vertices:
  // we use the normal, tangent and bitangent as a local reference frame
  for (int i = 0; i < nVertices_; ++i) {
    Eigen::Vector3d vertex_normal = vertices_.col(i) - sph_center;
    // The cosine of the polar angle is given as the dot product of the normal at the vertex and the
    // normal at the tessera center: R\cos\theta
    double cos_theta = vertex_normal.dot(normal_) / radius;
    if (cos_theta >=  1.0) cos_theta = 1.0;
    if (cos_theta <= -1.0) cos_theta = -1.0;
    theta[i] = std::acos(cos_theta);
    // The cosine of the azimuthal angle is given as the dot product of the normal at the vertex and the
    // tangent at the tessera center divided by the sine of the polar angle: R\sin\theta\cos\phi
    double cos_phi = vertex_normal.dot(t_) / (radius * std::sin(theta[i]));
    if (cos_phi >=  1.0) cos_phi = 1.0;
    if (cos_phi <= -1.0) cos_phi = -1.0;
    phi[i] = std::acos(cos_phi);
    // The sine of the azimuthal angle is given as the dot product of the normal at the vertex and the
    // bitangent at the tessera center divided by the sine of the polar angle: R\sin\theta\sin\phi
    double sin_phi = vertex_normal.dot(b_) / (radius * std::sin(theta[i]));
    if (sin_phi <= 0.0) phi[i] = 2 * M_PI - phi[i];
  }
  for (int i = 1; i < nVertices_; ++i) {
    phi[i] = phi[i] - phi[0];
    if (phi[i] < 0.0) phi[i] = 2 * M_PI + phi[i];
  }
  // Rewrite tangent as linear combination of original tangent and bitangent
  // then recalculate bitangent so that it's orthogonal to the tangent
  t_ = t_ * std::cos(phi[0]) + b_ * std::sin(phi[0]);
  b_ = normal_.cross(t_);
  // Populate numb and phinumb arrays
  phi[0] = 0.0;
  numb[0] = 0; numb[1] = 1;
  phinumb[0] = phi[0]; phinumb[1] = phi[1];
  for (int i = 2; i < nVertices_; ++i) {// This loop is 2-based
    for (int j = 1; j < i; ++j) {// This loop is 1-based
      if (phi[i] < phinumb[j]) {
        for (int k = 0; k < (i - j); ++k) {
          numb[i - k] = numb[i - k -1];
          phinumb[i - k] = phinumb[i - k -1];
        }
        numb[j] = i;
        phinumb[j] = phi[i];
        goto jump;
      }
    }
    numb[i] = i;
    phinumb[i] = phi[i];
jump:
    ; // Do nothing...
  }
  numb[nVertices_] = numb[0];
  phinumb[nVertices_] = 2 * M_PI;
}

void tangent_and_bitangent(const Eigen::Vector3d & n_,
    Eigen::Vector3d & t_, Eigen::Vector3d & b_)
{
  double rmin = 0.99;
  double n0 = n_(0), n1 = n_(1), n2 = n_(2);
  if (std::abs(n0) <= rmin) {
    rmin = std::abs(n0);
    t_(0) = 0.0;
    t_(1) = - n2 / std::sqrt(1.0 - std::pow(n0, 2));
    t_(2) =   n1 / std::sqrt(1.0 - std::pow(n0, 2));
  }
  if (std::abs(n1) <= rmin) {
    rmin = std::abs(n1);
    t_(0) =   n2 / std::sqrt(1.0 - std::pow(n1, 2));
    t_(1) =   0.0;
    t_(2) = - n0 / std::sqrt(1.0 - std::pow(n1, 2));
  }
  if (std::abs(n2) <= rmin) {
    rmin = std::abs(n2);
    t_(0) =  n1 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(1) = -n0 / std::sqrt(1.0 - std::pow(n2, 2));
    t_(2) =  0.0;
  }
  b_ = n_.cross(t_);
  // Check that the calculated Frenet-Serret frame is left-handed (levogiro)
  // by checking that the determinant of the matrix whose columns are the normal,
  // tangent and bitangent vectors has determinant 1 (the system is orthonormal!)
  Eigen::Matrix3d M;
  M.col(0) = n_;
  M.col(1) = t_;
  M.col(2) = b_;
  if (boost::math::sign(M.determinant()) != 1) {
    PCMSOLVER_ERROR("Frenet-Serret local frame is not left-handed!", BOOST_CURRENT_FUNCTION);
  }
}


#include <iomanip>
#include <boost/timer/timer.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/sum.hpp>
#include <iacaMarks.h>

template< typename FType > FType calcPi();

int main()
{
    boost::timer::cpu_timer timer;

    timer.start();
    double pi = calcPi<double>();
    timer.stop();

    std::cout << "Pi: " << std::setprecision( 20 ) << pi << std::endl;
    std::cout << timer.format();

    return 0;
}

template< typename FType > FType calcPi()
{
    using dpack = boost::simd::pack<FType, 4*boost::simd::pack<FType>::static_size>;

    dpack div;
    dpack one;
    dpack sum = dpack( 0 );
    dpack step = dpack( 2 * dpack::static_size );

    FType val = -1;
    FType fac = 3;
    for( size_t i = 0; i < dpack::static_size; ++i )
    {
        one[ i ] = val;
        div[ i ] = fac;
        val *= -1;
        fac += 2;
    }

        IACA_START
    for( size_t i = 0; i < 1024ull*1024ull*1024ull*16ull / dpack::static_size ; ++i )
    {
        sum += one / div;
        div += step;
    }
        IACA_END

    return 4 * (1 + boost::simd::sum( sum ) );
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef std::complex<double> complex;
  typedef ublas::vector<complex> vector;
  typedef ublas::vector<double> d_vector;
  typedef ublas::banded_matrix<complex, ublas::column_major> matrix;
  typedef ublas::matrix<complex, ublas::column_major> dense_matrix;
  typedef typename std::make_signed<vector::size_type>::type size_type;

  rand_normal<complex>::reset();
  size_type n=1024;
  matrix A(n, n, 1, 1);
  for (size_type i=0; i<n; ++i)
    A(i, i)=std::abs(rand_normal<complex>::get())+1;
  for (size_type i=0; i<n-1; ++i) {
    A(i+1, i)=complex(0);
    A(i, i+1)=complex(0);
  }
  for (int k=0; k<n-1; ++k) {
    // generate a random 2x2 unitary matrix
    double phi(rand_uniform<double>::get(0, 1.5707963267948966192));
    double alpha(rand_uniform<double>::get(0, 6.2831853071795864770));
    double psi(rand_uniform<double>::get(0, 6.2831853071795864770));
    double chi(rand_uniform<double>::get(0, 6.2831853071795864770));
    dense_matrix u(2, 2);
    u(0, 0)=complex(std::cos(alpha+psi), std::sin(alpha+psi))*std::cos(phi);
    u(1, 0)=-complex(std::cos(alpha-chi), std::sin(alpha-chi))*std::sin(phi);
    u(0, 1)=complex(std::cos(alpha+chi), std::sin(alpha+chi))*std::sin(phi);
    u(1, 1)=complex(std::cos(alpha-psi), std::sin(alpha-psi))*std::cos(phi);
    dense_matrix a(2, 2);
    a(0, 0)=A(k, k);
    a(1, 0)=A(k+1, k);
    a(0, 1)=A(k, k+1);
     a(1, 1)=A(k+1, k+1);
    a=ublas::prod(ublas::trans(ublas::conj(u)), a);
    a=ublas::prod(a, u);
    A(k, k)=a(0, 0);
    A(k+1, k)=a(1, 0);
    A(k, k+1)=a(0, 1);
    A(k+1, k+1)=a(1, 1);
  }
  vector b(n);
  for (size_type i=0; i<n; ++i)
    b(i)=rand_normal<complex>::get();
  vector x(b);
  d_vector d(n);
  vector e(n-1);
  for (size_type i=0; i<n; ++i)
    d(i)=A(i ,i).real();
  for (size_type i=0; i<n-1; ++i)
    e(i)=A(i+1, i);
  int info=lapack::ptsv(d, e, x); // solve
  if (info==0) {
    // res <- A*x - b
    vector res(b);
    blas::gbmv(complex(1, 0), A, x, complex(-1, 0), res);
    std::cout << "norm of residual : " << blas::nrm2(res) << '\n';
  } else
    if (info>0)
      std::cout << "singular matrix\n";
    else
      std::cout << "illegal arguments\n";
  return EXIT_SUCCESS;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/experimental/xva/gaussian1dscenariogenerator.hpp>
#include <ql/experimental/models/gaussian1dyieldtermstructure.hpp>
#include <ql/processes/forwardmeasureprocess.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

Gaussian1dSingleCurveScenarioGenerator::Gaussian1dSingleCurveScenarioGenerator(
    const boost::shared_ptr<Gaussian1dModel> &model, const unsigned long seed)
    : ScenarioGenerator<YieldTermStructure>(
          model->termStructure()->calendar(),
          model->termStructure()->dayCounter()),
      model_(model) {
    modelTime_ = time();
    modelState_ = model_->stateProcess()->x0();
    mt_ = boost::make_shared<MersenneTwisterUniformRng>(seed);
    icrng_ = boost::make_shared<InverseCumulativeRng<MersenneTwisterUniformRng,
                                                     InverseCumulativeNormal> >(
        *mt_);
}

bool Gaussian1dSingleCurveScenarioGenerator::nextPath() {
    ScenarioGenerator<YieldTermStructure>::nextPath();
    modelTime_ = time();
    return true;
}

const Date
Gaussian1dSingleCurveScenarioGenerator::advance(const Period &suggestedStep) {
    ScenarioGenerator<YieldTermStructure>::advance(suggestedStep);
    Real newModelTime_ = time();
    boost::shared_ptr<ForwardMeasureProcess> fmp =
        boost::dynamic_pointer_cast<ForwardMeasureProcess>(
            model_->stateProcess());
    if (fmp != NULL)
        if (newModelTime_ > fmp->getForwardMeasureTime())
            validHorizonDate_ = false;
    if (validHorizonDate_) {
        Real dt = newModelTime_ - modelTime_;
        Real dw = std::sqrt(dt) * icrng_->next().value;
        modelState_ =
            model_->stateProcess()->evolve(modelTime_, modelState_, dt, dw);
    }
    modelTime_ = newModelTime_;
    return horizonDate();
}

const boost::shared_ptr<YieldTermStructure>
Gaussian1dSingleCurveScenarioGenerator::state() const {
    return boost::make_shared<Gaussian1dYieldTermStructure>(
        model_, modelTime_, model_->y(modelState_, modelTime_));
}
} // namespace QuantLib


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      An example of a heritage code which uses such a mesh is found in:
 *          The Mark IV Supersonic-Hypersonic Arbitrary Body Program, Volume
 *          II-Program Formulation, Douglas Aircraft Company, AFFDL-TR-73-159,
 *          Volume II.
 *
 *    Notes
 *      The numberOfLines_ and numberOfPoints_ member variables denote the number of mesh points.
 *      The number of panels in the mesh will be numberOfLines_ - 1 by numberOfPoints_ - 1.
 *
 */

#include <boost/multi_array.hpp>
#include <iostream>
#include <limits>
#include <Eigen/Geometry>

#include "Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.h"

namespace tudat
{
namespace geometric_shapes
{

//! Calculate panel characteristics.
void QuadrilateralMeshedSurfaceGeometry::performPanelCalculations( )
{
    // Allocate memory for panel properties.
    panelCentroids_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelSurfaceNormals_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelAreas_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);

    // Declare local variables for normal and area determination.
    Eigen::Vector3d crossVector1;
    Eigen::Vector3d crossVector2;

    // Reset total area.
    totalArea_ = 0.0;

    // Loop over all panels to determine properties.
    for ( int i = 0; i < numberOfLines_ - 1; i++ )
    {
        for ( int j = 0; j < numberOfPoints_ - 1; j++ )
        {
            // Set panel centroid.
            panelCentroids_[ i ][ j ] = ( meshPoints_[ i ][ j ] +
                                          meshPoints_[ i + 1 ][ j ] +
                                          meshPoints_[ i ][ j + 1 ] +
                                          meshPoints_[ i + 1 ][ j + 1 ] ) / 4;

            // Set panel cross vectors.
            crossVector1 = meshPoints_[ i + 1 ][ j + 1 ] - meshPoints_[ i ][ j ];
            crossVector2 = meshPoints_[ i + 1 ][ j ] - meshPoints_[ i ][ j + 1 ];

            // Set panel normal (not yet normalized).
            panelSurfaceNormals_[ i ][ j ] = crossVector1.cross( crossVector2 );

            // Set panel area (not yet correct size).
            panelAreas_[ i ][ j ] = panelSurfaceNormals_[ i ][ j ].norm( );
            if ( panelAreas_[ i ][ j ] < std::numeric_limits< double >::epsilon( ) )
            {
                std::cerr << "Warning, panel area is zero in part at panel" << i
                          << ", " << j << std::endl;
            }

            // Normalize panel normal and, if necessary, invert normal direction.
            panelSurfaceNormals_[ i ][ j ] *= reversalOperator_;
            panelSurfaceNormals_[ i ][ j ].normalize( );

            // Set panel area to correct size.
            panelAreas_[ i ][ j ] *= 0.5;

            // Add panel area to total area.
            totalArea_ += panelAreas_[ i ][ j ];
        }
    }
}

//! Set reversal operator.
void QuadrilateralMeshedSurfaceGeometry::setReversalOperator( const bool isMeshInverted )
{
    if ( isMeshInverted == 0 )
    {
        reversalOperator_ = 1;
    }

    else
    {
        reversalOperator_ = -1;
    }
}

//! Get boolean denoting if the mesh is inverted.
bool QuadrilateralMeshedSurfaceGeometry::getReversalOperator( )
{
    bool isMeshInverted;
    if ( reversalOperator_ == 1 )
    {
        isMeshInverted = 0;
    }

    else
    {
        isMeshInverted = 1;
    }

    return isMeshInverted;
}

//! Overload ostream to print class information.
std::ostream& operator << ( std::ostream& stream,
                          QuadrilateralMeshedSurfaceGeometry& quadrilateralMeshedSurfaceGeometry )
{
    stream << "This is a quadrilateral meshed surface geometry"
           << " of a single part." << std::endl;
    stream << "The number of lines ( contours ) is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfLines_ << std::endl;
    stream << "The number of points per line is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfPoints_ << std::endl;

    // Return stream.
    return stream;
}

} // namespace geometric_shapes
} // namespace tudat


//==================================================================================================
/*!
  @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_NTHROOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_NTHROOT_HPP_INCLUDED
#include <boost/simd/function/raw.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_INVALID
#include <boost/simd/function/is_nan.hpp>
#endif
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sign.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    inline A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
#ifndef BOOST_SIMD_NO_INVALID
      if (is_nan(a0)) return a0;
#endif
      auto is_ltza0 = is_ltz(a0);
      auto is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
#ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0)) return (a1) ? a0 : One<A0>();
#endif
      A0 aa1 = static_cast<A0>(a1);
      A0 y = bs::raw_(bs::pow_abs)(x,rec(aa1));
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      if (y)
      {
       A0 p = raw_(bs::pow_abs)(y, aa1);
       y -= (p - x) / (aa1*p/y);
      }

      return (is_ltza0 && is_odda1)? -y : y;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( nthroot_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    inline A0 operator() (const raw_tag &,  A0 a0, A1 a1
                         ) const BOOST_NOEXCEPT
    {
       auto is_ltza0 = is_ltz(a0);
       auto is_odda1 = is_odd(a1);
       if (is_ltza0 && !is_odda1) return Nan<A0>();
       A0 x = bs::abs(a0);
      if (x == One<A0>()) return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
      A0 aa1 = static_cast<A0>(a1);
      A0 y = raw_(bs::pow_abs)(x,rec(aa1));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };
} } }

#endif


#ifndef MLT_MODELS_TRANSFORMERS_SPARSE_AUTOENCODER_HPP
#define MLT_MODELS_TRANSFORMERS_SPARSE_AUTOENCODER_HPP

#include <cmath>
#include <tuple>
#include <type_traits>

#include <Eigen/Core>

#include "transformer.hpp"
#include "../implementations/autoencoder.hpp"
#include "../../utils/eigen.hpp"

namespace mlt {
namespace models {
namespace transformers {
	using namespace utils::eigen;

	template <class HiddenActivation, class ReconstructionActivation, class Optimizer>
	class SparseAutoencoder : public Transformer<SparseAutoencoder<HiddenActivation, ReconstructionActivation, Optimizer>> {
	public:
		template <typename H, typename R, typename O,
			class = enable_if<is_convertible<decay_t<H>, HiddenActivation>::value
			&& is_convertible<decay_t<R>, ReconstructionActivation>::value
			&& is_convertible<decay_t<O>, Optimizer>::value>>
		explicit SparseAutoencoder(int hidden_units, H&& hidden_activation, R&& reconstruction_activation, O&& optimizer, double regularization,
		double sparsity, double sparsity_weight) : _hidden_units(hidden_units), _hidden_activation(forward<H>(hidden_activation)),
			_reconstruction_activation(forward<R>(reconstruction_activation)), _optimizer(forward<O>(optimizer)), _regularization(regularization),
			_sparsity(sparsity), _sparsity_weight(sparsity_weight) {}

		Result transform(Features input) const {
			assert(_fitted);
			return _hidden_activation.compute((_hidden_weights * input).colwise() + _hidden_intercepts);
		}

		Self& fit(Features input, bool cold_start = true) {
			VectorXd init(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units + input.rows());

			if (_fitted && !cold_start) {
				init.block(0, 0, _hidden_weights.size(), 1) = ravel(_hidden_weights);
				init.block(_hidden_weights.size(), 0, _hidden_intercepts.size(), 1) = _hidden_intercepts;

				init.block(_hidden_weights.size() + _hidden_intercepts.size(), 0, _reconstruction_weights.size(), 1) =
					ravel(_reconstruction_weights);
				init.block(_hidden_weights.size() + _hidden_intercepts.size() + _reconstruction_weights.size(),
					0, _reconstruction_intercepts.size(), 1) = _reconstruction_intercepts;
			} else {
				init = (init.setRandom() * 4 / sqrt(6.0 / (_hidden_units + input.rows())));
			}

			VectorXd coeffs = _optimizer(*this, input, input, init, cold_start);

			_hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			_hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			_reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, _hidden_units * input.rows(), 1), input.rows(), _hidden_units);
			_reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			_fitted = true;

			return *this;
		}

		using Transformer<Self>::fit;

		auto loss(VectorXdRef coeffs, Features input, MatrixXdRef target) const {
			auto hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows() * _hidden_units, 1), input.rows(), _hidden_units);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			return implementations::autoencoder::sparse_loss(_hidden_activation, _reconstruction_activation, hidden_weights, hidden_intercepts,
				reconstruction_weights, reconstruction_intercepts, _regularization, _sparsity, _sparsity_weight, input, target);
		}

		auto gradient(VectorXdRef coeffs, Features input, MatrixXdRef target) const {
			auto hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows() * _hidden_units, 1), input.rows(), _hidden_units);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			MatrixXd hid_weights_grad, rec_weights_grad;
			VectorXd hid_inter_grad, rec_inter_grad;

			tie(hid_weights_grad, hid_inter_grad, rec_weights_grad, rec_inter_grad) = implementations::autoencoder::sparse_gradient(_hidden_activation,
				_reconstruction_activation, hidden_weights, hidden_intercepts, reconstruction_weights, reconstruction_intercepts, _regularization,
				_sparsity, _sparsity_weight, input, target);

			VectorXd gradient(coeffs.rows());

			gradient.block(0, 0, hid_weights_grad.size(), 1) = ravel(hid_weights_grad);
			gradient.block(hid_weights_grad.size(), 0, hid_inter_grad.size(), 1) = hid_inter_grad;

			gradient.block(hid_weights_grad.size() + hid_inter_grad.size(), 0, rec_weights_grad.size(), 1) =
					ravel(rec_weights_grad);
			gradient.block(hid_weights_grad.size() + hid_inter_grad.size() + rec_weights_grad.size(),
					0, rec_inter_grad.size(), 1) = rec_inter_grad;

			return gradient;
		}

		tuple<double, VectorXd> loss_and_gradient(VectorXdRef coeffs, Features input, MatrixXdRef target) const {
			auto hidden_weights = unravel(coeffs.block(0, 0, _hidden_units * input.rows(), 1), _hidden_units, input.rows());
			auto hidden_intercepts = coeffs.block(_hidden_units * input.rows(), 0, _hidden_units, 1);
			auto reconstruction_weights = unravel(coeffs.block(_hidden_units * input.rows() + _hidden_units, 0, input.rows() * _hidden_units, 1), input.rows(), _hidden_units);
			auto reconstruction_intercepts = coeffs.block(_hidden_units * input.rows() + _hidden_units + input.rows() * _hidden_units, 0, input.rows(), 1);

			double loss;
			MatrixXd hid_weights_grad, rec_weights_grad;
			VectorXd hid_inter_grad, rec_inter_grad;

			tie(loss, hid_weights_grad, hid_inter_grad, rec_weights_grad, rec_inter_grad) = implementations::autoencoder::sparse_loss_and_gradient(_hidden_activation,
				_reconstruction_activation, hidden_weights, hidden_intercepts, reconstruction_weights, reconstruction_intercepts, _regularization,
				_sparsity, _sparsity_weight, input, target);

			VectorXd gradient(coeffs.rows());

			gradient.block(0, 0, hid_weights_grad.size(), 1) = ravel(hid_weights_grad);
			gradient.block(hid_weights_grad.size(), 0, hid_inter_grad.size(), 1) = hid_inter_grad;

			gradient.block(hid_weights_grad.size() + hid_inter_grad.size(), 0, rec_weights_grad.size(), 1) =
				ravel(rec_weights_grad);
			gradient.block(hid_weights_grad.size() + hid_inter_grad.size() + rec_weights_grad.size(),
				0, rec_inter_grad.size(), 1) = rec_inter_grad;

			return { loss, gradient };
		}

	protected:
		int _hidden_units;
		HiddenActivation _hidden_activation;
		ReconstructionActivation _reconstruction_activation;
		Optimizer _optimizer;
		double _regularization;
		double _sparsity;
		double _sparsity_weight;

		MatrixXd _hidden_weights;
		VectorXd _hidden_intercepts;
		MatrixXd _reconstruction_weights;
		VectorXd _reconstruction_intercepts;
	};

	template <class HiddenActivation, class ReconstructionActivation, class Optimizer>
	auto create_sparse_autoencoder(int hidden_units, HiddenActivation&& hidden_activation,
	ReconstructionActivation&& reconstruction_activation, Optimizer&& optimizer,
	double regularization, double sparsity, double sparsity_weight) {
		return SparseAutoencoder<HiddenActivation, ReconstructionActivation, Optimizer>(
			hidden_units,
			forward<HiddenActivation>(hidden_activation),
			forward<ReconstructionActivation>(reconstruction_activation),
			forward<Optimizer>(optimizer),
			regularization, sparsity, sparsity_weight);
	}
}
}
}
#endif


#include "wave/optimization/ceres/odom_linear/point_to_line_interpolated_transform.hpp"
#include <Eigen/QR>

namespace wave {

SE3PointToLine::SE3PointToLine(const double *const p, const double *const pA, const double *const pB, const double *const scal, const Mat3 &CovZ, bool calculate_weight)
        : pt(p), ptA(pA), ptB(pB), scale(scal) {
    Eigen::Matrix<double, 3, 12> JP_T;
    Eigen::Matrix<double, 3, 3> Jres_P;

    JP_T << this->pt[0], 0, 0, this->pt[1], 0, 0, this->pt[2], 0, 0, 1, 0, 0, //
            0, this->pt[0], 0, 0, this->pt[1], 0, 0, this->pt[2], 0, 0, 1, 0, //
            0, 0, this->pt[0], 0, 0, this->pt[1], 0, 0, this->pt[2], 0, 0, 1;

    this->diff[0] = this->ptB[0] - this->ptA[0];
    this->diff[1] = this->ptB[1] - this->ptA[1];
    this->diff[2] = this->ptB[2] - this->ptA[2];
    this->bottom = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2];

    if (this->bottom < 1e-10) {
        // The points defining the line are too close to each other
        throw std::out_of_range("Points defining line are too close!");
    }

    Jres_P(0,0) = 1 - (diff[0] * diff[0] / bottom);
    Jres_P(0,1) = -(diff[0] * diff[1] / bottom);
    Jres_P(0,2) = -(diff[0] * diff[2] / bottom);
    Jres_P(1,0) = -(diff[1] * diff[0] / bottom);
    Jres_P(1,1) = 1 - (diff[1] * diff[1] / bottom);
    Jres_P(1,2) = -(diff[1] * diff[2] / bottom);
    Jres_P(2,0) = -(diff[2] * diff[0] / bottom);
    Jres_P(2,1) = -(diff[2] * diff[1] / bottom);
    Jres_P(2,2) = 1 -(diff[2] * diff[2] / bottom);

    Eigen::Vector3d unitdiff;
    double invlength = 1.0/sqrt(this->bottom);
    if(this->diff[2] > 0) {
        unitdiff[0] = this->diff[0] * invlength;
        unitdiff[1] = this->diff[1] * invlength;
        unitdiff[2] = this->diff[2] * invlength;
    } else {
        unitdiff[0] = -this->diff[0] * invlength;
        unitdiff[1] = -this->diff[1] * invlength;
        unitdiff[2] = -this->diff[2] * invlength;
    }

    Eigen::Vector3d unitz;
    unitz << 0, 0, 1;

    auto v = unitdiff.cross(unitz);
    auto s = v.norm();
    auto c = unitz.dot(unitdiff);
    Mat3 skew;
    Transformation<>::skewSymmetric3(v, skew);
    this->rotation = Eigen::Matrix3d::Identity() + skew + skew*skew*((1-c)/(s*s));

    this->Jres_T = this->rotation * Jres_P * JP_T;

    if(calculate_weight) {
        auto rotated = (this->rotation * Jres_P * CovZ * Jres_P.transpose() * this->rotation.transpose());
        this->weight_matrix = rotated.block<2,2>(0,0).inverse().sqrt();
    } else {
        this->weight_matrix.setIdentity();
    }
}

bool SE3PointToLine::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    Eigen::Map<const Mat34> Tmap(parameters[0], 3, 4);
    Transformation<Eigen::Map<const Mat34>> Tk(Tmap);

    Transformation<Eigen::Matrix<double, 3, 4>> interpolated;

    Eigen::Map<const Vec3> PT(pt, 3, 1);
    auto twist = Tk.logMap();
    interpolated.setFromExpMap(*(this->scale) * twist);
    Vec3 POINT = interpolated.transform(PT);
    double point[3];
    Eigen::Map<Vec3>(point, 3, 1) = POINT;

    double p_A[3] = {point[0] - this->ptA[0], point[1] - this->ptA[1], point[2] - this->ptA[2]};

    double scaling = ceres::DotProduct(p_A, diff);
    // point on line closest to point
    double p_Tl[3] = {this->ptA[0] + (scaling / bottom) * diff[0],
                      this->ptA[1] + (scaling / bottom) * diff[1],
                      this->ptA[2] + (scaling / bottom) * diff[2]};

    double residual_o[3];
    residual_o[0] = point[0] - p_Tl[0];
    residual_o[1] = point[1] - p_Tl[1];
    residual_o[2] = point[2] - p_Tl[2];
    Eigen::Map<Vec3> res(residual_o, 3, 1);
    Eigen::Vector2d reduced = (this->rotation * res).block<2,1>(0,0);
    reduced = this->weight_matrix * reduced;
    Eigen::Map<Eigen::Vector2d>(residuals, 2, 1) = reduced;

    if (jacobians != NULL) {
        // Have to apply the "lift" jacobian to the Interpolation Jacobian because
        // of Ceres local parameterization
        Transformation<>::Jinterpolated(twist, *(this->scale), this->J_int);
        interpolated.J_lift(this->J_lift);
        Tk.J_lift(this->J_lift_full);

        this->J_lift_full_pinv = (this->J_lift_full.transpose() * this->J_lift_full).inverse() * this->J_lift_full.transpose();

        this->Jr_T = this->Jres_T * this->J_lift * this->J_int * this->J_lift_full_pinv;
        this->Jr_T_reduced = this->weight_matrix * this->Jr_T.block<2,12>(0,0);

        Eigen::Map<Eigen::Matrix<double, 2, 12, Eigen::RowMajor>>(jacobians[0], 2, 12) = this->Jr_T_reduced;
    }

    return true;
}

}  // namespace wave


#include <iostream>
#include <vector>
#include <numeric>
#include <Eigen/Dense>
#include "mex.h"
#include "matrix.h"

#if $(use_sparse_template)
#include <Eigen/Sparse>
#endif
#if $(use_sturm_eigensolver)
#define MAX_DEG  $(num_basis)
#include "sturm.h"
#include "charpoly.h"
#endif
#if $(use_sturm_dani_eigensolver)
#define MAX_DEG  $(num_basis)
// #include "sturm.h"
#define DEG  $(num_basis)
#include "sturm_mart.h"
#include "charpoly.h"
#endif

using namespace Eigen;

#if $(use_reduced_eigenvector_solver)
void fast_eigenvector_solver(double * eigv, int neig, Eigen::Matrix<double,$(num_basis),$(num_basis)> &AM, MatrixXcd &sols);
#endif

void solver_$(solv_name)($(function_param_declaration))
{
    // Compute coefficients

$(code_compute_coefficients)

    // Setup elimination template
    static const int coeffs0_ind[] = { $(coeffs0_ind) };
    static const int coeffs1_ind[] = { $(coeffs1_ind) };

$(code_setup_template)

    // Setup action matrix
    Matrix<double,$(num_available), $(num_basis)> RR;
    RR << -C12.bottomRows($(num_reducible)), Matrix<double,$(num_basis),$(num_basis)>::Identity($(num_basis), $(num_basis));

    static const int AM_ind[] = { $(AM_ind) };
    Matrix<double, $(num_basis), $(num_basis)> AM;
    for (int i = 0; i < $(num_basis); i++) {
        AM.row(i) = RR.row(AM_ind[i]);
    }

    MatrixXcd sols($(num_vars), $(num_basis));
    sols.setZero();

    // Solve eigenvalue problem
#if $(use_standard_eigensolver)
    EigenSolver<Matrix<double, $(num_basis), $(num_basis)> > es(AM);
    ArrayXcd D = es.eigenvalues();
    ArrayXXcd V = es.eigenvectors();

    $(code_normalize_eigenvectors)
    $(code_extract_solutions)
#endif
#if $(use_eigsonly_eigensolver)

    EigenSolver<MatrixXd> es(AM, false);
    ArrayXcd D = es.eigenvalues();

    int nroots = 0;
    double eigv[$(num_basis)];
    for (int i = 0; i < $(num_basis); i++) {
        if (std::abs(D(i).imag()) < 1e-6)
            eigv[nroots++] = D(i).real();
    }

    fast_eigenvector_solver(eigv, nroots, AM, sols);
#endif
#if $(use_sturm_eigensolver)
    double p[1+$(num_basis)];
    Matrix<double, $(num_basis), $(num_basis)> AMp = AM;
    charpoly_$(charpoly_method)(AMp, p);
    double roots[$(num_basis)];
    int nroots;
    find_real_roots_sturm(p, $(num_basis), roots, &nroots, 8, 0);
    fast_eigenvector_solver(roots, nroots, AM, sols);
#endif

#if $(use_sturm_dani_eigensolver)

    double p[1 + $(num_basis)];
    Matrix<double, $(num_basis), $(num_basis)> T;
    charpoly_danilevsky_piv_T(AM, p, T);
    double roots[$(num_basis)];
    int nroots;
    // find_real_roots_sturm(p, $(num_basis), roots, &nroots, 8, 0);
    nroots = realRoots(p, roots);
    sols.resize($(num_vars), nroots);

    Eigen::MatrixXd V($(num_basis), nroots);
    Eigen::Map<Eigen::MatrixXd> D(roots, 1, nroots);
    V.bottomRows(1).setConstant(1);
    for (int j = $(num_basis) - 2; j >= 0; j--) {
        V.row(j) = V.row(j + 1).array() * D.array();
    }
    V = T * V;
    Eigen::RowVectorXd row = V.row(0);
    V.array().rowwise() /= row.array();

    D.transposeInPlace();
    $(code_extract_solutions)

#endif
    $(code_pack_outputs)

}
$(debug_comments)

#if $(use_reduced_eigenvector_solver)
    void fast_eigenvector_solver(double * eigv, int neig, Eigen::Matrix<double,$(num_basis),$(num_basis)> &AM, MatrixXcd &sols) {
    static const int ind[] = { $(ind_non_trivial) };
    // Truncated action matrix containing non-trivial rows
    Matrix<double, $(length_ind_non_trivial), $(num_basis)> AMs;
    double zi[$(max_power)];

    for (int i = 0; i < $(length_ind_non_trivial); i++)    {
        AMs.row(i) = AM.row(ind[i]);
    }
    for (int i = 0; i < neig; i++) {
        zi[0] = eigv[i];
        for (int j = 1; j < $(max_power); j++)
        {
            zi[j] = zi[j - 1] * eigv[i];
        }
        Matrix<double, $(AA_sz)> AA;
$(code_setup_reduced_eigenvalue_eq)

        Matrix<double, $(ind_unit), 1>  s = AA.leftCols($(ind_unit)).colPivHouseholderQr().solve(-AA.col($(ind_unit)));
$(code_extract_solutions)
    }
}
#endif

mxArray* convertToMatlabCell(std::vector<Eigen::MatrixXcd> const& sols) {
    mxArray* cell = mxCreateCellMatrix(1, sols.size());
    for (int i = 0; i < sols.size(); ++i) {
        Eigen::MatrixXcd sol = sols.at(i);
        mxArray* mat = mxCreateDoubleMatrix(sol.rows(), sol.cols(), mxCOMPLEX);
        memcpy(mxGetComplexDoubles(mat), sol.data(), sizeof(mxComplexDouble) * sol.size());
        // double* mat_r = mxGetPr(mat);
        // double* mat_i = mxGetPi(mat);
        // for (int r = 0; r < sol.rows(); ++r) {
        //     for (int c = 0; c < sol.cols(); ++c) {
        //         mat_r[c * sol.rows() + r] = sol(r, c).real();
        //         mat_i[c * sol.rows() + r] = sol(r, c).imag();
        //     }
        // }
        mxSetCell(cell, i, mat);
    }
    return cell;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    std::ios_base::sync_with_stdio(false);
    if (nrhs != $(input_size)) {
        mexErrMsgIdAndTxt("automatic_generator_cvpr:$(solv_name):nrhs", "One input required.");
    }
    if (nlhs != $(output_size)) {
        mexErrMsgIdAndTxt("automatic_generator_cvpr:$(solv_name):nlhs", "One output required.");
    }
    if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
        mexErrMsgIdAndTxt("automatic_generator_cvpr:$(solv_name):notDouble", "Input data must be type double.");
    }
    $(code_mex_function_body)
}


/*
 *  MIT License
 *
 *  Copyright (c) 2017 Piotr Dobrowolski
 *
 *  permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in all
 *  copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *  SOFTWARE.
 *
 */
/*
 * Author: Piotr Dobrowolski
 * dobrypd[at]gmail[dot]com
 *
 */
#include "HDRCreator.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/math/special_functions.hpp>
#include <vector>
#include <cmath>

namespace HDRCreation
{

using namespace std;

HDRCreator::HDRCreator(const GlobalArgs_t & globalArgs)
    : globalArgs(globalArgs)
{
    // TODO Auto-generated constructor stub
}

typedef float(*luminanceCorrection_t)(float);

float logLumiCorrection(float value) {
    double base = 10; // TODO:

    double baseFraction = log(base);
    double logValue = log1p(value);

    return logValue / baseFraction;
}

float powLumiCorrection(float value) {
    double power = 3.3; // TODO:

    double powValue = pow(value + 1, power);

    return powValue;
}

void correctLuminocity(luminanceCorrection_t lumiCorrection, pfs::FramePtr frame) {
    using namespace pfs;
    pfs::Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    for (auto itX = X->begin(), itY = Y->begin(), itZ = Z->begin();
            itX != X->end(); itX++, itY++, itZ++) {
        (*itX) = lumiCorrection(*itX);
        (*itY) = lumiCorrection(*itY);
        (*itZ) = lumiCorrection(*itZ);
    }
}

typedef float(*agrFun_t)(pfs::FramePtr);

float avgAgr(pfs::FramePtr frame) {
    using namespace pfs;
    pfs::Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    double sumX=0, sumY=0, sumZ=0;
    for (auto itX = X->begin(), itY = Y->begin(), itZ = Z->begin();
            itX != X->end(); itX++, itY++, itZ++) {
        sumX += *itX;
        sumY += *itY;
        sumZ += *itZ;
    }
    sumX /= frame->getWidth() * frame->getHeight();
    sumY /= frame->getWidth() * frame->getHeight();
    sumZ /= frame->getWidth() * frame->getHeight();

    return (sumX + sumY + sumZ) / 3; // dummy heuristic, use colorspace characteristic
}

float heuristicRelativeEV(agrFun_t aggregation, pfs::FramePtr frame)
{
    return aggregation(frame);
}

void getChannelsIterators(pfs::Channel *X, pfs::Channel *Y, pfs::Channel *Z,
        pfs::Channel::iterator & itX, pfs::Channel::iterator & itY, pfs::Channel::iterator & itZ)
{
    itX = X->begin();
    itY = Y->begin();
    itZ = Z->begin();
}

void getChannelsIterators(pfs::FramePtr frame,
        pfs::Channel::iterator & itX, pfs::Channel::iterator & itY, pfs::Channel::iterator & itZ)
{
    using pfs::Channel;
    Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    getChannelsIterators(X, Y, Z, itX, itY, itZ);
}

void getChannelsIterators(pfs::FramePtr frame,
        pfs::Channel::iterator & itX, pfs::Channel::iterator & itY, pfs::Channel::iterator & itZ,
        pfs::Channel::iterator & endX)
{
    using pfs::Channel;
    Channel *X, *Y, *Z;
    frame->getXYZChannels(X, Y, Z);
    getChannelsIterators(X, Y, Z, itX, itY, itZ);
    endX = X->end();
}

void createBeginIterators(vector<pfs::FramePtr> & frames,
        vector<pfs::Channel::iterator> & XChannelsIt,
        vector<pfs::Channel::iterator> & YChannelsIt,
        vector<pfs::Channel::iterator> & ZChannelsIt)
{
    XChannelsIt.resize(frames.size());
    YChannelsIt.resize(frames.size());
    ZChannelsIt.resize(frames.size());
    int expNo = 0;
    for (auto frameIt = frames.begin(); frameIt != frames.end(); ++frameIt, ++expNo)
    {
        pfs::Channel::iterator itX, itY, itZ;
        getChannelsIterators(*frameIt, itX, itY, itZ);
        XChannelsIt[expNo] = itX;
        YChannelsIt[expNo] = itY;
        ZChannelsIt[expNo] = itZ;
    }
}

/**
 * Assumptions:
 * 1 - all frames are aligned, with fixed size.
 * 2 - frames are exposure value ordered
 */
bool HDRCreator::create(kernel::GenericFramePtr output, std::vector<kernel::GenericFramePtr> & framesGeneric)
{
    /**
     * Assertions
     */
    debug_print(LVL_INFO, "Found %d frame(s) to process.\n", framesGeneric.size());
    if (output == 0) {
    	return false;
    }

    if (!output->isValid()) {
    	if (framesGeneric.front()->isValid())
    	{
    		if (!output->setEmptyFrameFrom(framesGeneric.front()->getRawFramePFS())) {
    		    return false;
    		}
    	} else {
    		return false;
    	}
    }
    assert(output->isValid());
    pfs::FramePtr outputFrame = output->getRawFramePFS();

    /**
     * Create pointers.
     * Map luminocity in each frame.
     */
    int noOfExposures = framesGeneric.size();
    size_t frameSize = outputFrame->getWidth() * outputFrame->getHeight();
    vector<pfs::FramePtr> frames;
    vector<double> framesRelativeEVComputed; // X translation
    vector<double> avgLuminocityRelativeFractionX, avgLuminocityRelativeFractionY,avgLuminocityRelativeFractionZ; // Y translation, than frame 1
    for (auto frameGenIt = framesGeneric.begin(); frameGenIt != framesGeneric.end(); frameGenIt++) {
        pfs::FramePtr frame = (*frameGenIt)->getRawFramePFS();
        frames.push_back(frame);
        correctLuminocity(&logLumiCorrection, frame);
        framesRelativeEVComputed.push_back(heuristicRelativeEV(&avgAgr, frame));

        avgLuminocityRelativeFractionX.push_back(0.0f);
        avgLuminocityRelativeFractionY.push_back(0.0f);
        avgLuminocityRelativeFractionZ.push_back(0.0f);
    }

    /**
     * Prepare vectors of channels. Channel data iterators.
     */
    pfs::Channel::iterator itX, itY, itZ, endX;

    /**
     *
     */
    vector<pfs::Channel::iterator> XChannelsIt, YChannelsIt, ZChannelsIt;
    createBeginIterators(frames, XChannelsIt, YChannelsIt, ZChannelsIt);
    float relativeX = **XChannelsIt.begin(), relativeY = **YChannelsIt.begin(), relativeZ = **ZChannelsIt.begin();
    int expNo = 1;
    for (auto XChannelsDataIt = ++XChannelsIt.begin(), YChannelsDataIt = ++YChannelsIt.begin(), ZChannelsDataIt = ++ZChannelsIt.begin();
            XChannelsDataIt != XChannelsIt.end();
            XChannelsDataIt++, YChannelsDataIt++, ZChannelsDataIt++, expNo++)
    {
        auto XValueIt = *XChannelsDataIt, YValueIt = *YChannelsDataIt, ZValueIt = *ZChannelsDataIt;
        for (size_t pixel = 0; pixel < frameSize;
                ++pixel, ++XValueIt, ++YValueIt, ++ZValueIt)
        {
            auto XValue = *XValueIt, YValue = *YValueIt, ZValue = *ZValueIt;
            avgLuminocityRelativeFractionX[expNo] += XValue - relativeX;
            avgLuminocityRelativeFractionY[expNo] += YValue - relativeY;
            avgLuminocityRelativeFractionZ[expNo] += ZValue - relativeZ;
        }
    }
    // Normalize
    for(auto avgLRFXIt = avgLuminocityRelativeFractionX.begin(), avgLRFYIt = avgLuminocityRelativeFractionY.begin(),
            avgLRFZIt = avgLuminocityRelativeFractionZ.begin();
            avgLRFXIt != avgLuminocityRelativeFractionX.end();
            avgLRFXIt++, avgLRFYIt++, avgLRFZIt++)
    {
        (*avgLRFXIt) /= frameSize;
        (*avgLRFYIt) /= frameSize;
        (*avgLRFZIt) /= frameSize;
        if (boost::math::isnan((*avgLRFXIt))) { debug_print(LVL_LOW, "*avgLRFXIt isn't a number = %f\n", *avgLRFXIt); }
        if (boost::math::isnan((*avgLRFYIt))) { debug_print(LVL_LOW, "*avgLRFYIt isn't a number = %f\n", *avgLRFYIt); }
        if (boost::math::isnan((*avgLRFZIt))) { debug_print(LVL_LOW, "*avgLRFZIt isn't a number = %f\n", *avgLRFZIt); }
    }

    /**
     * Translate vector.
     */
    createBeginIterators(frames, XChannelsIt, YChannelsIt, ZChannelsIt);
    expNo = 0;
    for (auto XChannelsDataIt = XChannelsIt.begin(), YChannelsDataIt = YChannelsIt.begin(), ZChannelsDataIt = ZChannelsIt.begin();
            XChannelsDataIt != XChannelsIt.end();
            XChannelsDataIt++, YChannelsDataIt++, ZChannelsDataIt++, expNo++)
    {
        auto & XValueIt = *XChannelsDataIt, & YValueIt = *YChannelsDataIt, & ZValueIt = *ZChannelsDataIt;
        auto expLumin = avgLuminocityRelativeFractionX[expNo];
        auto frameRelEV = framesRelativeEVComputed[expNo];
        auto pixelFraction = expLumin + frameRelEV;
        if (boost::math::isnan(pixelFraction)) { debug_print(LVL_LOW, "pixelFraction isn't a number = %f\n", pixelFraction); }
        for (size_t pixel = 0; pixel < frameSize;
                ++pixel, ++XValueIt, ++YValueIt, ++ZValueIt)
        {
            auto & XValue = *XValueIt, YValue = *YValueIt, ZValue = *ZValueIt;
            XValue += pixelFraction;
            YValue += pixelFraction;
            ZValue += pixelFraction;
        }
    }

    // Create HDR Image
    int it = 0;
    createBeginIterators(frames, XChannelsIt, YChannelsIt, ZChannelsIt);
    auto XChannelsDataIt = XChannelsIt.begin(), YChannelsDataIt = YChannelsIt.begin(), ZChannelsDataIt = ZChannelsIt.begin();
    float minXV = 1000, minYV = 1000, minZV = 1000;
    float maxXV = -1, maxYV = -1, maxZV = -1;
    pfs::Channel *X, *Y, *Z;
    outputFrame->createXYZChannels(X, Y, Z);
    endX = X->end();
    for(getChannelsIterators(X, Y, Z, itX, itY, itZ); itX != endX;
            ++itX, ++itY, ++itZ, it++) {
        auto & XOutputVal = *itX, & YOutputVal = *itY, & ZOutputVal = *itZ;
        XOutputVal = 0, YOutputVal = 0, ZOutputVal = 0;
        XChannelsDataIt = XChannelsIt.begin(), YChannelsDataIt = YChannelsIt.begin(), ZChannelsDataIt = ZChannelsIt.begin();
        for (int expNo = 0; expNo < noOfExposures; XChannelsDataIt++, YChannelsDataIt++, ZChannelsDataIt++, expNo++)
        {
            auto & XValueIt = *XChannelsDataIt, & YValueIt = *YChannelsDataIt, & ZValueIt = *ZChannelsDataIt;
            auto XValue = *XValueIt, YValue = *YValueIt, ZValue = *ZValueIt;

            XOutputVal += XValue;
            YOutputVal += YValue;
            ZOutputVal += ZValue;

            ++XValueIt;
            ++YValueIt;
            ++ZValueIt;
        }
        if (minXV > XOutputVal) minXV = XOutputVal;
        if (minYV > YOutputVal) minYV = YOutputVal;
        if (minZV > ZOutputVal) minZV = ZOutputVal;
        if (maxXV < XOutputVal) maxXV = XOutputVal;
        if (maxYV < YOutputVal) maxYV = YOutputVal;
        if (maxZV < ZOutputVal) maxZV = ZOutputVal;
        //debug_print(LVL_LOW, "X = %f, Y = %f, Z = %f,\n", XOutputVal, YOutputVal, ZOutputVal);
        if (boost::math::isnan(XOutputVal)) { debug_print(LVL_LOW, "XOutputVal isn't a number = %f\n", XOutputVal); }
        if (boost::math::isnan(YOutputVal)) { debug_print(LVL_LOW, "YOutputVal isn't a number = %f\n", YOutputVal); }
        if (boost::math::isnan(ZOutputVal)) { debug_print(LVL_LOW, "ZOutputVal isn't a number = %f\n", ZOutputVal); }
    }

    float rangeX = abs(maxXV - minXV), rangeY = abs(maxYV - minYV), rangeZ = abs(maxZV - minZV);
    debug_print(LVL_DEBUG, "rangeX=(%f - %f) rangeY=(%f - %f) rangeZ=(%f - %f)\n", minXV, maxXV, minYV, maxYV, minZV, maxZV);
    long nanX = 0, nanY = 0, nanZ = 0;

    // Normalize output (TONE MAP)
    for(getChannelsIterators(outputFrame, itX, itY, itZ, endX); itX != endX;
            ++itX, ++itY, ++itZ) {
        auto & XOutputVal = *itX, & YOutputVal = *itY, & ZOutputVal = *itZ;
        XOutputVal = (XOutputVal - minXV) / rangeX;
        YOutputVal = (YOutputVal - minYV) / rangeY;
        ZOutputVal = (ZOutputVal - minZV) / rangeZ;

        if (boost::math::isnan(XOutputVal)) {
            XOutputVal = 0;
            nanX++;
        }
        if (boost::math::isnan(YOutputVal)) {
            YOutputVal = 0;
            nanY++;
        }
        if (boost::math::isnan(ZOutputVal)) {
            ZOutputVal = 0;
            nanZ++;
        }
    }
    debug_print(LVL_DEBUG, "NanX = %f, NanY = %f, NanZ = %f\n", (float)nanX / (float)frameSize,(float)nanY / (float)frameSize,(float)nanZ / (float)frameSize);

    // Create output.
    return true; // XXX
}

} /* namespace HDRCreation */


#pragma once

#include <cstdio>

#include <string_view>
#include <array>
#include <string>
#include <algorithm>
#include <type_traits>

#include <boost/rational_minimal.hpp>
#include <numeric>

namespace threads {
  //using ratio_t = boost::rational<uint64_t>;
  using Rational = boost::rational<unsigned>;

  constexpr auto tpi_pitch = [](auto tpi) -> Rational {
    return {127, 5 * tpi};
  };

  enum class pitch_type {
    mm,
    tpi,
  };

  struct pitch_info {
    std::string_view pitch_str;
    Rational value;
    pitch_type type;

    std::string_view unit() const {
      if (type == pitch_type::mm)
        return "mm";
      else
        return "tpi";
    }
  };

  struct thread {
    std::string_view name;
    pitch_info pitch;
    bool is_custom{false};

    char * description_c_str(char *buf) const {
      buf = std::copy(name.begin(), name.end(), buf);
      *buf++ = ' ';
      buf = std::copy(pitch.pitch_str.begin(), pitch.pitch_str.end(), buf);
      auto unit = pitch.unit();
      buf = std::copy(unit.begin(), unit.end(), buf);
      if (is_custom) {
        *buf++ = '*';
      }
      return buf;
    }
  };

  namespace detail {
    // some stdlib functionality is either not constexpr or uses too much space
    constexpr unsigned sv_to_unsigned(std::string_view sv) {
      unsigned result = 0;
      for (auto i = sv.begin(); i != sv.end(); ++i) {
        uint8_t digit = *i - '0';
        result = result * 10 + digit;
      }
      return result;
    }

    constexpr unsigned pow10(unsigned power) {
      unsigned result = 1;
      for (; power > 0; --power) { result *= 10; }
      return result;
    }

    // Convert string containing decimal into rational number with no error
    // e.g. "1.865" -> 373/200
    constexpr Rational decimal_to_rational(std::string_view decimal) {
      auto i_dot = decimal.find('.');
      if (i_dot == decimal.npos) {
        return {sv_to_unsigned(decimal), 1};
      } else {
        auto fraction = decimal;
        fraction.remove_prefix(i_dot + 1);
        auto denom = pow10(fraction.size());
        decimal.remove_suffix(decimal.size() - i_dot);
        auto num = sv_to_unsigned(decimal) * denom + sv_to_unsigned(fraction);
        return {num, denom};
      }
    }

    constexpr pitch_info make_pitch_info(const char* pitch, uint16_t num, uint16_t denom, pitch_type type) {
      if (type == pitch_type::mm) {
        return {pitch, {denom, num}, pitch_type::mm};
      } else {
        return {pitch, {127 * denom, 5 * num}, pitch_type::tpi};
      }
    }

  }

  inline namespace literals {
    constexpr pitch_info operator"" _mm(const char* pitch) {
      return {pitch, detail::decimal_to_rational(pitch), pitch_type::mm};
    }

    constexpr pitch_info operator"" _tpi(const char* pitch) {
      auto r = detail::decimal_to_rational(pitch);
      // TPI = 1 inch/#Threads : 1 inch=25.4mm = 127 / 5 mm
      return {pitch, {127 * r.denominator(), 5 * r.numerator()}, pitch_type::tpi};
    }
  }

}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on June 4, 2020, 10:13 AM
 */

#include "PermeabilityMohrCoulombFailureIndexModel.h"

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

#include "BaseLib/Error.h"
#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "MaterialLib/MPL/Utils/GetSymmetricTensor.h"
#include "MathLib/KelvinVector.h"
#include "MathLib/MathTools.h"
#include "ParameterLib/CoordinateSystem.h"
#include "ParameterLib/Parameter.h"

namespace MaterialPropertyLib
{
template <int DisplacementDim>
PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::
    PermeabilityMohrCoulombFailureIndexModel(
        std::string name, ParameterLib::Parameter<double> const& k0,
        double const kr, double const b, double const c, double const phi,
        double const k_max, double const t_sigma_max,
        ParameterLib::CoordinateSystem const* const local_coordinate_system)
    : k0_(k0),
      kr_(kr),
      b_(b),
      c_(c),
      phi_(boost::math::constants::degree<double>() * phi),
      k_max_(k_max),
      t_sigma_max_(t_sigma_max),
      local_coordinate_system_(local_coordinate_system)
{
    const double t_sigma_upper = c_ / std::tan(phi_);
    if (t_sigma_max_ <= 0.0 || t_sigma_max_ > t_sigma_upper ||
        std::fabs(t_sigma_max_ - t_sigma_upper) <
            std::numeric_limits<double>::epsilon())
    {
        OGS_FATAL(
            "Tensile strength parameter of {:e} is out of the range (0, "
            "c/tan(phi)) = (0, {:e})",
            t_sigma_max_, t_sigma_upper);
    }

    name_ = std::move(name);
}

template <int DisplacementDim>
void PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::checkScale()
    const
{
    if (!std::holds_alternative<Medium*>(scale_))
    {
        OGS_FATAL(
            "The property 'PermeabilityMohrCoulombFailureIndexModel' is "
            "implemented on the 'medium' scale only.");
    }
}

template <int DisplacementDim>
PropertyDataType
PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::value(
    VariableArray const& variable_array,
    ParameterLib::SpatialPosition const& pos, double const t,
    double const /*dt*/) const
{
    auto const& stress_vector = std::get<SymmetricTensor<DisplacementDim>>(
        variable_array[static_cast<int>(Variable::total_stress)]);

    auto const& stress_tensor =
        formEigenTensor<3>(static_cast<PropertyDataType>(stress_vector));

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>>
        eigenvalue_solver(stress_tensor);

    // Principle stress
    auto const sigma = eigenvalue_solver.eigenvalues();

    auto k_data = k0_(t, pos);

    double const max_sigma = std::max(std::fabs(sigma[0]), std::fabs(sigma[2]));

    if (max_sigma < std::numeric_limits<double>::epsilon())
    {
        return fromVector(k_data);
    }

    double const sigma_m = 0.5 * (sigma[2] + sigma[0]);

    double const tau_m = 0.5 * std::fabs(sigma[2] - sigma[0]);
    double f = 0.0;
    if (sigma_m > t_sigma_max_)
    {
        // tensile failure criterion
        f = sigma_m / t_sigma_max_;

        double const tau_tt =
            c_ * std::cos(phi_) - t_sigma_max_ * std::sin(phi_);

        f = std::max(f, tau_m / tau_tt);
    }
    else
    {
        // Mohr Coulomb failure criterion
        f = tau_m / (c_ * std::cos(phi_) - sigma_m * std::sin(phi_));
    }

    if (f >= 1.0)
    {
        const double exp_value = std::exp(b_ * f);
        for (auto& k_i : k_data)
        {
            k_i = std::min(k_i + kr_ * exp_value, k_max_);
        }
    }

    // Local coordinate transformation is only applied for the case that the
    // initial intrinsic permeability is given with orthotropic assumption.
    if (local_coordinate_system_ && (k_data.size() == DisplacementDim))
    {
        Eigen::Matrix<double, DisplacementDim, DisplacementDim> const e =
            local_coordinate_system_->transformation<DisplacementDim>(pos);
        Eigen::Matrix<double, DisplacementDim, DisplacementDim> k =
            Eigen::Matrix<double, DisplacementDim, DisplacementDim>::Zero();

        for (int i = 0; i < DisplacementDim; ++i)
        {
            Eigen::Matrix<double, DisplacementDim, DisplacementDim> const
                ei_otimes_ei = e.col(i) * e.col(i).transpose();

            k += k_data[i] * ei_otimes_ei;
        }
        return k;
    }

    return fromVector(k_data);
}

template <int DisplacementDim>
PropertyDataType
PermeabilityMohrCoulombFailureIndexModel<DisplacementDim>::dValue(
    VariableArray const& /*variable_array*/, Variable const variable,
    ParameterLib::SpatialPosition const& /*pos*/, double const /*t*/,
    double const /*dt*/) const
{
    if (variable == Variable::mechanical_strain)
    {
        return 0.;
    }

    OGS_FATAL(
        "The derivative of the intrinsic permeability k(sigma, ...) with "
        "respect to stress tensor (sigma) is not implemented because that "
        "dk/du is normally omitted.");
}
template class PermeabilityMohrCoulombFailureIndexModel<2>;
template class PermeabilityMohrCoulombFailureIndexModel<3>;
}  // namespace MaterialPropertyLib


// Author: Senthil Kumar Thangavelu kingjuliyen@gmail.com

/*
  Compile:
  g++  HiddenFactorsLearner.cpp \
       -I/opt/boost/1_61_0/include/						\
       -L/opt/boost/1_61_0/lib						\
       -lboost_program_options \
       -O3 -o /tmp/hidden-factor-learner

  Usage:
  export DYLD_LIBRARY_PATH=/opt/boost/1_61_0/lib:$DYLD_LIBRARY_PATH

  DYLD_LIBRARY_PATH=/opt/boost/1_61_0/lib:$DYLD_LIBRARY_PATH \
  time /tmp/hidden-factor-learner --num-factors 5 \
          --default-rating 2.8485 \
          --regularization-param-p 0.002 \
          --regularization-param-q 0.002 \
          --learning-rate-p 0.000002 \
          --learning-rate-q 0.000002 \
          --training-sample-percentage 0.7 \
          --gradient-descent-iteration-count 100 \
          --max-row-dimension 100000000 \
          --max-column-dimension 100000000 \
          --verbose-mode-level 1 \
          --loop-mode-count 3 \
          --input-csv-file-path /tmp/1000.csv \
          --p-q-matrix-output-file-path /tmp/

e.g csv file (remove header)
usrid  itemid rating
785166 6066 3
521295 4972 3
1665652 11701 5
1963419 15887 3

  Linux:

  LD_LIBRARY_PATH=/opt/boost/1_61_0/lib:$LD_LIBRARY_PATH \
  time /tmp/hidden-factor-learner --num-factors 2 \
          --default-rating 2.8485 \
          --regularization-param-p 0.002 \
          --regularization-param-q 0.002 \
          --learning-rate-p 0.000002 \
          --learning-rate-q 0.000002 \
          --training-sample-percentage 0.7 \
          --gradient-descent-iteration-count 100 \
          --max-row-dimension 100000000 \
          --max-column-dimension 100000000 \
          --verbose-mode-level 1 \
          --loop-mode-count 3 \
          --input-csv-file-path /tmp/sf41.csv \
          --p-q-matrix-output-file-path /tmp/

  Install boost:
  ./bootstrap.sh --prefix=/opt/boost/1_61_0
  ./b2 install
*/

#include <iostream>
#include <string>
#include <boost/program_options.hpp>

#include "HiddenFactorsLearner.hpp"

namespace ProgOpts = boost::program_options;

const char * num_factors_str =
  "Number of hidden factors aka K ";
const char * default_rating_str =
  "Default rating used in the matrix factorization"
  " at start of gradient descent ";
const char * regularization_param_p_str =
  "Regularization parameter for matrix P ";
const char * regularization_param_q_str =
  "Regularization parameter for matrix Q ";
const char * learning_rate_p_str =
  "Learning rate for matrix P parameters in gradient descent ";
const char * learning_rate_q_str =
  "Learning rate for matrix Q parameters in gradient descent ";
const char * gradient_descent_iteration_count_str =
  "Number of times gradient descent iterations need to be run ";
const char * training_sample_percentage_str =
  "Percentage of data set to be used for training ";
  //"remaining percentage will be used for validation ";
const char * max_row_dim_str= "Max dimension for rows in matrix";
const char * max_col_dim_str = "Max dimension for columns in matrix";
const char * input_csv_str = "Path of input csv file to read rating "
  "entries from ";
const char * verbose_mode_level_str = "Show debug info about inner workings ";
const char * loop_mode_count_str = "Run repeatedly in loop mode for same input ";
const char * P_Q_matrix_output_file_path_str = "path to store P and Q matrix output";

bool processInputArgs(int argc, char * argv[], ProgOpts::variables_map &varMap,
        MatrixFactorizationParams &params)
{
  try {
    ProgOpts::options_description desc("Allowed Options");

    desc.add_options()
      ("help", "produce help message")
      ("num-factors", ProgOpts::value<INT_T>(), num_factors_str)
      ("default-rating", ProgOpts::value<FLT_T>(), default_rating_str)
      ("regularization-param-p", ProgOpts::value<FLT_T>(), regularization_param_p_str)
      ("regularization-param-q", ProgOpts::value<FLT_T>(), regularization_param_q_str)
      ("learning-rate-p", ProgOpts::value<FLT_T>(), learning_rate_p_str)
      ("learning-rate-q", ProgOpts::value<FLT_T>(), learning_rate_q_str)
      ("gradient-descent-iteration-count", ProgOpts::value<INT_T>(), gradient_descent_iteration_count_str)
      ("training-sample-percentage", ProgOpts::value<FLT_T>(), training_sample_percentage_str)
      ("max-row-dimension", ProgOpts::value<INT_T>(), max_row_dim_str)
      ("max-column-dimension", ProgOpts::value<INT_T>(), max_col_dim_str)
      ("input-csv-file-path", ProgOpts::value<STRING_T>(), input_csv_str)
      ("verbose-mode-level", ProgOpts::value<INT_T>(), verbose_mode_level_str)
      ("loop-mode-count", ProgOpts::value<INT_T>(), loop_mode_count_str)
      ("p-q-matrix-output-file-path", ProgOpts::value<STRING_T>(), P_Q_matrix_output_file_path_str)
      ; // leave this semi colon at end don't move this

    ProgOpts::store(ProgOpts::parse_command_line(argc, argv, desc), varMap);
    ProgOpts::notify(varMap);

    if (varMap.count("help")) {
      cout << desc << "\n";
      return false;
    }

    OPT(num_factors , "num-factors", INT_T,  5);
    OPT(default_rating ,"default-rating" , FLT_T,  2.8485);
    OPT(regularization_param_p ,"regularization-param-p", FLT_T,  0.002);
    OPT(regularization_param_q ,"regularization-param-q", FLT_T,  0.002);
    OPT(learning_rate_p ,"learning-rate-p", FLT_T,  0.000002);
    OPT(learning_rate_q ,"learning-rate-q", FLT_T,  0.000002);
    OPT(gradient_descent_iteration_count ,"gradient-descent-iteration-count", INT_T,  100);
    OPT(training_sample_percentage ,"training-sample-percentage", FLT_T,  0.70);
    OPT(max_row_dim ,"max-row-dimension", INT_T,  100000000);
    OPT(max_col_dim ,"max-column-dimension", INT_T,  100000000);
    OPT(csv_input_file_path ,"input-csv-file-path", STRING_T,  STRING_T("unknown_bad.csv"));
    OPT(verbose_mode_level ,"verbose-mode-level", INT_T,  0);
    OPT(loop_mode_count ,"loop-mode-count", INT_T,  1);
    OPT(p_q_matrix_output_file_path, "p-q-matrix-output-file-path", STRING_T,  STRING_T("P_Q_matrix.mtx"));

  }
  catch(exception &e)
  {
    cerr << "\n processInputArgs error: " << e.what() << "\n";
    return false;
  }
  return true;
}

void Sleep() {
  cout << "\n\n\n\n\n\nSleeping for 10 seconds \n";
  system("sleep 10");
}

void printHeader() {
  cout << "\n\n\n=================================================\n";
  cout << "=============== START TRAINING ==================\n";
  cout << "=================================================\n\n";
}

int startMatrixFactorization_Trainer(int argc, char * argv[])
{
  ProgOpts::variables_map vmp; // varmap
  MatrixFactorizationParams params;

  bool b = processInputArgs(argc, argv, vmp, params);
  if(!b)
    return 3;

  printHeader();
  FLT_T rmseSum = 0;
  FLT_T iterationCount = 0;

  for(int i=0; i<params.loop_mode_count; i++) {
    MatrixFactorization trainer(params);
    if(!trainer.startTraining()) {
      cout << "Training failed check input files and training params\n";
      return 4;
    }
    cout << "Trial " << i << " finalRMSE " << trainer.getFinalRMSE()
      << " num iterations " << trainer.getNumIterations() << "\n";
    rmseSum += trainer.getFinalRMSE();
    iterationCount += 1;
    trainer.printPredictedValues(10);
  }
  cout << " avg RMSE " << (rmseSum/iterationCount)
    << " found using "<< iterationCount << " trials \n";
  return 0;
}

int main(int argc, char * argv[])
{
  return startMatrixFactorization_Trainer(argc, argv);
}


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/dls_pnp.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <cmath>
#include <vector>

#include "theia/util/random.h"
#include "theia/sfm/pose/dls_impl.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;
using dls_impl::CreateMacaulayMatrix;
using dls_impl::ExtractJacobianCoefficients;
using dls_impl::LeftMultiplyMatrix;

// This implementation is ported from the Matlab version provided by the authors
// of "A Direct Least-Squares (DLS) Method for PnP". The general approach is to
// first rewrite the reprojection constraint (i.e., cost function) such that all
// unknowns appear linearly in terms of the rotation parameters (which are 3
// parameters in the Cayley-Gibss-Rodriguez formulation). Then we create a
// system of equations from the jacobian of the cost function, and solve these
// equations via a Macaulay matrix to obtain the roots (i.e., the 3 parameters
// of rotation). The translation can then be obtained through back-substitution.
void DlsPnp(const std::vector<Vector2d>& feature_position,
            const std::vector<Vector3d>& world_point,
            std::vector<Quaterniond>* solution_rotation,
            std::vector<Vector3d>* solution_translation) {
  CHECK_GE(feature_position.size(), 3);
  CHECK_EQ(feature_position.size(), world_point.size());

  const int num_correspondences = feature_position.size();

  // Holds the normalized feature positions cross multiplied with itself
  // i.e. n * n^t. This value is used multiple times so it is efficient to
  // pre-compute it.
  std::vector<Matrix3d> normalized_feature_cross;
  normalized_feature_cross.reserve(num_correspondences);
  for (int i = 0; i < num_correspondences; i++) {
    const Vector3d normalized_feature_pos =
        feature_position[i].homogeneous().normalized();
    normalized_feature_cross.push_back(normalized_feature_pos *
                                       normalized_feature_pos.transpose());
  }

  // The bottom-right symmetric block matrix of inverse(A^T * A). Matrix H from
  // Eq. 25 in the Appendix of the DLS paper.
  Matrix3d h_inverse = num_correspondences * Matrix3d::Identity();
  for (int i = 0; i < num_correspondences; i++) {
    h_inverse = h_inverse - normalized_feature_cross[i];
  }
  const Matrix3d h_matrix = h_inverse.inverse();

  // Compute V*W*b with the rotation parameters factored out. This is the
  // translation parameterized by the 9 entries of the rotation matrix.
  Matrix<double, 3, 9> translation_factor = Matrix<double, 3, 9>::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    translation_factor = translation_factor +
                         (normalized_feature_cross[i] - Matrix3d::Identity()) *
                             LeftMultiplyMatrix(world_point[i]);
  }

  translation_factor = h_matrix * translation_factor;

  // Compute the cost function J' of Eq. 17 in DLS paper. This is a factorized
  // version where the rotation matrix parameters have been pulled out. The
  // entries to this equation are the coefficients to the cost function which is
  // a quartic in the rotation parameters.
  Matrix<double, 9, 9> ls_cost_coefficients = Matrix<double, 9, 9>::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    ls_cost_coefficients =
        ls_cost_coefficients +
        (LeftMultiplyMatrix(world_point[i]) + translation_factor).transpose() *
            (Matrix3d::Identity() - normalized_feature_cross[i]) *
            (LeftMultiplyMatrix(world_point[i]) + translation_factor);
  }

  // Extract the coefficients of the jacobian (Eq. 18) from the
  // ls_cost_coefficients matrix. The jacobian represent 3 monomials in the
  // rotation parameters. Each entry of the jacobian will be 0 at the roots of
  // the polynomial, so we can arrange a system of polynomials from these
  // equations.
  double f1_coeff[20];
  double f2_coeff[20];
  double f3_coeff[20];
  ExtractJacobianCoefficients(ls_cost_coefficients, f1_coeff, f2_coeff,
                              f3_coeff);

  // We create one equation with random terms that is generally non-zero at the
  // roots of our system.
  const double macaulay_term[4] = { RandDouble(0.0, 100.0),
                                    RandDouble(0.0, 100.0),
                                    RandDouble(0.0, 100.0),
                                    RandDouble(0.0, 100.0) };

  // Create Macaulay matrix that will be used to solve our polynonomial system.
  const MatrixXd& macaulay_matrix =
      CreateMacaulayMatrix(f1_coeff, f2_coeff, f3_coeff, macaulay_term);

  // Via the Schur complement trick, the top-left of the Macaulay matrix
  // contains a multiplication matrix whose eigenvectors correspond to solutions
  // to our system of equations.
  const MatrixXd solution_polynomial =
      macaulay_matrix.block<27, 27>(0, 0) -
      (macaulay_matrix.block<27, 93>(0, 27) *
       macaulay_matrix.block<93, 93>(27, 27).partialPivLu().solve(
           macaulay_matrix.block<93, 27>(27, 0)));

  // Extract eigenvectors of the solution polynomial to obtain the roots which
  // are contained in the entries of the eigenvectors.
  const Eigen::EigenSolver<MatrixXd> eigen_solver(solution_polynomial);

  // Many of the eigenvectors will contain complex solutions so we must filter
  // them to find the real solutions.
  const auto eigen_vectors = eigen_solver.eigenvectors();
  for (int i = 0; i < 27; i++) {
    // The first entry of the eigenvector should equal 1 according to our
    // polynomial, so we must divide each solution by the first entry.
    std::complex<double> s1 = eigen_vectors(9, i) / eigen_vectors(0, i);
    std::complex<double> s2 = eigen_vectors(3, i) / eigen_vectors(0, i);
    std::complex<double> s3 = eigen_vectors(1, i) / eigen_vectors(0, i);

    // If the rotation solutions are real, treat this as a valid candidate
    // rotation.
    const double kEpsilon = 1e-6;
    if (fabs(s1.imag()) < kEpsilon && fabs(s2.imag()) < kEpsilon &&
        fabs(s3.imag()) < kEpsilon) {
      // Compute the rotation (which is the transpose rotation of our solution)
      // and translation.
      Quaterniond soln_rotation(1.0, s1.real(), s2.real(), s3.real());
      soln_rotation = soln_rotation.inverse().normalized();

      const Matrix3d rot_mat = soln_rotation.inverse().toRotationMatrix();
      const Eigen::Map<const Matrix<double, 9, 1> > rot_vec(rot_mat.data());
      const Vector3d soln_translation = translation_factor * rot_vec;

      // TODO(cmsweeney): evaluate cost function and return it as an output
      // variable.

      // Check that all points are in front of the camera. Discard the solution
      // if this is not the case.
      bool all_points_in_front_of_camera = true;
      for (int j = 0; j < num_correspondences; j++) {
        const Vector3d transformed_point =
            soln_rotation * world_point[j] + soln_translation;
        if (transformed_point.z() < 0) {
          all_points_in_front_of_camera = false;
          break;
        }
      }

      if (all_points_in_front_of_camera) {
        solution_rotation->push_back(soln_rotation);
        solution_translation->push_back(soln_translation);
      }
    }
  }
}

}  // namespace theia


// Copyright 2010-2014, D. E. Shaw Research.
// 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 )

#ifndef BOOST_RANDOM_PHILOX_HPP
#define BOOST_RANDOM_PHILOX_HPP
#include <boost/array.hpp>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/mulhilo.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/range_c.hpp>

namespace boost{
namespace random{

template <unsigned _N, typename Uint>
struct philox_constants{
    // specializations will hold the Mutlipliers: M0, M1
    // and the Weyl constants:  W0, W1
};

template <>
struct philox_constants<2, uint64_t>{
    static const uint64_t M0 = UINT64_C(0xD2B74407B1CE6E93);
    static const uint64_t W0 = UINT64_C(0x9E3779B97F4A7C15);
};

template <>
struct philox_constants<2, uint32_t>{
    static const uint32_t M0 = UINT32_C(0xD256D193);
    static const uint32_t W0 = UINT32_C(0x9E3779B9);
};

template <>
struct philox_constants<4, uint64_t>{
    static const uint64_t M0 = UINT64_C(0xD2E7470EE14C6C93);
    static const uint64_t M1 = UINT64_C(0xCA5A826395121157);
    static const uint64_t W0 = UINT64_C(0x9E3779B97F4A7C15);  /* golden ratio */
    static const uint64_t W1 = UINT64_C(0xBB67AE8584CAA73B);  /* sqrt(3)-1 */
};

template <>
struct philox_constants<4, uint32_t>{
    static const uint32_t M0 = UINT32_C(0xD2511F53);
    static const uint32_t M1 = UINT32_C(0xCD9E8D57);
    static const uint32_t W0 = UINT64_C(0x9E3779B9);  /* golden ratio */
    static const uint32_t W1 = UINT64_C(0xBB67AE85);  /* sqrt(3)-1 */
};

template <unsigned N, typename Uint, unsigned R=10, typename Constants = philox_constants<N, Uint> >
struct philox{
    BOOST_STATIC_ASSERT( N%2 == 0 );
};

template <typename Uint, unsigned R, typename Constants>
struct philox<2, Uint, R, Constants> {
    typedef array<Uint, 2> domain_type;
    typedef array<Uint, 2> range_type;
    typedef array<Uint, 1> key_type ;

    philox() : k(){}
    philox(key_type _k) : k(_k) {}
    philox(const philox& v) : k(v.k){}

    void setkey(key_type _k){
        k = _k;
    }

    key_type getkey() const{
        return k;
    }

    bool operator==(const philox& rhs) const{
        return k == rhs.k;
    }

    bool operator!=(const philox& rhs) const{
        return k != rhs.k;
    }

    range_type operator()(domain_type c){
        key_type kcopy = k;
#if 0   // using mpl to unroll the loop doesn't seem to help much.
        _roundapplyer ra(c, kcopy);
        mpl::for_each<mpl::range_c<unsigned, 0, R> >(ra);
#else
        for(unsigned r=0; r<R; ++r)
            round(c, kcopy);
#endif
        return c;
    }
protected:
    static inline void round(domain_type& ctr, key_type& key){
        Uint hi;
        Uint lo = detail::mulhilo(Constants::M0, ctr[0], hi);
        domain_type out = {{hi^key[0]^ctr[1], lo}};
        ctr = out;
        key[0] += Constants::W0;
    }

    struct _roundapplyer{
        domain_type& c;
        key_type& k;
        _roundapplyer(domain_type& _c, key_type& _k): c(_c), k(_k){}
        void operator()(unsigned){ round(c, k); }
    };
    key_type k;
};

template<typename Uint, unsigned R, typename Constants>
struct philox<4, Uint, R, Constants> {
    typedef array<Uint, 4> domain_type;
    typedef array<Uint, 4> range_type;
    typedef array<Uint, 2> key_type ;
public:

    philox() : k(){}
    philox(key_type _k) : k(_k) {}
    philox(const philox& v) : k(v.k){}

    void setkey(key_type _k){
        k = _k;
    }

    key_type getkey() const{
        return k;
    }

    bool operator==(const philox& rhs) const{
        return k == rhs.k;
    }

    bool operator!=(const philox& rhs) const{
        return k != rhs.k;
    }

    range_type operator()(domain_type c){
        key_type kcopy = k;
#if 0   // using mpl to unroll the loop doesn't seem to help much.
        _roundapplyer ra(c, kcopy);
        mpl::for_each<mpl::range_c<unsigned, 0, R> >(ra);
#else
        for(unsigned r=0; r<R; ++r)
            round(c, kcopy);
#endif
        return c;
    }

protected:
    static inline void round(domain_type& ctr, key_type& key){
        Uint hi0;
        Uint hi1;
        Uint lo0 = detail::mulhilo(Constants::M0, ctr[0], hi0);
        Uint lo1 = detail::mulhilo(Constants::M1, ctr[2], hi1);
        domain_type out = {{hi1^ctr[1]^key[0], lo1,
                                      hi0^ctr[3]^key[1], lo0}};
        ctr = out;
        key[0] += Constants::W0;
        key[1] += Constants::W1;
    }

    struct _roundapplyer{
        domain_type& c;
        key_type& k;
        _roundapplyer(domain_type& _c, key_type& _k): c(_c), k(_k){}
        void operator()(unsigned){ round(c, k); }
    };

    key_type k;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_PHILOX_HPP