#include <cmath>

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

#include <TList.h>

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

using namespace boost::numeric;

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

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

enum cham_group_t {prop_2nd, drift_left, drift_right};

double  Psi_L = 1.0447;
double  Psi_R = -1.0209;

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

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

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

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

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

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

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

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

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

	return track3d_t({a, b});
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

		if (!incident)
		{
			continue;
		}

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

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

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

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

		events_new->Fill();
	}

	return events_new;
}


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

#include <cmath>

#include <Eigen/Core>

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

namespace tudat
{
namespace basic_mathematics
{

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

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

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

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

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

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

} // namespace basic_mathematics
} // namespace tudat


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include "test_utils/sample_graph.hpp"
#include "test_utils/logger.hpp"

#include "paal/local_search/k_median/k_median.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(kmedian_test) {
    // sample data
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();

    // define voronoi and solution
    const int k = 2;
    typedef paal::data_structures::voronoi<decltype(gm)> VorType;
    typedef paal::data_structures::k_median_solution<VorType> Sol;
    typedef paal::data_structures::voronoi_traits<VorType> VT;
    typedef typename VT::GeneratorsSet GSet;
    typedef typename VT::VerticesSet VSet;
    typedef typename Sol::UnchosenFacilitiesSet USet;

    // create voronoi and solution
    VorType voronoi(GSet{ SGM::B, SGM::D },
                    VSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E }, gm);
    Sol sol(std::move(voronoi), USet{ SGM::A, SGM::C }, k);

    // create facility location local search components
    paal::local_search::default_k_median_components swap;

    // search
    paal::local_search::facility_location_first_improving(sol, swap);

    // print result
    ON_LOG(auto const &ch = ) sol.get_chosen_facilities();
    LOGLN("Solution:");
    LOG_COPY_RANGE_DEL(ch, ",");
}


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

#ifndef ITL_CGS_INCLUDE
#define ITL_CGS_INCLUDE

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

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

namespace itl {

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

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

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

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

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

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

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

	rho_2= rho_1;
    }
    return iter;
}

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

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

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

  private:
    Preconditioner        L;
};

} // namespace itl

#endif // ITL_CGS_INCLUDE


#pragma once

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    unsigned getNPoints() const
    {
      return d_nPoints;
    }

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

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

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

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

    bool d_dirty;
  };
}


#include <omp.h>
#include <Eigen/Sparse>
#include <boost/lexical_cast.hpp>
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <functional>
#include <iomanip>
#include <iostream>
// own includes ------------------------------------------------------------
#include "aux/message.hpp"
#include "aux/timer.hpp"
//#include "base/numbers.hpp"

#include <mpi.h>

#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "collision_tensor/collision_tensor_galerkin.hpp"

using namespace std;
using namespace boltzmann;
namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  MPI_Init(&argc, &argv);
  int K;
  string tensor_file;

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("K", po::value<int>(&K), "K")
      ("tensor", po::value<string>(&tensor_file), "/path/to/tensor/h5")
      ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  if (!boost::filesystem::exists(tensor_file)) {
    throw std::runtime_error("path not found: " + tensor_file);
  }

  typedef typename SpectralBasisFactoryKS::basis_type basis_type;
  // trial space
  basis_type trial_basis;
  SpectralBasisFactoryKS::create(trial_basis, K);

  CollisionTensorGalerkin ct(trial_basis);
  ct.read_hdf5(tensor_file.c_str());

  MPI_Finalize();
  return 0;
}


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.

// 2. 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.

// 3. Neither the name of the copyright holder 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 HOLDER 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.

#include "rotation_averaging/hybrid_rotation_estimator.h"

#include <ceres/rotation.h>
#include <glog/logging.h>
#include <omp.h>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <limits>

#include "Spectra/MatOp/SparseSymMatProd.h"
#include "Spectra/SymEigsSolver.h"

#include "rotation_averaging/internal/rotation_estimator_util.h"
#include "solver/bcm_sdp_solver.h"
#include "solver/rbr_sdp_solver.h"
#include "solver/rank_restricted_sdp_solver.h"
#include "solver/riemannian_staircase.h"

namespace gopt {

HybridRotationEstimator::HybridRotationEstimator(
    const int N, const int dim)
    : HybridRotationEstimator(N, dim, HybridRotationEstimatorOptions()) {}

HybridRotationEstimator::HybridRotationEstimator(
    const int N, const int dim,
    const HybridRotationEstimator::HybridRotationEstimatorOptions& options)
    : options_(options),
      images_num_(N),
      dim_(dim),
      ld_rotation_estimator_(new LagrangeDualRotationEstimator(
          N, dim, options.sdp_solver_options)),
      irls_rotation_refiner_(nullptr) {}

bool HybridRotationEstimator::EstimateRotations(
    const std::unordered_map<ImagePair, TwoViewGeometry>& view_pairs,
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  const int N = images_num_;

  CHECK_NOTNULL(global_rotations);
  CHECK_GT(N, 0);
  CHECK_EQ(N, (*global_rotations).size());
  CHECK_GT(view_pairs.size(), 0);

  irls_rotation_refiner_.reset(
      new IRLSRotationLocalRefiner(N, view_pairs.size(), options_.irls_options));

  internal::ViewIdToAscentIndex(*global_rotations, &view_id_to_index_);
  ld_rotation_estimator_->SetViewIdToIndex(view_id_to_index_);

  Eigen::SparseMatrix<double> sparse_matrix;
  internal::SetupLinearSystem(
      view_pairs, (*global_rotations).size(),
      view_id_to_index_, &sparse_matrix);
  irls_rotation_refiner_->SetViewIdToIndex(view_id_to_index_);
  irls_rotation_refiner_->SetSparseMatrix(sparse_matrix);

  // Estimate global rotations that resides within the cone of
  // convergence for IRLS.
  LOG(INFO) << "Estimating Rotations Using LagrangeDual";
  ld_rotation_estimator_->EstimateRotations(view_pairs, global_rotations);

  // Refine the globally optimal result by IRLS.
  Eigen::VectorXd tangent_space_step;
  GlobalRotationsToTangentSpace(*global_rotations, &tangent_space_step);
  irls_rotation_refiner_->SetInitTangentSpaceStep(tangent_space_step);

  LOG(INFO) << "Refining Global Rotations";
  irls_rotation_refiner_->SolveIRLS(view_pairs, global_rotations);

  return true;
}

void HybridRotationEstimator::GlobalRotationsToTangentSpace(
    const std::unordered_map<image_t, Eigen::Vector3d>& global_rotations,
    Eigen::VectorXd* tangent_space_step) {
  (*tangent_space_step).resize((global_rotations.size() - 1) * 3);

  for (const auto& rotation : global_rotations) {
    const int view_index = FindOrDie(view_id_to_index_, rotation.first) - 1;

    if (view_index == IRLSRotationLocalRefiner::kConstantRotationIndex) {
      continue;
    }

    (*tangent_space_step).segment<3>(3 * view_index) = rotation.second;
  }
}

}  // namespace gopt


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

