/***************************************************************************
 *   Copyright (C) 2007 by Reed A. Cartwright                              *
 *   reed@scit.us                                                          *
 *                                                                         *
 *   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 BE LIABLE FOR ANY CLAIM, DAMAGES OR     *
 *   OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, *
 *   ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR *
 *   OTHER DEALINGS IN THE SOFTWARE.                                       *
 ***************************************************************************/

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

#include "models.h"

inline double zeta(double z) { return boost::math::zeta<double>(z); }

extern const int g_pupy[] = {0,1,0,1,-1};
extern const int g_atgc[] = {0,1,1,0,-1};

const model_k2p_zeta::freq_type model_k2p_zeta::k2p_freqs = {0.25,0.25,0.25,0.25};
const model_k2p_geo::freq_type model_k2p_geo::k2p_freqs = {0.25,0.25,0.25,0.25};

/***************************************************************************
 * class model_k2p_zeta                                                    *
 ***************************************************************************/

void model_k2p_zeta::preallocate(size_t maxa, size_t maxd)
{
	size_t sz_max = std::max(maxa, maxd)+1;
	p_indel_size.resize(sz_max, 0.0);
}

void model_k2p_zeta::expectation_setup(const params_type &params, const freq_type &freq)
{
	p_ts = 0.25-0.5*exp(-params[pT]*(2.0*params[pK]+1.0)/(params[pK]+1.0))
			+ 0.25*exp(-2.0*params[pT]/(params[pK]+1.0));
	p_tv = 0.5-0.5*exp(-2.0*params[pT]/(params[pK]+1.0));
	p_match = 1.0-p_ts-p_tv;

	p_end = 1.0/(params[pA]+1.0);
	p_2h =  (1.0-p_end)*exp(-2.0*params[pR]*params[pT]);
	p_2g =  (1.0-p_end)*0.5*(1.0-exp(-2.0*params[pR]*params[pT]));

	double nuc_scale2 = 0.25*p_2h
		*pow(p_match, p_match)
		*pow(p_ts, p_ts)
		*pow(0.5*p_tv, p_tv);
	nuc_scale = sqrt(nuc_scale2);
	amb_scale = 4.0;

	for(size_t i=0;i<nN;++i)
	{
		p_substitution[i][i] = 0.25*p_2h*p_match/nuc_scale2;

		for(size_t j=i+1;j<nN;++j)
		{
			p_substitution[j][i] = p_substitution[i][j] = p_2h*
				((g_pupy[i] == g_pupy[j]) ? 0.25*p_ts : 0.125*p_tv)
				/ nuc_scale2;
		}
		p_substitution[nN][i] = p_substitution[i][nN] = 0.0625*p_2h / nuc_scale2;
	}
	p_substitution[nN][nN] = 0.0625*p_2h / nuc_scale2;

	double z = params[pZ];
	double Z = p_2g/zeta(z);
	double s = 0.25/nuc_scale;
	double ss = s;
	for(size_t u = 1; u < p_indel_size.size(); ++u)
	{
		p_indel_size[u] = pow((double)u, -z)*Z*ss;
		ss *= s;
	}
}

/***************************************************************************
 * class model_k2p_geo                                                     *
 ***************************************************************************/

void model_k2p_geo::preallocate(size_t maxa, size_t maxd)
{
	size_t sz_max = std::max(maxa, maxd)+1;
	p_indel_size.resize(sz_max, 0.0);
}