igl::BBWData::BBWData():
  partition_unity(false),
  W0(),
  active_set_params(),
  verbosity(0)
{
  // We know that the Bilaplacian is positive semi-definite
  active_set_params.Auu_pd = true;
}

void igl::BBWData::print()
{
  using namespace std;
  cout<<"partition_unity: "<<partition_unity<<endl;
  cout<<"W0=["<<endl<<W0<<endl<<"];"<<endl;
}

template <
  typename DerivedV,
  typename DerivedEle,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::bbw(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedEle> & Ele,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  igl::BBWData & data,
  Eigen::PlainObjectBase<DerivedW> & W
  )
{
  using namespace std;
  using namespace Eigen;
  assert(!data.partition_unity && "partition_unity not implemented yet");
  // number of domain vertices
  int n = V.rows();
  // number of handles
  int m = bc.cols();
  // Build biharmonic operator
  Eigen::SparseMatrix<typename DerivedV::Scalar> Q;
  harmonic(V,Ele,2,Q);
  W.derived().resize(n,m);
  // No linear terms
  VectorXd c = VectorXd::Zero(n);
  // No linear constraints
  SparseMatrix<typename DerivedW::Scalar> A(0,n),Aeq(0,n),Aieq(0,n);
  VectorXd Beq(0,1),Bieq(0,1);
  // Upper and lower box constraints (Constant bounds)
  VectorXd ux = VectorXd::Ones(n);
  VectorXd lx = VectorXd::Zero(n);
  active_set_params eff_params = data.active_set_params;
  if(data.verbosity >= 1)
  {
    cout<<"BBW: max_iter: "<<data.active_set_params.max_iter<<endl;
    cout<<"BBW: eff_max_iter: "<<eff_params.max_iter<<endl;
  }
  if(data.verbosity >= 1)
  {
    cout<<"BBW: Computing initial weights for "<<m<<" handle"<<
      (m!=1?"s":"")<<"."<<endl;
  }
  min_quad_with_fixed_data<typename DerivedW::Scalar > mqwf;
  min_quad_with_fixed_precompute(Q,b,Aeq,true,mqwf);
  min_quad_with_fixed_solve(mqwf,c,bc,Beq,W);
  // decrement
  eff_params.max_iter--;
  bool error = false;
  // Loop over handles
  std::mutex critical;
  const auto & optimize_weight = [&](const int i)
  {
    // Quicker exit for paralle_for
    if(error)
    {
      return;
    }
    if(data.verbosity >= 1)
    {
      std::lock_guard<std::mutex> lock(critical);
      cout<<"BBW: Computing weight for handle "<<i+1<<" out of "<<m<<
        "."<<endl;
    }
    VectorXd bci = bc.col(i);
    VectorXd Wi;
    // use initial guess
    Wi = W.col(i);
    SolverStatus ret = active_set(
        Q,c,b,bci,Aeq,Beq,Aieq,Bieq,lx,ux,eff_params,Wi);
    switch(ret)
    {
      case SOLVER_STATUS_CONVERGED:
        break;
      case SOLVER_STATUS_MAX_ITER:
        cerr<<"active_set: max iter without convergence."<<endl;
        break;
      case SOLVER_STATUS_ERROR:
      default:
        cerr<<"active_set error."<<endl;
        error = true;
    }
    W.col(i) = Wi;
  };
#ifdef WIN32
  for (int i = 0; i < m; ++i)
    optimize_weight(i);
#else
  parallel_for(m,optimize_weight,2);
#endif
  if(error)
  {
    return false;
  }

#ifndef NDEBUG
  const double min_rowsum = W.rowwise().sum().array().abs().minCoeff();
  if(min_rowsum < 0.1)
  {
    cerr<<"bbw.cpp: Warning, minimum row sum is very low. Consider more "
      "active set iterations or enforcing partition of unity."<<endl;
  }
#endif

  return true;
}

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


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

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

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

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

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

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

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

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

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

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


#include "engine/douglas_peucker.hpp"
#include "util/coordinate_calculation.hpp"

#include <boost/assert.hpp>
#include "osrm/coordinate.hpp"

#include <cmath>
#include <algorithm>
#include <iterator>
#include <stack>
#include <utility>

namespace osrm
{
namespace engine
{

namespace
{
struct CoordinatePairCalculator
{
    CoordinatePairCalculator(const util::FixedPointCoordinate coordinate_a,
                             const util::FixedPointCoordinate coordinate_b)
    {
        // initialize distance calculator with two fixed coordinates a, b
        first_lat = (coordinate_a.lat / COORDINATE_PRECISION) * util::RAD;
        first_lon = (coordinate_a.lon / COORDINATE_PRECISION) * util::RAD;
        second_lat = (coordinate_b.lat / COORDINATE_PRECISION) * util::RAD;
        second_lon = (coordinate_b.lon / COORDINATE_PRECISION) * util::RAD;
    }