void model_k2p_geo::expectation_setup(const params_type &params, const freq_type &freq)
{
	p_ts = 0.25-0.5*exp(-params[pT]*(2.0*params[pK]+1.0)/(params[pK]+1.0))
			+ 0.25*exp(-2.0*params[pT]/(params[pK]+1.0));
	p_tv = 0.5-0.5*exp(-2.0*params[pT]/(params[pK]+1.0));
	p_match = 1.0-p_ts-p_tv;

	p_end = 1.0/(params[pA]+1.0);
	p_2h =  (1.0-p_end)*exp(-2.0*params[pR]*params[pT]);
	p_2g =  (1.0-p_end)*0.5*(1.0-exp(-2.0*params[pR]*params[pT]));

	double nuc_scale2 = 0.25*p_2h
		*pow(p_match, p_match)
		*pow(p_ts, p_ts)
		*pow(0.5*p_tv, p_tv);
	nuc_scale = sqrt(nuc_scale2);
	amb_scale = 4.0;

	for(size_t i=0;i<nN;++i)
	{
		p_substitution[i][i] = 0.25*p_2h*p_match/nuc_scale2;

		for(size_t j=i+1;j<nN;++j)
		{
			p_substitution[j][i] = p_substitution[i][j] = p_2h*
				((g_pupy[i] == g_pupy[j]) ? 0.25*p_ts : 0.125*p_tv)
				/ nuc_scale2;
		}
		p_substitution[nN][i] = p_substitution[i][nN] = 0.0625*p_2h / nuc_scale2;
	}
	p_substitution[nN][nN] = 0.0625*p_2h / nuc_scale2;

	double s = 0.25/nuc_scale;
	double qq = 1.0/params[pQ];
	double dp = qq*s;
	for(size_t u = 1; u < p_indel_size.size(); ++u)
	{
		p_indel_size[u] = p_2g*dp;
		dp *= (1.0-qq)*s;
	}
	p_open = qq*p_2g*s;
	p_extend = (1.0-qq)*s;
}


#include <qlo/objects/obj_credit.hpp>

#include <qlo/enumerations/factories/termstructuresfactory.hpp>

#include <ql/instruments/stock.hpp>
#include <ql/quote.hpp>
#include <ql/currencies/europe.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/credit/interpolatedhazardratecurve.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/credit/piecewisedefaultcurve.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/bicubicsplineinterpolation.hpp>
#include <ql/math/solver1d.hpp>
#include <ql/math/solvers1d/brent.hpp>

#include <ql/pricingengines/credit/midpointcdsengine.hpp>

#include <ql/instruments/creditdefaultswap.hpp>
#include <ql/experimental/credit/riskybond.hpp>
#include <ql/experimental/credit/syntheticcdo.hpp>
#include <ql/experimental/credit/midpointcdoengine.hpp>
#include <ql/experimental/credit/nthtodefault.hpp>
#include <ql/experimental/credit/integralntdengine.hpp>
#include <ql/experimental/credit/basecorrelationstructure.hpp>
#include <ql/experimental/credit/cdsoption.hpp>
#include <ql/experimental/credit/blackcdsoptionengine.hpp>

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/make_shared.hpp>

#include <ql/settings.hpp>

QuantLibAddin::CreditDefaultSwap::CreditDefaultSwap(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            QuantLib::Protection::Side BuyerSeller,
            QuantLib::Real Notional,
            QuantLib::Rate Upfront,
            QuantLib::Rate Spread,
            QuantLib::Schedule const &PremiumSchedule,
            QuantLib::BusinessDayConvention PaymentConvention,
            QuantLib::DayCounter const &DayCounter,
            bool SettlesAccrual,
            bool PayAtDefault,
            QuantLib::Date const &ProtectionStart,
            QuantLib::Date const &UpfrontDate,
            // END   typemap rp_tm_default
    bool permanent)
: Instrument(properties, permanent) {
		// dirty way to decide if this is constructed through a run only version
        if(UpfrontDate == QuantLib::Null<QuantLib::Date>() && Upfront == 0.) {
            libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
                new QuantLib::CreditDefaultSwap(
                    BuyerSeller,
                    Notional,
                    Spread,
                    PremiumSchedule,
                    PaymentConvention,
                    DayCounter,
                    SettlesAccrual,
                    PayAtDefault,
                    ProtectionStart,
                    boost::shared_ptr<QuantLib::Claim>()));
        }else{
                libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
                    new QuantLib::CreditDefaultSwap(
                        BuyerSeller,
                        Notional,
                        Upfront,
                        Spread,
                        PremiumSchedule,
                        PaymentConvention,
                        DayCounter,
                        SettlesAccrual,
                        PayAtDefault,
                        ProtectionStart,
                        UpfrontDate,
                        boost::shared_ptr<QuantLib::Claim>()));
        }
}

QuantLibAddin::HazardRateCurve::HazardRateCurve(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            std::vector< QuantLib::Date > const &CurveDates,
            std::vector< QuantLib::Rate > const &CurveRates,
            QuantLib::DayCounter const &DayCounter,
            // END   typemap rp_tm_default
    bool permanent)
: DefaultProbabilityTermStructure(properties, permanent) {
        QL_REQUIRE(!CurveDates.empty(), "no input dates given");
        QL_REQUIRE(CurveDates.size() == CurveRates.size(),
                   "vector sizes differ");
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(
        new QuantLib::InterpolatedHazardRateCurve<QuantLib::BackwardFlat>(
 				 CurveDates, CurveRates, DayCounter));
}

QuantLibAddin::PiecewiseHazardRateCurve::PiecewiseHazardRateCurve(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            std::vector< boost::shared_ptr< QuantLib::DefaultProbabilityHelper > > const &Helpers,
            QuantLib::DayCounter const &DayCounter,
            QuantLib::Calendar const &Calendar,
            std::string const &Interpolation,
            QuantLib::Real Accuracy,
            // END   typemap rp_tm_default
    bool permanent)
: DefaultProbabilityTermStructure(properties, permanent) {
        if(Interpolation == std::string("LINEAR")){
            libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
                   QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,
                        QuantLib::Linear>(
                            0,
                            Calendar,
                            Helpers,
                            DayCounter));
        }else if(Interpolation == std::string("BACKWARDFLAT")) {
            libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
                   QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,
                        QuantLib::BackwardFlat>(
                            0,
                            Calendar,
                            Helpers,
                            DayCounter));
        }else{
            QL_FAIL("Unrecognised interpolator");
        }

        libraryObject_->enableExtrapolation();
}

const std::vector<QuantLib::Date>& QuantLibAddin::PiecewiseHazardRateCurve::dates() const {
    typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::BackwardFlat> flat_curve;
    typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::Linear> lin_curve;
    boost::shared_ptr<flat_curve> ptrBF =
        boost::dynamic_pointer_cast<flat_curve>(libraryObject_);
    if(ptrBF) return ptrBF->dates();
    boost::shared_ptr<lin_curve> ptrLIN =
        boost::dynamic_pointer_cast<lin_curve>(libraryObject_);
    if(ptrLIN) return ptrLIN->dates();
    QL_FAIL("Unable to cast default probability term structure.");
}

const std::vector<QuantLib::Real>& QuantLibAddin::PiecewiseHazardRateCurve::data() const {
    typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::BackwardFlat> flat_curve;
    typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::Linear> lin_curve;
    boost::shared_ptr<flat_curve> ptrBF =
        boost::dynamic_pointer_cast<flat_curve>(libraryObject_);
    if(ptrBF) return ptrBF->data();
    boost::shared_ptr<lin_curve> ptrLIN =
        boost::dynamic_pointer_cast<lin_curve>(libraryObject_);
    if(ptrLIN) return ptrLIN->data();
    QL_FAIL("Unable to cast default probability term structure.");
}

QuantLibAddin::PiecewiseFlatForwardCurve::PiecewiseFlatForwardCurve(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            QuantLib::Date const &ReferenceDate,
            std::vector< boost::shared_ptr< QuantLib::RateHelper > > const &RateHelpers,
            QuantLib::DayCounter const &DayCounter,
            QuantLib::Real Accuracy,
            // END   typemap rp_tm_default
    bool permanent)
: YieldTermStructure(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
               QuantLib::PiecewiseYieldCurve<QuantLib::Discount,QuantLib::LogLinear>(ReferenceDate, RateHelpers, DayCounter));
}

QuantLibAddin::RiskyFixedBond::RiskyFixedBond(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            std::string Bondname,
            QuantLib::Currency Currency,
            QuantLib::Real Recovery,
            QuantLib::Handle< QuantLib::DefaultProbabilityTermStructure > const &DefaultCurve,
            QuantLib::Schedule const &Schedule,
            QuantLib::Real Rate,
            QuantLib::DayCounter DayCounter,
            QuantLib::BusinessDayConvention PaymentConvention,
            QuantLib::Real Notional,
            QuantLib::Handle< QuantLib::YieldTermStructure > const &DiscountingCurve,
            QuantLib::Date PricingDate,
            // END   typemap rp_tm_default
    bool permanent)