    int operator()(const util::FixedPointCoordinate other) const
    {
        // set third coordinate c
        const float float_lat1 = (other.lat / COORDINATE_PRECISION) * util::RAD;
        const float float_lon1 = (other.lon / COORDINATE_PRECISION) * util::RAD;

        // compute distance (a,c)
        const float x_value_1 = (first_lon - float_lon1) * cos((float_lat1 + first_lat) / 2.f);
        const float y_value_1 = first_lat - float_lat1;
        const float dist1 = std::hypot(x_value_1, y_value_1) * util::EARTH_RADIUS;

        // compute distance (b,c)
        const float x_value_2 = (second_lon - float_lon1) * cos((float_lat1 + second_lat) / 2.f);
        const float y_value_2 = second_lat - float_lat1;
        const float dist2 = std::hypot(x_value_2, y_value_2) * util::EARTH_RADIUS;

        // return the minimum
        return static_cast<int>(std::min(dist1, dist2));
    }

    float first_lat;
    float first_lon;
    float second_lat;
    float second_lon;
};
}

void douglasPeucker(std::vector<SegmentInformation>::iterator begin,
                    std::vector<SegmentInformation>::iterator end,
                    const unsigned zoom_level)
{
    using Iter = decltype(begin);
    using GeometryRange = std::pair<Iter, Iter>;

    std::stack<GeometryRange> recursion_stack;

    const auto size = std::distance(begin, end);
    if (size < 2)
    {
        return;
    }

    begin->necessary = true;
    std::prev(end)->necessary = true;

    {
        BOOST_ASSERT_MSG(zoom_level < detail::DOUGLAS_PEUCKER_THRESHOLDS_SIZE,
                         "unsupported zoom level");
        auto left_border = begin;
        auto right_border = std::next(begin);
        // Sweep over array and identify those ranges that need to be checked
        do
        {
            // traverse list until new border element found
            if (right_border->necessary)
            {
                // sanity checks
                BOOST_ASSERT(left_border->necessary);
                BOOST_ASSERT(right_border->necessary);
                recursion_stack.emplace(left_border, right_border);
                left_border = right_border;
            }
            ++right_border;
        } while (right_border != end);
    }

    // mark locations as 'necessary' by divide-and-conquer
    while (!recursion_stack.empty())
    {
        // pop next element
        const GeometryRange pair = recursion_stack.top();
        recursion_stack.pop();
        // sanity checks
        BOOST_ASSERT_MSG(pair.first->necessary, "left border must be necessary");
        BOOST_ASSERT_MSG(pair.second->necessary, "right border must be necessary");
        BOOST_ASSERT_MSG(std::distance(pair.second, end) > 0, "right border outside of geometry");
        BOOST_ASSERT_MSG(std::distance(pair.first, pair.second) >= 0,
                         "left border on the wrong side");

        int max_int_distance = 0;
        auto farthest_entry_it = pair.second;
        const CoordinatePairCalculator dist_calc(pair.first->location, pair.second->location);

        // sweep over range to find the maximum
        for (auto it = std::next(pair.first); it != pair.second; ++it)
        {
            const int distance = dist_calc(it->location);
            // found new feasible maximum?
            if (distance > max_int_distance &&
                distance > detail::DOUGLAS_PEUCKER_THRESHOLDS[zoom_level])
            {
                farthest_entry_it = it;
                max_int_distance = distance;
            }
        }

        // check if maximum violates a zoom level dependent threshold
        if (max_int_distance > detail::DOUGLAS_PEUCKER_THRESHOLDS[zoom_level])
        {
            //  mark idx as necessary
            farthest_entry_it->necessary = true;
            if (1 < std::distance(pair.first, farthest_entry_it))
            {
                recursion_stack.emplace(pair.first, farthest_entry_it);
            }
            if (1 < std::distance(farthest_entry_it, pair.second))
            {
                recursion_stack.emplace(farthest_entry_it, pair.second);
            }
        }
    }
}
} // ns engine
} // ns osrm


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


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice,
// this
//    list of conditions and the following disclaimer.

// 2. 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.

// 3. Neither the name of the copyright holder 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 HOLDER 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.

// edited by Steffen Urban (urbste@googlemail.com), August 2021

#include "theia/math/rank_restricted_sdp_solver.h"