: Instrument(properties, permanent) {
        std::vector<QuantLib::Real> notionals(1,Notional);

        libraryObject_ = boost::shared_ptr<QuantLib::RiskyFixedBond>(
            new QuantLib::RiskyFixedBond(
                    Bondname,Currency,Recovery,DefaultCurve,Schedule,Rate,DayCounter,
                    PaymentConvention,notionals,DiscountingCurve///, PricingDate
                                       ));
}

QuantLibAddin::Issuer::Issuer(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            boost::shared_ptr< QuantLib::DefaultProbabilityTermStructure > const &DefaultCurves,
            boost::shared_ptr< QuantLib::DefaultEventSet > const &DefaultEvents,
            // END   typemap rp_tm_default
    bool permanent)
: reposit::LibraryObject<QuantLib::Issuer>(properties, permanent) {
        std::vector<QuantLib::Issuer::key_curve_pair> curves(1, std::make_pair(
            QuantLib::NorthAmericaCorpDefaultKey(QuantLib::EURCurrency(),
                                                     QuantLib::SeniorSec,
                                                     QuantLib::Period(),
                                                     1. // amount threshold
                                                     ),
            QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure>(DefaultCurves)
        ));
        libraryObject_ = boost::shared_ptr<QuantLib::Issuer>(new QuantLib::Issuer(curves, *DefaultEvents));
}

QuantLibAddin::DefaultEventSet::DefaultEventSet(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            std::string const &EventType,
            QuantLib::Date const &EventDate,
            QuantLib::Currency const &Currency,
            QuantLib::Seniority Seniority,
            QuantLib::Date const &SettlementDate,
            QuantLib::Real SettledRecovery,
            // END   typemap rp_tm_default
    bool permanent)
: reposit::LibraryObject<QuantLib::DefaultEventSet>(properties, permanent) {
        // if no match return empty set
        libraryObject_ = boost::shared_ptr<QuantLib::DefaultEventSet>(new QuantLib::DefaultEventSet());

        // only one recovery parsed by now; bankruptcy events need the whole
        //  set or they fail to construct.
        std::map<QuantLib::Seniority, QuantLib::Real> rrs;
        rrs.insert(std::pair<QuantLib::Seniority, QuantLib::Real>(Seniority, SettledRecovery));
        if(EventType==std::string("FailureToPayEvent")) {
            libraryObject_->insert(boost::shared_ptr<QuantLib::FailureToPayEvent> (
                new QuantLib::FailureToPayEvent(EventDate, Currency, Seniority, 1.e7,
                //implSettlemt,
                SettlementDate,
                rrs)));
        }else if(EventType==std::string("BankruptcyEvent")){
            libraryObject_->insert(boost::shared_ptr<QuantLib::BankruptcyEvent> (
                new QuantLib::BankruptcyEvent(EventDate, Currency, Seniority,
                SettlementDate,
                rrs)));
        }
}

QuantLibAddin::BaseCorrelationTermStructure::BaseCorrelationTermStructure(
    const boost::shared_ptr<reposit::ValueObject>& properties,
            // BEGIN typemap rp_tm_default
            std::string const &InterpolatorType,
            QuantLib::Natural SettlementDays,
            QuantLib::Calendar const &Calendar,
            QuantLib::BusinessDayConvention Convention,
            std::vector< QuantLib::Period > const &Tenors,
            std::vector< QuantLib::Real > const &LossLevel,
            std::vector< std::vector< QuantLib::Handle< QuantLib::Quote > > > const &Correlations,
            QuantLib::DayCounter const &DayCounter,
            // END   typemap rp_tm_default
    bool permanent)
: CorrelationTermStructure(properties, permanent) {
        // select 2D interpolator:
        //  Another option is to write one class per interpolator.
        if(InterpolatorType == std::string("BILIN")) {
            libraryObject_ = boost::shared_ptr<QuantLib::BaseCorrelationTermStructure<QuantLib::BilinearInterpolation> >(new
                QuantLib::BaseCorrelationTermStructure<QuantLib::BilinearInterpolation>(SettlementDays, Calendar, Convention, Tenors, LossLevel, Correlations, DayCounter));
        //}else if(InterpolatorType == std::string("")) {
        }else if(InterpolatorType == std::string("BICUBIC")) {
            libraryObject_ = boost::shared_ptr<QuantLib::BaseCorrelationTermStructure<QuantLib::BicubicSpline> >(new
                QuantLib::BaseCorrelationTermStructure<QuantLib::BicubicSpline>(SettlementDays, Calendar, Convention, Tenors, LossLevel, Correlations, DayCounter));
        }else{
            QL_FAIL("Can't determine base correlation surface interpolator.");
        }
}

QuantLib::Real QuantLibAddin::BaseCorrelationTermStructure::correlation(const QuantLib::Date& d, QuantLib::Real lossLevel) {
    if(interpolType_ == std::string("BILIN")) {
        return boost::dynamic_pointer_cast<QuantLib::BaseCorrelationTermStructure<QuantLib::BilinearInterpolation> >
            (libraryObject_)->correlation(d, lossLevel);
    }else if(interpolType_ == std::string("BICUBIC")) {
        return boost::dynamic_pointer_cast<QuantLib::BaseCorrelationTermStructure<QuantLib::BicubicSpline> >
            (libraryObject_)->correlation(d, lossLevel);
    }else{
        QL_FAIL("unknown 2D interpolator");
    }
}


// Copyright (C) 2019 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)

// This file was created by Steffen Urban (urbste@googlemail.com) or
// company address (steffen.urban@zeiss.com)
// December 2018

#include <glog/logging.h>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>
#include <random>

#include "theia/sfm/pose/four_point_focal_length_radial_distortion.h"
#include "theia/sfm/pose/four_point_focal_length_radial_distortion_helper.h"
#include "theia/util/random.h"

namespace theia {

double sgn(double val) { return (0.0 < val) - (val < 0.0); }

using Matrix34d = Eigen::Matrix<double, 3, 4>;
using Matrix24d = Eigen::Matrix<double, 2, 4>;
using Matrix42d = Eigen::Matrix<double, 4, 2>;
using Vector8d  = Eigen::Matrix<double, 8, 1>;
using Vector5d  = Eigen::Matrix<double, 5, 1>;
using Eigen::Map;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::Matrix4d;
using Eigen::Matrix3d;
using Eigen::MatrixXd;
using Eigen::Matrix;

bool FourPointsPoseFocalLengthRadialDistortion(
    const std::vector<Eigen::Vector2d>& feature_vectors,
    const std::vector<Eigen::Vector3d>& world_points,
    std::vector<Eigen::Matrix3d>* rotations,
    std::vector<Eigen::Vector3d>* translations,
    std::vector<double>* radial_distortions,
    std::vector<double>* focal_lengths) {
  // check that input size of features and world points is 4
  CHECK_GE(feature_vectors.size(), 4);
  CHECK_EQ(feature_vectors.size(), world_points.size());

  Vector4d d;
  Matrix34d world_points_;
  Matrix34d u;  // image points. Will be normalized.
  for (int i = 0; i < 4; ++i) {
    d[i] = feature_vectors[i].squaredNorm();
    world_points_.col(i) = world_points[i];
    u.col(i) = feature_vectors[i].homogeneous();
  }

  const Vector3d t0 = world_points_.rowwise().mean();
  Matrix34d t0_mat;
  t0_mat << t0, t0, t0, t0;

  Matrix4d U;
  U.topRows<3>() = world_points_ - t0_mat;
  U.bottomRows<1>() << 1.0, 1.0, 1.0, 1.0;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      U.topRows<3>(), Eigen::ComputeFullV | Eigen::ComputeFullU);
  Matrix3d R0 = svd.matrixU();

  if (sgn(R0.determinant()) < 0.0) {
    R0.col(0) *= -1;
  }
  R0.transposeInPlace();

  U.topRows<3>() = R0 * U.topRows<3>();
  const double scale =
      U.topRows<3>().array().pow(2).colwise().sum().sqrt().mean();