#include <glog/logging.h>
#include <limits>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace theia {
namespace math {

RankRestrictedSDPSolver::RankRestrictedSDPSolver(const size_t n,
                                                 const size_t block_dim)
    : RankRestrictedSDPSolver(n, block_dim, math::SDPSolverOptions()) {}

RankRestrictedSDPSolver::RankRestrictedSDPSolver(
    const size_t n,
    const size_t block_dim,
    const math::SDPSolverOptions& options)
    : BCMSDPSolver(n, block_dim, options), rank_(3) {
  Y_ = Eigen::MatrixXd::Zero(rank_, dim_ * n_);
}

void RankRestrictedSDPSolver::Solve(math::Summary& summary) {
  Eigen::MatrixXd G = Eigen::MatrixXd::Zero(rank_, dim_ * n_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  // Compute inital G according to Equ.(3)
  for (size_t i = 0; i < n_; i++) {
    const std::vector<size_t>& adjs = adj_edges_[i];
    for (auto j : adjs) {
      G.block(0, i * dim_, rank_, dim_) +=
          Y_.block(0, j * dim_, rank_, dim_) *
          Q_.block(j * dim_, i * dim_, dim_, dim_);
    }
  }

  double prev_func_val = std::numeric_limits<double>::max();
  double cur_func_val = this->EvaluateFuncVal();
  double duration = 0.0;
  double error = 0.0;

  summary.begin_time = std::chrono::high_resolution_clock::now();
  while (summary.total_iterations_num < sdp_solver_options_.max_iterations) {
    if (sdp_solver_options_.verbose) {
      LogToStd(summary.total_iterations_num,
               prev_func_val,
               cur_func_val,
               error,
               duration);
    }

    if (IsConverge(prev_func_val,
                   cur_func_val,
                   sdp_solver_options_.tolerance,
                   &error)) {
      break;
    }

    for (size_t i = 0; i < n_; i++) {
      const Eigen::MatrixXd G_block = G.block(0, i * dim_, rank_, dim_);
      Eigen::JacobiSVD<Eigen::MatrixXd> jacobi_svd(
          -G_block, Eigen::ComputeThinU | Eigen::ComputeThinV);

      const Eigen::MatrixXd prev_Y = Y_.block(0, i * dim_, rank_, dim_).eval();
      Y_.block(0, i * dim_, rank_, dim_) =
          jacobi_svd.matrixU() * jacobi_svd.matrixV().transpose();

      const std::vector<size_t>& adjs = adj_edges_[i];
#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
      for (size_t idx = 0; idx < adjs.size(); ++idx) {
        const size_t j = adjs[idx];
        G.block(0, j * dim_, rank_, dim_) +=
            (Y_.block(0, i * dim_, rank_, dim_) - prev_Y) *
            Q_.block(i * dim_, j * dim_, dim_, dim_);
      }
    }

    summary.total_iterations_num++;
    duration = summary.Duration();

    // Update function value
    prev_func_val = cur_func_val;
    cur_func_val = this->EvaluateFuncVal();
  }

  summary.total_iterations_num++;
  if (sdp_solver_options_.verbose) {
    LogToStd(summary.total_iterations_num,
             prev_func_val,
             cur_func_val,
             error,
             duration);
  }
}

Eigen::MatrixXd RankRestrictedSDPSolver::GetSolution() const {
  Eigen::MatrixXd solution = Eigen::MatrixXd(dim_, dim_ * n_);
  for (size_t i = 0; i < n_; i++) {
    solution.block(0, dim_ * i, dim_, dim_) =
        Y_.block(0, 0, rank_, dim_).transpose() *
        Y_.block(0, dim_ * i, rank_, dim_);
  }
  return solution;
}

const Eigen::MatrixXd& RankRestrictedSDPSolver::GetYStar() const { return Y_; }

const Eigen::MatrixXd RankRestrictedSDPSolver::ComputeQYt(
    const Eigen::MatrixXd& Y) const {
  return Q_ * Y.transpose();
}

Eigen::MatrixXd RankRestrictedSDPSolver::ComputeLambdaMatrix() const {
  const Eigen::MatrixXd QYt = this->ComputeQYt(Y_);

  const size_t rank = Y_.rows();
  // \Lambda = \SymblockDiag(Q * Y^T * Y).
  Eigen::MatrixXd Lambda = Eigen::MatrixXd::Zero(dim_, n_ * dim_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  for (size_t i = 0; i < n_; ++i) {
    Eigen::MatrixXd P =
        QYt.block(i * dim_, 0, dim_, rank) * Y_.block(0, i * dim_, rank, dim_);
    Lambda.block(0, i * dim_, dim_, dim_) = 0.5 * (P + P.transpose());
  }

  return Lambda;
}

double RankRestrictedSDPSolver::EvaluateFuncVal() const {
  // tr(Q * Y^T * Y) = tr(Y * Q * Y^T)
  // return (Q_ * Y_.transpose() * Y_).trace();
  return (Y_ * (Q_ * Y_.transpose())).trace();
}

double RankRestrictedSDPSolver::EvaluateFuncVal(
    const Eigen::MatrixXd& Y) const {
  return (Y * (Q_ * Y.transpose())).trace();
}

void RankRestrictedSDPSolver::AugmentRank() {
  rank_++;
  Y_.conservativeResize(Y_.rows() + 1, Y_.cols());
  Y_.row(Y_.rows() - 1).setZero();
}

void RankRestrictedSDPSolver::SetOptimalY(const Eigen::MatrixXd& Y) {
  CHECK_EQ(Y.rows(), Y_.rows());
  CHECK_EQ(Y.cols(), Y_.cols());
  Y_ = Y;
}

size_t RankRestrictedSDPSolver::CurrentRank() const { return rank_; }

Eigen::MatrixXd RankRestrictedSDPSolver::Project(
    const Eigen::MatrixXd& A) const {
  // We use a generalization of the well-known SVD-based projection for the
  // orthogonal and special orthogonal groups; see for example Proposition 7
  // in the paper "Projection-Like Retractions on Matrix Manifolds" by Absil
  // and Malick.

  Eigen::MatrixXd P(rank_, dim_ * n_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  for (size_t i = 0; i < n_; ++i) {
    // Compute the (thin) SVD of the ith block of A
    Eigen::JacobiSVD<Eigen::MatrixXd> SVD(
        A.block(0, i * dim_, rank_, dim_),
        Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Set the ith block of P to the SVD-based projection of the ith block of A
    P.block(0, i * dim_, rank_, dim_) =
        SVD.matrixU() * SVD.matrixV().transpose();
  }
  return P;
}

Eigen::MatrixXd RankRestrictedSDPSolver::Retract(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& V) const {
  // We use projection-based retraction, as described in "Projection-Like
  // Retractions on Matrix Manifolds" by Absil and Malick.
  return Project(Y + V);
}

Eigen::MatrixXd RankRestrictedSDPSolver::EuclideanGradient(
    const Eigen::MatrixXd& Y) const {
  return 2.0 * (Q_ * Y.transpose()).transpose();
}

Eigen::MatrixXd RankRestrictedSDPSolver::RiemannianGradient(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& nablaF_Y) const {
  return TangentSpaceProjection(Y, nablaF_Y);
}

Eigen::MatrixXd RankRestrictedSDPSolver::RiemannianGradient(
    const Eigen::MatrixXd& Y) const {
  return TangentSpaceProjection(Y, EuclideanGradient(Y));
}

Eigen::MatrixXd RankRestrictedSDPSolver::TangentSpaceProjection(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& dotY) const {
  return dotY - SymBlockDiagProduct(Y, Y, dotY);
}

Eigen::MatrixXd RankRestrictedSDPSolver::SymBlockDiagProduct(
    const Eigen::MatrixXd& A,
    const Eigen::MatrixXd& B,
    const Eigen::MatrixXd& C) const {
  // Preallocate result matrix
  Eigen::MatrixXd R(rank_, dim_ * n_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  for (size_t i = 0; i < n_; ++i) {
    // Compute block product Bi' * Ci.
    Eigen::MatrixXd P = B.block(0, i * dim_, rank_, dim_).transpose() *
                        C.block(0, i * dim_, rank_, dim_);
    // Symmetrize this block.
    Eigen::MatrixXd S = 0.5 * (P + P.transpose());
    // Compute Ai * S and set corresponding block of R.
    R.block(0, i * dim_, rank_, dim_) = A.block(0, i * dim_, rank_, dim_) * S;
  }
  return R;
}

Eigen::MatrixXd RankRestrictedSDPSolver::Precondition(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& dotY) const {
  switch (sdp_solver_options_.preconditioner_type) {
    case PreconditionerType::None:
      return dotY;
      break;
    case PreconditionerType::JACOBI:
      /* code */
      break;
    case PreconditionerType::INCOMPLETE_CHOLESKY:
      /* code */
      break;
    case PreconditionerType::REGULARIZED_CHOLESKY:
    default:
      break;
  }
}

}  // namespace math
}  // namespace theia


#include <armadillo>
#include <cmath>
#include <emissions.hpp>
#include <HSMM.hpp>
#include <iostream>
#include <vector>

using namespace arma;
using namespace hsmm;
using namespace std;

#define NDIM 2
#define NDUR 70
#define MINDUR 30
#define NOISE_STDDEV 0.05

double gaussianlogpdf_(double x, double mu, double sigma) {
    double ret = ((x - mu)*(x - mu)) / (-2*sigma*sigma);
    ret = ret - log(sqrt(2 * M_PI) * sigma);
    return ret;
}

mat fieldToMat(int njoints, field<mat> &samples) {
    mat ret(njoints, samples.n_elem);
    for(int i = 0; i < samples.n_elem; i++)
        ret.col(i) = samples(i);
    return ret;
}

void fillDurationMatrix(mat& duration, const ivec& centers) {
    assert(centers.n_elem == duration.n_rows);
    duration.zeros();
    //vec pmf_pattern = {0.1, 0.2, 0.4, 0.2, 0.1};
    vec pmf_pattern(20);
    pmf_pattern.fill(1.0/pmf_pattern.n_elem);
    for(int i = 0; i < centers.n_elem; i++) {
        int m = centers(i);
        int half = pmf_pattern.n_elem / 2;
        for(int j = 0; j < pmf_pattern.n_elem; j++)
            duration(i, m - half + j) = pmf_pattern(j);
    }
}

class ToyEmission : public AbstractEmissionOnlineSetting {
    public:
        ToyEmission(vector<function<vec(double)>> states, int ndim,
                double noise_stddev) : states_(states),
                AbstractEmissionOnlineSetting(states.size(), ndim) {
            output_gaussian_stddev_ = ones<mat>(getDimension(),
                    getNumberStates()) * noise_stddev;
        }

        ToyEmission* clone() const {
            return new ToyEmission(*this);
        }

        vec getSampleLocations(int length) const {
            return linspace<vec>(0, 1.0, length);
        }

        double loglikelihood(int state, const field<mat>& obs) const {
            vec t = getSampleLocations(obs.n_elem);
            double ret = 0;
            for(int i = 0; i < obs.n_elem; i++) {
                // Making sure all the missing obs are at the end.
                // Other missing obs patterns are not supported yet.
                if (obs(i).is_empty()) {
                    for(int j = i; j < obs.n_elem; j++)
                        assert(obs(j).is_empty());
                    break;
                }
                vec diff = obs(i) - states_.at(state)(t(i));
                for(int j = 0; j < diff.n_elem; j++)
                    ret += gaussianlogpdf_(diff(j), 0,
                            output_gaussian_stddev_(j, state));
            }
            return ret;
        }

        void reestimate(int min_duration, const field<cube>& meta,
                const field<field<mat>>& mobs ) {
            throw logic_error("Not implemented yet");
        }

        field<mat> sampleFromState(int state, int nsegments,
                mt19937& rng) const {
            vec t = getSampleLocations(nsegments);
            field<mat> ret(nsegments);
            for(int i = 0; i < nsegments; i++) {
                ret(i) = randn<vec>(getDimension());
                ret(i) = ret(i) % output_gaussian_stddev_.col(state);
                ret(i) = ret(i) + states_.at(state)(t(i));
            }
            return ret;
        }

        field<mat> sampleNextObsGivenPastObs(int state, int seg_dur,
                const field<mat>& past_obs, mt19937 &rng) const {
            throw logic_error("Not implemented yet");
        }

    private:
        vector<function<vec(double)>> states_;
        mat output_gaussian_stddev_;
};

vec halfsine1(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {sin(t), -sin(t)};
    return ret;
}

vec halfsine2(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {-sin(t), sin(t)};
    return ret;
}

vec halfsine3(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {0.5*sin(t), 0.25*sin(t)};
    return ret;
}

vec halfsine4(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {-0.25*sin(t), -0.5*sin(t)};
    return ret;
}

int main(int arc, char* argv[]) {
    vector<function<vec(double)>> states = {halfsine1, halfsine2,
            halfsine3, halfsine4};
    shared_ptr<ToyEmission> ptr_emission(new ToyEmission(states, NDIM,
            NOISE_STDDEV));
    vec pi(states.size());
    mat transition(states.size(), states.size());
    mat duration(states.size(), NDUR, fill::zeros);

    // Init the parameters.
    pi.fill(1.0 / states.size());
    transition.fill(1.0 / (states.size() - 1));
    transition.diag().zeros();
    ivec duration_centers = {15, 30, 45, 60};
    fillDurationMatrix(duration, duration_centers);

    OnlineHSMM online_toy_model(static_pointer_cast<
            AbstractEmissionOnlineSetting>(ptr_emission), transition, pi,
            duration, MINDUR);
    int nsegments = 12;
    ivec hs, hd;
    field<mat> toy_seq = online_toy_model.sampleSegments(nsegments, hs, hd);
    int nobs = toy_seq.n_elem;
    imat vit_mat = join_horiz(hs, hd);
    cout << "vit file" << endl << vit_mat << endl;

    // Online inference.
    mat state_marginals(states.size(), nobs);
    mat runlength_marginals(MINDUR + NDUR, nobs);
    mat implicit_remaining_marginals(MINDUR + NDUR, nobs);
    mat duration_marginals(NDUR, nobs);
    mat implicit_duration_marginals(NDUR, nobs);
    for(int c = 0; c < nobs; c++) {
        cout << "time step: " << c << endl;
        online_toy_model.addNewObservation(toy_seq(c));
        state_marginals.col(c) = online_toy_model.getStateMarginal();
        runlength_marginals.col(c) = online_toy_model.getRunlengthMarginal();
        duration_marginals.col(c) = online_toy_model.getDurationMarginal();
        implicit_remaining_marginals.col(c) = online_toy_model.
                getImplicitResidualTimeMarginal();
        //implicit_duration_marginals.col(c) = online_toy_model.
        //        getImplicitDurationMarginal();
    }

    // Saving everything.
    string prefix = "/local_data/dagudelo/paper_synth_experiment/";
    mat output = fieldToMat(NDIM, toy_seq);
    output.save(prefix + "paper_synth_obs.txt", raw_ascii);
    vit_mat.save(prefix + "paper_synth_gt_vit_seq.txt", raw_ascii);
    state_marginals.save(prefix + "paper_synth_mstates.txt", raw_ascii);
    runlength_marginals.save(prefix + "paper_synth_runlength.txt", raw_ascii);
    duration_marginals.save(prefix + "paper_synth_duration.txt", raw_ascii);
    implicit_remaining_marginals.save(prefix + "paper_synth_remaining.txt",
            raw_ascii);
    //implicit_duration_marginals.save(prefix + "paper_synth_impli_duration.txt",
    //        raw_ascii);
    return 0;
}


#include <unsupported/Eigen/CXX11/Tensor>
#include <Eigen/Dense>

using Eigen::Matrix;
using Eigen::Dynamic;
using Eigen::Tensor;
using Eigen::VectorXcf;
using Eigen::Vector3i;
using Eigen::Vector3f;

#ifndef GRAVITYSOLVERS_H
#define GRAVITYSOLVERS_H

/*
  MatrixData[:,j] = [m, x, y, z, vx, vy, vz, fx, fy, fz]
*/
#define MATRIX_DATA_ROWS 10
typedef Matrix<float, MATRIX_DATA_ROWS, Dynamic> MatrixData;

#define FIELD_TENSOR_DIMENSIONS 3
typedef Tensor<std::complex<float>, FIELD_TENSOR_DIMENSIONS> FieldTensorCF;
typedef Tensor<float, FIELD_TENSOR_DIMENSIONS> FieldTensorF;

namespace Gravitysolver {
  class DataIO
  {
  protected:
    float epsilon;
    MatrixData particles;
  public:
    DataIO();
    bool readDataOld(const std::string &filename);
    bool readData(const std::string &filename);
    bool writeData(const std::string &filename);
  };

  class Direct : public DataIO
  {
  public:
    Direct();
    float softening();
    void setSoftening(float eps);
    void solve();
    const MatrixData &data();
  };

  class PM : public DataIO
  {
  private:
    int Ng; // number of cells per dimension
    float h; // cell size
    float worldLen; // the size of the smallest enclosing cube of the galaxy
    FieldTensorCF density;
    FieldTensorCF greenFunction;
    FieldTensorCF potential;
    FieldTensorF ax;
    FieldTensorF ay;
    FieldTensorF az;
    Vector3i worldToGrid(float x, float y, float z);
    Vector3f gridToWorld(int i, int j, int k);
    void fft3d(FieldTensorCF &t);
    void ifft3d(FieldTensorCF &t);
    void conv3d(FieldTensorCF &out, FieldTensorCF &in, FieldTensorCF &kernel);
  public:
    PM(int numGridCells);
    void solve();
    const MatrixData &data();
  };
}

#endif // GRAVITYSOLVERS_H


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>
#include <frovedis/matrix/lapack_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm (
           make_rowmajor_matrix_local_load<float>("./sample_3x3"));
    auto inv_cm = inv(cm);

    std::vector<int> ipiv; // empty ipiv array
    getrf<float> (cm,ipiv); // cm will be factorized and ipiv will contain pivoting info
    getri<float> (cm,ipiv); // cm will be overwritten with inversed matrix

    // checking whether the above operations successfully taken place
    cm.to_rowmajor().save("./out_3x3_1");
    inv_cm.to_rowmajor().save("./out_3x3_2");
    BOOST_CHECK (system("diff ./out_3x3_1 ./ref_3x3") == 0);
    BOOST_CHECK (system("diff ./out_3x3_2 ./ref_3x3") == 0);
    system("rm -f ./out_3x3_1");
    system("rm -f ./out_3x3_2");
}


#include <boost/math/complex/atan.hpp>


#include <iostream>
#include <cfloat>

#include <armadillo>

#include <glm/models/glm_model.hpp>
#include <glm/models/links/glm_link.hpp>
#include <glm/irls.hpp>
#include <dcdflib/libdcdf.hpp>

using namespace arma;

void
set_missing_to_zero(const uvec &missing, vec &w)
{
    for(int i = 0; i < w.size( ); i++)
    {
        if( missing[ i ] == 1 )
        {
            w[ i ] = 0.0;
        }
    }
}

vec
chi_square_cdf(const vec &x, unsigned int df)
{
    vec p = ones<vec>( x.n_elem );
    for(int i = 0; i < x.n_elem; i++)
    {
        p[ i ] = chi_square_cdf( x[ i ], df );
    }

    return p;
}

vec
weighted_least_squares(const mat &X, const vec &y, const vec &w, bool fast_inversion)
{
    /* A = sqrt( w ) * X */
    mat A = diagmat( sqrt( w ) ) * X;

    /* ty = sqrt( w ) * y */
    vec ty = y % sqrt( w );

    vec beta;
    if( fast_inversion )
    {
        if( solve( beta, A, ty, solve_opts::fast + solve_opts::no_approx ) )
        {
            return beta;
        }
        else
        {
            return vec( );
        }
    }
    else
    {
        if( solve( beta, A, ty ) )
        {
            return beta;
        }
        else
        {
            return vec( );
        }
    }
}

vec
compute_z(const vec &eta, const vec &mu, const vec &mu_eta, const vec &y)
{
    return eta + mu_eta % ( y - mu );
}

vec
compute_w(const vec &var, const vec& mu_eta)
{
    return 1.0 / ( var % ( mu_eta % mu_eta ) );
}

vec
init_beta(const mat &X, const vec&y, const uvec &missing, const glm_model &model, bool fast_inversion = false)
{
    vec eta = model.get_link( ).eta( (y + 0.5) / 3.0 );

    return weighted_least_squares( X, eta, ones<vec>( missing.n_elem ) - missing, fast_inversion );
}

vec
irls(const mat &X, const vec &y, const uvec &missing, const glm_model &model, glm_info &output, bool fast_inversion)
{
    const glm_link &link = model.get_link( );
    vec b = init_beta( X, y, missing, model );
    vec w( X.n_rows );
    vec z( X.n_rows );
    vec eta = X * b;
    vec mu = link.mu( eta );

    vec mu_eta = link.mu_eta( mu );

    int num_iter = 0;
    double old_logl = -DBL_MAX;
    double logl = model.likelihood( mu, y, missing );
    bool invalid_mu = false;
    bool inverse_fail = false;
    vec b_old = b;
    bool first_attempt = true;
    while( num_iter < IRLS_MAX_ITERS && ! ( fabs( logl - old_logl ) / ( 0.1 + fabs( logl ) ) < IRLS_TOLERANCE ) )
    {
        w = compute_w( model.var( mu ), mu_eta );
        z = compute_z( eta, mu, mu_eta, y );
        set_missing_to_zero( missing, w );
        b = weighted_least_squares( X, z, w, fast_inversion );
        if( b.n_elem <= 0 )
        {
            inverse_fail = true;
            break;
        }

compute_eta:
        eta = X * b;
        mu = link.mu( eta );
        mu_eta = link.mu_eta( mu );

        if( !model.valid_mu( mu ) )
        {
            if( first_attempt )
            {
                /* Try a smaller step */
                b = 0.5*b_old + 0.5*b;
                first_attempt = false;
                goto compute_eta;
            }
            else
            {
                invalid_mu = true;
                break;
            }
        }

        old_logl = logl;
        b_old = b;
        logl = model.likelihood( mu, y, missing );

        num_iter++;
    }

    if( num_iter < IRLS_MAX_ITERS && !invalid_mu && !inverse_fail )
    {
        mat I = X.t( ) * diagmat( w ) * X;
        mat C;
        if( I.is_finite( ) && inv( C, I ) )
        {
            float dispersion = model.dispersion( mu, y, missing, b.n_elem );
            output.se_beta = sqrt( model.dispersion( mu, y, missing, dispersion ) * diagvec( C ) );
            output.num_iters = num_iter;
            output.converged = true;
            output.success = true;
            output.mu = mu;
            output.logl = model.likelihood( mu, y, missing, dispersion );

            vec wald_z = b / output.se_beta;
            vec chi2_value = wald_z % wald_z;
            output.p_value = -1.0 * ones<vec>( chi2_value.n_elem );
            for(int i = 0; i < chi2_value.n_elem; i++)
            {
                try
                {
                    output.p_value[ i ] = 1.0 - chi_square_cdf( chi2_value[ i ], 1 );
                }
                catch(bad_domain_value &e)
                {
                    continue;
                }
            }
        }
        else
        {
            output.success = false;
        }
    }
    else
    {
        output.num_iters = num_iter;
        output.converged = false;
        output.success = false;
    }

    return b;
}

vec
irls(const mat &X, const vec &y, const glm_model &model, glm_info &output, bool fast_inversion)
{
    uvec missing = zeros<uvec>( y.n_elem );
    return irls( X, y, missing, model, output );
}


#include <cstdio>
#include <iterator>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>

#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/compiler_config.h>

int format_output(const char* lib, const char* generator, int n, float time) {
  return std::printf("| %s || %s || %d || %.4fM items/sec\n", lib, generator, n, time);
}

typedef CGAL::Simple_cartesian<double>         R;
typedef R::Point_2                             Point;
typedef CGAL::Creator_uniform_2<double,Point>  Creator;
typedef std::vector<Point>                     Vector;

int main(int argc, char* argv[]) {
  int n = 10000000;
  int repeats = 10;

  if(argc > 1)
    n = boost::lexical_cast<int>(argv[1]);

  if(argc > 2)
    repeats = boost::lexical_cast<int>(argv[2]);

  Vector points(n, Point());

  CGAL::Random_points_in_disc_2<Point,Creator> g( 1000.0);
  boost::timer timer;
  const char* generator = "Random_points_in_disc_2";
  float time;

  std::cout <<
    "{| \n"
    "! Library !! Generator !! #Elements !! items/sec \n"
    "|- \n";

  timer.restart();
  for (int i = 0; i < repeats; ++i) { CGAL::copy_n( g, n, points.begin()); }
  time = (double)n*repeats/timer.elapsed()/1.0E6;
  format_output("CGAL", generator , n, time);
  std::cout << "|- \n";

#ifndef CGAL_CFG_NO_CPP0X_COPY_N
  timer.restart();
  for (int i = 0; i < repeats; ++i) { std::copy_n( g, n, points.begin()); }
  time = (double)n*repeats/timer.elapsed()/1.0E6;
  format_output("stdlib", generator, n, time);
#endif

  //wiki markup footer
  std::cout << "|}" << std::endl;
}


/**
 * @file IndexGraph.hpp
 * @author bwu
 * @brief Model of index graph concept and graph related algorithms
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_TOPOLOGY_INDEXGRAPH_HPP
#define GENERIC_TOPOLOGY_INDEXGRAPH_HPP
#include "generic/common/Exception.hpp"
#include "generic/common/Archive.hpp"
#include "Common.hpp"
#include <boost/graph/connected_components.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/functional/hash.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/visitors.hpp>
#include <unordered_set>

namespace generic  {
namespace topology {

///@brief represents model of undirected index edge concept
struct UndirectedIndexEdge
{
    ///@brief constructs an invalid edge
    UndirectedIndexEdge()
    {
        m_vertices = std::make_pair(noIndex, noIndex);
    }

    ///@brief constructs an undirected edge with vertex `iv1` and `iv2`
    UndirectedIndexEdge(index_t iv1, index_t iv2)
    {
       SetVertices(iv1, iv2);
    }

    ///@brief checks if this edge equals to `e`
    bool operator==(const UndirectedIndexEdge & e) const { return m_vertices == e.m_vertices; }
    ///@brief checks if this edge not equals to `e`
    bool operator!=(const UndirectedIndexEdge & e) const { return !(*this == e); }

    ///@brief set two vertices index of this edges
    void SetVertices(index_t iv1, index_t iv2) { m_vertices.first = std::min(iv1, iv2); m_vertices.second = std::max(iv1, iv2); }

    ///@brief returns the smaller vertex index of this edge
    index_t v1() const { return m_vertices.first ; }
    ///@brief returns the bigger vertex index of this edge
    index_t v2() const { return m_vertices.second; }

    ///@brief chechks if this edge contains vertex `iv`
    bool hasVertex(index_t iv) const { return iv == m_vertices.first || iv == m_vertices.second; }

private:
    std::pair<index_t, index_t> m_vertices;

#ifdef BOOST_SERIALIZATION_SUPPORT
private:
    friend class boost::serialization::access;
    template <typename Archive>
    void serialize(Archive & ar, const unsigned int)
    {
        ar & m_vertices;
    }
#endif
};

struct UndirectedIndexEdgeHash
{
    size_t operator() (const UndirectedIndexEdge & edge) const noexcept
    {
        size_t seed(0);
        boost::hash_combine(seed, edge.v1());
        boost::hash_combine(seed, edge.v2());
        return seed;
    }
};

struct UndirectedIndexEdgeCompare
{
    bool operator() (const UndirectedIndexEdge & e1, const UndirectedIndexEdge & e2) const noexcept { return e1 == e2; }
};

using UndirectedIndexEdgeSet = std::unordered_set<UndirectedIndexEdge, UndirectedIndexEdgeHash, UndirectedIndexEdgeCompare>;

template <typename T>
using UndirectedIndexEdgeMap = std::unordered_map<UndirectedIndexEdge, T, UndirectedIndexEdgeHash, UndirectedIndexEdgeCompare>;

///@brief represents model of sparse undirect index graph concept
using SparseIndexGraph = boost::adjacency_list<
                         boost::setS,
                         boost::vecS,
                         boost::undirectedS,
                         boost::property<boost::vertex_index_t, index_t >
                         >;

using SIGEdge = boost::graph_traits<SparseIndexGraph>::edge_descriptor;
using SIGEdgeIter = boost::graph_traits<SparseIndexGraph>::edge_iterator;
using SIGVertex = boost::graph_traits<SparseIndexGraph>::vertex_descriptor;

inline void AddEdge(index_t i, index_t j, SparseIndexGraph & g)
{
    boost::add_edge(i, j, g);
}

inline std::pair<SIGEdgeIter, SIGEdgeIter> Edges(const SparseIndexGraph & g)
{
    return boost::edges(g);
}

inline SIGVertex Source(const SIGEdge & e, const SparseIndexGraph & g)
{
    return boost::source(e, g);
}

inline SIGVertex Target(const SIGEdge & e, const SparseIndexGraph & g)
{
    return boost::target(e, g);
}

/**
 * @brief gets connected component by BFS
 *
 * @param[in] g the connection graph
 * @param[in] v the source vertex
 * @param[out] c contains the vertices connected to source v
 */
inline void ConnectedComponent(const SparseIndexGraph & g, const index_t v, std::list<index_t> & c)
{
    using namespace boost;
    GENERIC_ASSERT(v < num_vertices(g))

    class BFSVisitor : public default_bfs_visitor
    {
    public:
        std::list<index_t> & visited;
        BFSVisitor(std::list<index_t> & _visited) : visited(_visited) {}
        void discover_vertex(index_t s, const SparseIndexGraph &) { visited.push_back(s); }
    };

    c.clear();
    BFSVisitor vis(c);
    breadth_first_search(g, v, visitor(vis));
}

/**
 * @brief gets all connected components from graph
 *
 * @param[in] g the connection graph
 * @param[out] cc connected components
 */
inline void ConnectedComponents(const SparseIndexGraph & g, std::vector<std::list<index_t> > & cc)
{
    using namespace boost;
    std::vector<index_t> c(num_vertices(g));
    auto numComp = connected_components(g, make_iterator_property_map(c.begin(), get(vertex_index, g)));

    cc.clear();
    cc.resize(numComp);
    for(size_t i = 0; i < c.size(); ++i)
        cc[c[i]].push_back(i);
}

inline UndirectedIndexEdgeSet EdgeSet(const SparseIndexGraph & g)
{
    UndirectedIndexEdgeSet edgeSet;
    auto [begin, end] = topology::Edges(g);
    for(auto iter = begin; iter != end; ++iter){
        auto u = topology::Source(*iter, g);
        auto v = topology::Target(*iter, g);
        edgeSet.insert(UndirectedIndexEdge{u, v});
    }
    return edgeSet;
}

}//namespace topology
}//namespace generic
#endif//GENERIC_TOPOLOGY_INDEXGRAPH_HPP