  U.topRows<3>() /= scale;

  // rescale image points
  const double f0 = u.topRows<2>().array().pow(2).colwise().sum().sqrt().mean();
  u.topRows<2>() /= f0;

  const double k0 = d.array().mean();
  d /= k0;

  Matrix<double, 5, 8> M;
  M.fill(0.0);
  M.row(0).leftCols<4>() = U.col(0);
  M.row(1).rightCols<4>() = U.col(0);
  for (int k = 1; k < 4; ++k) {
    M.row(k + 1).leftCols<4>() = u.row(1).col(k) * U.col(k).transpose();
    M.row(k + 1).rightCols<4>() = -u.row(0).col(k) * U.col(k).transpose();
  }

  Vector5d b;
  b.fill(0.0);
  b.topRows<2>() = u.col(0).topRows<2>();
  Eigen::HouseholderQR<Eigen::MatrixXd> qr;
  qr.compute(M.transpose());

  Matrix<double, 8, 8> Q = qr.householderQ();
  Matrix<double, 8, 5> R = qr.matrixQR().triangularView<Eigen::Upper>();

  Matrix<double, 8, 4> N;
  N.fill(0.0);
  N.leftCols<3>() = Q.rightCols<3>();

  // this random rotation is supposed to make the solver more stable
  static RandomNumberGenerator random_number_gen(42);
  Vector3d rot_vec(random_number_gen.RandDouble(-0.5, 0.5),
                   random_number_gen.RandDouble(-0.5, 0.5),
                   random_number_gen.RandDouble(-0.5, 0.5));
  Eigen::AngleAxisd random_rot(rot_vec.norm(), rot_vec);
  N.leftCols<3>() *= random_rot.toRotationMatrix();
  Matrix<double, 8, 1> x0 =
      Q.leftCols<5>() * (R.topRows<5>().transpose().fullPivLu().solve(b));
  N.rightCols<1>() = x0;

  Matrix<double, 6, 3> C;
  C.fill(0.0);
  Matrix34d UN1 = U.rightCols<3>().transpose() * N.topRows<4>();
  Matrix34d UN2 = U.rightCols<3>().transpose() * N.bottomRows<4>();
  Matrix<double, 6, 9> B;
  B.fill(0.0);

  B.topLeftCorner<3, 3>() = UN1.leftCols<3>();
  B.bottomLeftCorner<3, 3>() = UN2.leftCols<3>();
  B.topRightCorner<3, 1>() = UN1.rightCols<1>();
  B.bottomRightCorner<3, 1>() = UN2.rightCols<1>();

  B.block<3, 4>(0, 3) =
      d.bottomRows<3>().transpose().replicate(4, 1).transpose().cwiseProduct(
          UN1);
  B.block<3, 4>(3, 3) =
      d.bottomRows<3>().transpose().replicate(4, 1).transpose().cwiseProduct(
          UN2);

  B.col(7).topRows<3>() = -u.row(0).rightCols<3>().transpose().cwiseProduct(
      U.row(2).rightCols<3>().transpose());
  B.col(7).bottomRows<3>() = -u.row(1).rightCols<3>().transpose().cwiseProduct(
      U.row(2).rightCols<3>().transpose());

  // fill these guys
  Matrix3d Utmp;
  Utmp.row(0) = U.row(0).rightCols<3>();
  Utmp.row(1) = U.row(1).rightCols<3>();
  Utmp.row(2) = U.row(3).rightCols<3>();

  Matrix3d u1temp;
  u1temp.row(0) = u.row(0).rightCols<3>();
  u1temp.row(1) = u.row(0).rightCols<3>();
  u1temp.row(2) = u.row(0).rightCols<3>();
  Matrix3d u2temp;
  u2temp.row(0) = u.row(1).rightCols<3>();
  u2temp.row(1) = u.row(1).rightCols<3>();
  u2temp.row(2) = u.row(1).rightCols<3>();

  C.block<3, 3>(0, 0) = Utmp.transpose().cwiseProduct(u1temp.transpose());
  C.block<3, 3>(3, 0) = Utmp.transpose().cwiseProduct(u2temp.transpose());

  Matrix<double, 3, 9> D = C.colPivHouseholderQr().solve(B);
  Map<Eigen::RowVectorXd> N_(N.data(), N.size());
  Map<Eigen::RowVectorXd> D_(D.data(), D.size());

  Matrix<double, 64, 1> data;
  data(0) = 0.0;  // used to keep matlab indices, just an index offset of 1
  data.block<32, 1>(1, 0) = N_;
  data.block<27, 1>(33, 0) = D_;
  data(60, 0) = d(0, 0);
  data.bottomRows<3>() = U.topRows<3>().col(0);

  std::vector<Vector5d> valid_solutions;
  FourPointsPoseFocalLengthRadialDistortionSolver(data, &valid_solutions);

  rotations->resize(valid_solutions.size());
  translations->resize(valid_solutions.size());
  radial_distortions->resize(valid_solutions.size());
  focal_lengths->resize(valid_solutions.size());

  for (int i = 0; i < valid_solutions.size(); ++i) {
    const double k = valid_solutions[i][3];
    const double P33 = valid_solutions[i][4];
    Eigen::Vector4d alpha(valid_solutions[i][0], valid_solutions[i][1],
                          valid_solutions[i][2], 1.0);

    Vector8d P12_ = N * alpha;
    Map<Matrix42d> P12(P12_.data(), 4, 2);

    Matrix<double, 9, 1> tmp;
    tmp(0, 0) = alpha[0];
    tmp(1, 0) = alpha[1];
    tmp(2, 0) = alpha[2];
    tmp(3, 0) = k * alpha(0);
    tmp(4, 0) = k * alpha(1);
    tmp(5, 0) = k * alpha(2);
    tmp(6, 0) = k;
    tmp(7, 0) = P33;
    tmp(8, 0) = 1.0;

    const Vector3d P3_124 = D * tmp;

    Vector4d P3;
    P3(0) = P3_124(0);
    P3(1) = P3_124(1);
    P3(2) = P33;
    P3(3) = P3_124(2);

    Matrix34d P;
    P.topRows<2>() = P12.transpose();
    P.bottomRows<1>() = P3;
    P /= P.bottomRows<1>().leftCols<3>().norm();
    const double f = P.topRows<1>().leftCols<3>().norm();
    Matrix3d K = Matrix3d::Identity();
    K(0, 0) = 1. / f;
    K(1, 1) = 1. / f;

    (*focal_lengths)[i] = f * f0;
    (*radial_distortions)[i] = k / k0;

    Matrix34d Rt = K * P;

    if (Rt.topLeftCorner<3, 3>().determinant() < 0.0) Rt *= -1.0;

    // scale radial distortion
    // radial_distortions[i] *= (focal_lengths[i]*focal_lengths[i]);

    Rt.col(3) = Rt.col(3) * scale - Rt.topLeftCorner<3, 3>() * R0 * t0;
    Rt.topLeftCorner<3, 3>() = Rt.topLeftCorner<3, 3>() * R0;

    (*rotations)[i] = Rt.topLeftCorner<3, 3>();
    (*translations)[i] = Rt.col(3);
  }

  return valid_solutions.size() > 0;
}
}


#include <vector>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <boost/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>

#include "../../src/edmonds_optimum_branching.hpp"

// Define a directed graph type that associates a weight with each
// edge. We store the weights using internal properties as described
// in BGL.
typedef boost::property<boost::edge_weight_t, double>       EdgeProperty;
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::directedS,
                              boost::no_property,
                              EdgeProperty>                 Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor       Vertex;
typedef boost::graph_traits<Graph>::edge_descriptor         Edge;

int
main(int argc, char *argv[])
{
    const int N = 4;

    // Graph with N vertices
    Graph G(N);

    // Create a vector to keep track of all the vertices and enable us
    // to index them. As a side note, observe that this is not
    // necessary since Vertex is probably an integral type. However,
    // this may not be true of arbitrary graphs and I think this code
    // is a better illustration of a more general case.
    std::vector<Vertex> the_vertices;
    BOOST_FOREACH (Vertex v, vertices(G))
    {
        the_vertices.push_back(v);
    }

    // add a few edges with weights to the graph
    add_edge(the_vertices[0], the_vertices[1], 3.0, G);
    add_edge(the_vertices[0], the_vertices[2], 1.5, G);
    add_edge(the_vertices[0], the_vertices[3], 1.8, G);
    add_edge(the_vertices[1], the_vertices[2], 4.3, G);
    add_edge(the_vertices[2], the_vertices[3], 2.2, G);

    // This is how we can get a property map that gives the weights of
    // the edges.
    boost::property_map<Graph, boost::edge_weight_t>::type weights =
        get(boost::edge_weight_t(), G);

    // This is how we can get a property map mapping the vertices to
    // integer indices.
    boost::property_map<Graph, boost::vertex_index_t>::type vertex_indices =
        get(boost::vertex_index_t(), G);

    // Print the graph (or rather the edges of the graph).
    std::cout << "This is the graph:\n";
    BOOST_FOREACH (Edge e, edges(G))
    {
        std::cout << "(" << boost::source(e, G) << ", "
                  << boost::target(e, G) << ")\t"
                  << get(weights, e) << "\n";
    }

    // Find the maximum branching.
    std::vector<Edge> branching;
    edmonds_optimum_branching<true, false, false>(G,
                                                  vertex_indices,
                                                  weights,
                                                  static_cast<Vertex *>(0),
                                                  static_cast<Vertex *>(0),
                                                  std::back_inserter(branching));

    // Print the edges of the maximum branching
    std::cout << "This is the maximum branching\n";
    BOOST_FOREACH (Edge e, branching)
    {
        std::cout << "(" << boost::source(e, G) << ", "
                  << boost::target(e, G) << ")\t"
                  << get(weights, e) << "\n";
    }
    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft,
//                          University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_NUMDIFF_STATE_HPP_
#define CROCODDYL_CORE_NUMDIFF_STATE_HPP_

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include "crocoddyl/core/state-base.hpp"

namespace crocoddyl {

class StateNumDiff : public StateAbstract {
 public:
  explicit StateNumDiff(boost::shared_ptr<StateAbstract> state);
  ~StateNumDiff();

  Eigen::VectorXd zero() const;
  Eigen::VectorXd rand() const;
  void diff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
            Eigen::Ref<Eigen::VectorXd> dxout) const;
  void integrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
                 Eigen::Ref<Eigen::VectorXd> xout) const;
  /**
   * @brief This computes the Jacobian of the diff method by finite
   * differentiation:
   * \f{equation}{
   *    Jfirst[:,k] = diff(int(x_1, dx_dist), x_2) - diff(x_1, x_2)/disturbance
   * \f}
   * and
   * \f{equation}{
   *    Jsecond[:,k] = diff(x_1, int(x_2, dx_dist)) - diff(x_1, x_2)/disturbance
   * \f}
   *
   * @param Jfirst
   * @param Jsecond
   * @param firstsecond
   */
  void Jdiff(const Eigen::Ref<const Eigen::VectorXd>& x0, const Eigen::Ref<const Eigen::VectorXd>& x1,
             Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
             Jcomponent firstsecond = both) const;
  /**
   * @brief This computes the Jacobian of the integrate method by finite
   * differentiation:
   * \f{equation}{
   *    Jfirst[:,k] = diff( int(x, d_x), int( int(x, dx_dist), dx) )/disturbance
   * \f}
   * and
   * \f{equation}{
   *    Jsecond[:,k] = diff( int(x, d_x), int( x, dx + dx_dist) )/disturbance
   * \f}
   *
   * @param Jfirst
   * @param Jsecond
   * @param firstsecond
   */
  void Jintegrate(const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& dx,
                  Eigen::Ref<Eigen::MatrixXd> Jfirst, Eigen::Ref<Eigen::MatrixXd> Jsecond,
                  Jcomponent firstsecond = both) const;
  const double& get_disturbance() const;
  void set_disturbance(const double& disturbance);

 private:
  /**
   * @brief This is the state we need to compute the numerical differentiation
   * from.
   */
  boost::shared_ptr<StateAbstract> state_;
  /**
   * @brief This the increment used in the finite differentiation and integration.
   */
  double disturbance_;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_NUMDIFF_STATE_HPP_