/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <iostream>
#include <memory>
#include <ctime>
#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

#include <boost/random.hpp>

#include "GaussianProcess.h"
#include "Kernel.h"
#include "Likelihood.h"
#include "LikelihoodUtils.h"
#include "Prior.h"
#include "MatrixIO.h"

// typedefinitions
typedef gpr::Kernel<double>                     KernelType;
typedef std::shared_ptr<KernelType>             KernelTypePointer;
typedef gpr::WhiteKernel<double>                 WhiteKernelType;
typedef std::shared_ptr<WhiteKernelType>        WhiteKernelTypePointer;
typedef gpr::GaussianKernel<double>             GaussianKernelType;
typedef std::shared_ptr<GaussianKernelType>     GaussianKernelTypePointer;
typedef gpr::PeriodicKernel<double>             PeriodicKernelType;
typedef std::shared_ptr<PeriodicKernelType>     PeriodicKernelTypePointer;
typedef gpr::SumKernel<double>                  SumKernelType;
typedef std::shared_ptr<SumKernelType>          SumKernelTypePointer;
typedef gpr::GaussianProcess<double>            GaussianProcessType;
typedef std::shared_ptr<GaussianProcessType>    GaussianProcessTypePointer;

void Test1(){
    /*
     * Test 1: perform maximum aposteriori
     */
    std::cout << "Test 1: maximum log gaussian likelihood + log gaussian prior (gradient descent)... " << std::flush;

    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;

    // generating a signal
    // ground truth periodic variable
    //auto f = [](double x)->double { return std::sin(x)*std::cos(2.2*std::sin(x)); };
    auto f = [](double x)->double { return x+10*std::sin(x); };

    double val = 0;
    unsigned n = 70;
    double upper = 30;
    MatrixType signal = MatrixType::Zero(2,n);
    for(unsigned i=0; i<n; i++){
        signal(0,i) = val;
        signal(1,i) = f(val);
        val += upper/n;
    }

    // build Gaussian process and add the 1D samples
    WhiteKernelTypePointer pk(new WhiteKernelType(0)); // dummy kernel
    GaussianProcessTypePointer gp(new GaussianProcessType(pk));
    gp->SetSigma(0.01); // zero since with the white kernel (see later) this is considered

    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    bool cout = false;

    double p_sigma = 1;
    double p_sigma_variance = 1;
    double p_scale = 10;
    double p_scale_variance = 1;
    double p_period = 6.3;
    double p_period_variance = 2;
    double g_sigma = 100;
    double g_sigma_variance = 20;
    double g_scale = 60;
    double g_scale_variance = 30;

    // construct Gaussian log likelihood
    typedef gpr::GaussianLogLikelihood<double> GaussianLogLikelihoodType;
    typedef std::shared_ptr<GaussianLogLikelihoodType> GaussianLogLikelihoodTypePointer;
    GaussianLogLikelihoodTypePointer gl(new GaussianLogLikelihoodType());

    // construct Gaussian priors
    typedef gpr::GaussianDensity<double> GaussianDensityType;
    typedef std::shared_ptr<GaussianDensityType> GaussianDensityTypePointer;

    typedef gpr::InverseGaussianDensity<long double> InverseGaussianDensityType;
    typedef std::shared_ptr<InverseGaussianDensityType> InverseGaussianDensityTypePointer;

    std::vector<InverseGaussianDensityTypePointer> g_densities;
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_scale, p_scale_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_period, p_period_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_sigma, p_sigma_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(g_sigma, g_sigma_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(g_scale, g_scale_variance))));

    if(cout){
        for(auto p : g_densities){
            std::cout << "mode : " << p->mode() << ", variance: " << p->variance() << std::endl;
        }
    }

    double lambda = 1e-6;
    double w = 0.8; // weighting of w*likelihood resp. (1-w)*prior
    for(unsigned i=0; i<100; i++){
        // analytical
        try{
            PeriodicKernelTypePointer pk(new PeriodicKernelType(p_scale, M_PI/p_period, p_sigma));
            GaussianKernelTypePointer gk(new GaussianKernelType(g_sigma, g_scale));
            SumKernelTypePointer sk(new SumKernelType(pk,gk));
            gp->SetKernel(sk);
            gp->SetSigma(0.1);
            //gp->DebugOn();

            GaussianLogLikelihoodType::ValueDerivativePair lp = gl->GetValueAndParameterDerivatives(gp);
            VectorType likelihood = lp.first;
            VectorType likelihood_gradient = lp.second;

            double prior = 0;
            prior += g_densities[0]->log(p_scale);
            prior += g_densities[1]->log(p_period);
            prior += g_densities[2]->log(p_sigma);
            prior += g_densities[3]->log(g_sigma);
            prior += g_densities[4]->log(g_scale);

            double posterior = -2*(w*likelihood[0]+(1-w)*prior);

            MatrixType J = MatrixType::Zero(1,g_densities.size());
            //std::cout << likelihood_gradient << std::endl;
            J(0,0) = 2*(w*likelihood_gradient[0] + (1-w)*g_densities[0]->GetLogDerivative(p_scale));
            J(0,1) = 2*(w*likelihood_gradient[1] + (1-w)*g_densities[1]->GetLogDerivative(p_period));
            J(0,2) = 2*(w*likelihood_gradient[2] + (1-w)*g_densities[2]->GetLogDerivative(p_sigma));
            J(0,3) = 2*(w*likelihood_gradient[3] + (1-w)*g_densities[3]->GetLogDerivative(g_sigma));
            J(0,4) = 2*(w*likelihood_gradient[4] + (1-w)*g_densities[4]->GetLogDerivative(g_scale));

            VectorType update = lambda * gpr::pinv<MatrixType>(J.adjoint()*J)*J.adjoint();

            if(cout){
                std::cout << i << ": likelihood: " << likelihood[0] << ", prior: " << prior << ", posterior: " << posterior;
                std::cout << ", scale/period/sigma: " << p_scale << "/" << p_period << "/" << p_sigma << ", sigma/scale: " << g_sigma << "/" << g_scale;
                std::cout << ", update: " << update.adjoint() << std::endl;
            }

            p_scale -= update[0] * posterior;
            p_period -= update[1] * posterior;
            p_sigma -= update[2] * posterior;
            g_sigma -= update[3] * posterior;
            g_scale -= update[4] * posterior;

            if(p_scale < std::numeric_limits<double>::min()) p_scale = std::numeric_limits<double>::min();
            if(p_period < std::numeric_limits<double>::min()) p_period = std::numeric_limits<double>::min();
            if(p_sigma < std::numeric_limits<double>::min()) p_sigma = std::numeric_limits<double>::min();
            if(g_sigma < std::numeric_limits<double>::min()) g_sigma = std::numeric_limits<double>::min();
            if(g_scale < std::numeric_limits<double>::min()) g_scale = std::numeric_limits<double>::min();

        }
        catch(std::string& s){
            std::cout << "[failed] " << s << std::flush;
            std::cout << ", scale/period/sigma: " << p_scale << "/" << p_period << "/" << p_sigma << ", sigma/scale: " << g_sigma << "/" << g_scale << std::endl;
            return;
        }
    }

    std::vector<double> prediction_y;
    std::vector<double> prediction_x;
    // predict some stuff:
    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        prediction_y.push_back(gp->Predict(x)[0]);
        prediction_x.push_back(signal(0,i));
    }

    double err = 0;
    for(unsigned i=0; i<prediction_x.size(); i++ ){
        err += std::abs(prediction_y[i]-signal(1,i));
    }

    if(err/prediction_x.size() < 0.5){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err/prediction_x.size(); throw ss.str();
    }
}

void Test2(){
    /*
     * Test 1: perform maximum aposteriori
     */
    std::cout << "Test 2: maximum log gaussian likelihood + log gaussian prior (gradient descent, only period length)... " << std::flush;

    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;

    // generating a signal
    // ground truth periodic variable
    //auto f = [](double x)->double { return std::sin(x)*std::cos(2.2*std::sin(x)); };
    auto f = [](double x)->double { return x+10*std::sin(x); };

    double val = 0;
    unsigned n = 70;
    double upper = 30;
    MatrixType signal = MatrixType::Zero(2,n);
    for(unsigned i=0; i<n; i++){
        signal(0,i) = val;
        signal(1,i) = f(val);
        val += upper/n;
    }

    // build Gaussian process and add the 1D samples
    WhiteKernelTypePointer pk(new WhiteKernelType(0)); // dummy kernel
    GaussianProcessTypePointer gp(new GaussianProcessType(pk));
    gp->SetSigma(0.01); // zero since with the white kernel (see later) this is considered

    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    bool cout = false;

    double p_sigma = 1;
    double p_sigma_variance = 1;
    double p_scale = 10;
    double p_scale_variance = 1;
    double p_period = 5;// should be 6.3
    double p_period_variance = 2;
    double g_sigma = 100;
    double g_sigma_variance = 20;
    double g_scale = 60;
    double g_scale_variance = 30;

    // construct Gaussian log likelihood
    typedef gpr::GaussianLogLikelihood<double> GaussianLogLikelihoodType;
    typedef std::shared_ptr<GaussianLogLikelihoodType> GaussianLogLikelihoodTypePointer;
    GaussianLogLikelihoodTypePointer gl(new GaussianLogLikelihoodType());

    // construct Gaussian priors
    typedef gpr::GaussianDensity<double> GaussianDensityType;
    typedef std::shared_ptr<GaussianDensityType> GaussianDensityTypePointer;

    typedef gpr::InverseGaussianDensity<long double> InverseGaussianDensityType;
    typedef std::shared_ptr<InverseGaussianDensityType> InverseGaussianDensityTypePointer;

    std::vector<InverseGaussianDensityTypePointer> g_densities;
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_scale, p_scale_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_period, p_period_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_sigma, p_sigma_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(g_sigma, g_sigma_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(g_scale, g_scale_variance))));

    if(cout){
        for(auto p : g_densities){
            std::cout << "mode : " << p->mode() << ", variance: " << p->variance() << std::endl;
        }
    }

    double w = 0.5; // weighting of w*likelihood resp. (1-w)*prior
    for(unsigned i=0; i<100; i++){
        // analytical
        try{
            PeriodicKernelTypePointer pk(new PeriodicKernelType(p_scale, M_PI/p_period, p_sigma));
            GaussianKernelTypePointer gk(new GaussianKernelType(g_sigma, g_scale));
            SumKernelTypePointer sk(new SumKernelType(pk,gk));
            gp->SetKernel(sk);
            gp->SetSigma(0.1);
            //gp->DebugOn();

            GaussianLogLikelihoodType::ValueDerivativePair lp = gl->GetValueAndParameterDerivatives(gp);
            VectorType likelihood = lp.first;
            VectorType likelihood_gradient = lp.second;

            double prior = 0;
            prior += g_densities[0]->log(p_scale);
            prior += g_densities[1]->log(p_period);
            prior += g_densities[2]->log(p_sigma);
            prior += g_densities[3]->log(g_sigma);
            prior += g_densities[4]->log(g_scale);

            double posterior = -2*(w*likelihood[0]+(1-w)*prior);

            double p_dev = 2*(w*likelihood_gradient[1] + (1-w)*g_densities[1]->GetLogDerivative(p_period));

            if(cout){
                std::cout << i << ": likelihood: " << likelihood[0] << ", prior: " << prior << ", posterior: " << posterior;
                std::cout << ", scale/period/sigma: " << p_scale << "/" << p_period << "/" << p_sigma << ", sigma/scale: " << g_sigma << "/" << g_scale << std::endl;
            }
            p_period -= posterior/p_dev;

            if(p_period < std::numeric_limits<double>::min()) p_period = std::numeric_limits<double>::min();

        }
        catch(std::string& s){
            std::cout << "[failed] " << s << std::endl;
            std::cout << ", scale/period/sigma: " << p_scale << "/" << p_period << "/" << p_sigma << ", sigma/scale: " << g_sigma << "/" << g_scale << std::endl;
            return;
        }
    }

    std::vector<double> prediction_y;
    std::vector<double> prediction_x;
    // predict some stuff:
    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        prediction_y.push_back(gp->Predict(x)[0]);
        prediction_x.push_back(signal(0,i));
    }

    double err = 0;
    for(unsigned i=0; i<prediction_x.size(); i++ ){
        err += std::abs(prediction_y[i]-signal(1,i));
    }

    if(err/prediction_x.size() < 0.1){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err/prediction_x.size(); throw ss.str();
    }
}

void Test3(){
    /*
     * Test 3: perform maximum aposteriori
     */
    std::cout << "Test 3: maximum log gaussian likelihood + log gaussian prior (gradient descent only sigma)... " << std::flush;

    typedef GaussianProcessType::VectorType VectorType;
    typedef GaussianProcessType::MatrixType MatrixType;

    // generating a signal
    // ground truth periodic variable
    //auto f = [](double x)->double { return std::sin(x)*std::cos(2.2*std::sin(x)); };
    auto f = [](double x)->double { return x+10*std::sin(x); };

    double val = 0;
    unsigned n = 30;
    double upper = 30;
    MatrixType signal = MatrixType::Zero(2,n);
    for(unsigned i=0; i<n; i++){
        signal(0,i) = val;
        signal(1,i) = f(val);
        val += upper/n;
    }

    // build Gaussian process and add the 1D samples
    WhiteKernelTypePointer pk(new WhiteKernelType(0)); // dummy kernel
    GaussianProcessTypePointer gp(new GaussianProcessType(pk));
    gp->SetSigma(0.01); // zero since with the white kernel (see later) this is considered

    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        VectorType y = VectorType::Zero(1); y[0] = signal(1,i);
        gp->AddSample(x,y);
    }

    bool cout = false;

    double p_sigma = 0.5;
    double p_sigma_variance = 3;
    double p_scale = 10;
    double p_scale_variance = 1;
    double p_period = 6.3;
    double p_period_variance = 2;
    double g_sigma = 100;
    double g_sigma_variance = 20;
    double g_scale = 60;
    double g_scale_variance = 30;

    // construct Gaussian log likelihood
    typedef gpr::GaussianLogLikelihood<double> GaussianLogLikelihoodType;
    typedef std::shared_ptr<GaussianLogLikelihoodType> GaussianLogLikelihoodTypePointer;
    GaussianLogLikelihoodTypePointer gl(new GaussianLogLikelihoodType());

    // construct Gaussian priors
    typedef gpr::GaussianDensity<double> GaussianDensityType;
    typedef std::shared_ptr<GaussianDensityType> GaussianDensityTypePointer;

    typedef gpr::InverseGaussianDensity<long double> InverseGaussianDensityType;
    typedef std::shared_ptr<InverseGaussianDensityType> InverseGaussianDensityTypePointer;

    std::vector<InverseGaussianDensityTypePointer> g_densities;
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_scale, p_scale_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_period, p_period_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(p_sigma, p_sigma_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(g_sigma, g_sigma_variance))));
    g_densities.push_back(InverseGaussianDensityTypePointer( new InverseGaussianDensityType(InverseGaussianDensityType::GetMeanAndLambda(g_scale, g_scale_variance))));

    if(cout){
        for(auto p : g_densities){
            std::cout << "mode : " << p->mode() << ", variance: " << p->variance() << std::endl;
        }
    }

    double lambda = 1e-2;
    double w = 0.9; // weighting of w*likelihood resp. (1-w)*prior
    for(unsigned i=0; i<500; i++){
        // analytical
        try{
            PeriodicKernelTypePointer pk(new PeriodicKernelType(p_scale, M_PI/p_period, p_sigma));
            GaussianKernelTypePointer gk(new GaussianKernelType(g_sigma, g_scale));
            SumKernelTypePointer sk(new SumKernelType(pk,gk));
            gp->SetKernel(sk);
            gp->SetSigma(0.1);
            //gp->DebugOn();

            GaussianLogLikelihoodType::ValueDerivativePair lp = gl->GetValueAndParameterDerivatives(gp);
            VectorType likelihood = lp.first;
            VectorType likelihood_gradient = lp.second;

            double prior = 0;
            prior += g_densities[0]->log(p_scale);
            prior += g_densities[1]->log(p_period);
            prior += g_densities[2]->log(p_sigma);
            prior += g_densities[3]->log(g_sigma);
            prior += g_densities[4]->log(g_scale);

            double posterior = -2*(w*likelihood[0]+(1-w)*prior);

            double p_dev = g_densities[2]->GetLogDerivative(p_sigma);

            if(p_dev==0) break;

            double update;
            if(posterior/p_dev>0){
                update = std::log(1+posterior/p_dev);
            }
            else{
                update = -std::log(1+std::fabs(posterior/p_dev));
            }

            if(cout){
                std::cout << i << ": likelihood: " << likelihood[0] << ", prior: " << prior << ", posterior: " << posterior;
                std::cout << ", scale/period/sigma: " << p_scale << "/" << p_period << "/" << p_sigma << ", sigma/scale: " << g_sigma << "/" << g_scale;
                std::cout << ", gradient p_sigma: " << p_dev << ", update " << update << std::endl;
            }
            p_sigma -= lambda*update;

            if(p_sigma < std::numeric_limits<double>::min()) p_sigma = std::numeric_limits<double>::min();

        }
        catch(std::string& s){
            std::cout << "[failed] " << s << std::flush;
            std::cout << ", scale/period/sigma: " << p_scale << "/" << p_period << "/" << p_sigma << ", sigma/scale: " << g_sigma << "/" << g_scale << std::endl;
            return;
        }
    }

    std::vector<double> prediction_y;
    std::vector<double> prediction_x;
    // predict some stuff:
    for(unsigned i=0; i<n; i++){
        VectorType x = VectorType::Zero(1); x[0] = signal(0,i);
        prediction_y.push_back(gp->Predict(x)[0]);
        prediction_x.push_back(signal(0,i));
    }

    double err = 0;
    for(unsigned i=0; i<prediction_x.size(); i++ ){
        err += std::abs(prediction_y[i]-signal(1,i));
    }

    if(err/prediction_x.size() < 0.5){
        std::cout << "[passed]" << std::endl;
    }
    else{
        std::stringstream ss; ss<<err/prediction_x.size(); throw ss.str();
    }
}

int main (int argc, char *argv[]){
    std::cout << "Gaussian likelihood kernel test: " << std::endl;
    try{
        Test1();
        Test2();
        Test3();
    }
    catch(std::string& s){
        std::cout << "[failed] Error: " << s << std::endl;
        return -1;
    }

    return 0;
}


// Copyright (c) 2014 cDc and Pierre MOULON.

// 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 "i23dSFM/multiview/rotation_averaging_l1.hpp"

#ifdef HAVE_BOOST
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/foreach.hpp>
using namespace boost;
#else
#include "lemon/list_graph.h"
#include "lemon/kruskal.h"
#include "lemon/adaptors.h"
#include "lemon/dfs.h"
#include "lemon/path.h"
using namespace lemon;
#endif

#include "ceres/ceres.h"
#include "ceres/rotation.h"

#include <map>
#include <queue>
#include <stdint.h>

namespace i23dSFM   {
namespace rotation_averaging  {
namespace l1  {

// Minimum l1 error approximation:
//
// Let A be a M x N matrix with full rank. Given y of R^M, the problem
// (PA) minx ||y - Ax||1
// finds the vector x of R^N such that the error y - Ax has minimum l1 norm
// (i.e. we are asking that the difference between Ax and y be sparse).
// This problem arises in the context of channel coding
// (see E. J. Candes and T. Tao. "Decoding by linear programming". in IEEE Trans.
// Inform. Theory, December 2005).
//
// Suppose we have a channel code that produces a codeword c = Ax for a message x. The
// message travels over the channel, and has an unknown number of its entries corrupted.
// The decoder observes y = c + e, where e is the corruption. If e is sparse enough, then
// the decoder can use (PA) to recover x exactly. When x, A, y have real-valued entries,
// (PA) can be recast as an LP.
//
template<typename MATRIX_TYPE>
inline bool TRobustRegressionL1PD(
  const MATRIX_TYPE& A,
  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& y,
  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& xp,
  REAL pdtol, unsigned pdmaxiter)
{
  typedef Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
  typedef Eigen::Matrix<REAL, Eigen::Dynamic, 1> Vector;
  const unsigned M = (unsigned)y.size();
  const unsigned N = (unsigned)xp.size();
  assert(A.rows() == M && A.cols() == N);

  const REAL alpha(0.01);
  const REAL beta(0.5);
  const REAL mu(10);

  Vector x(xp);
  Vector Ax(A*x);
  Vector tmpM1(y-Ax);
  Vector tmpM2(-tmpM1);
  Vector tmpM3(tmpM1.cwiseAbs()), tmpM4(M);
  Vector u = (tmpM3*REAL(0.95)).array() + tmpM3.maxCoeff()*REAL(0.10);
  Vector fu1 = tmpM2-u;
  Vector fu2 = tmpM1-u;

  Vector lamu1(M), lamu2(M);
  for (unsigned i=0; i<M; ++i) {
    lamu1(i) = -1.0/fu1(i);
    lamu2(i) = -1.0/fu2(i);
  }
  const MATRIX_TYPE At(A.transpose());
  Vector Atv(At*(lamu1-lamu2));
  REAL AtvNormSq = Atv.squaredNorm();
  Vector rdual((-lamu1-lamu2).array() + REAL(1));
  REAL rdualNormSq = rdual.squaredNorm();

  Vector w2(M), sig1(M), sig2(M), sigx(M), dx(N), up(N), Atdv(N);
  Vector Axp(M), Atvp(M);
  Vector &Adx(sigx), &du(w2), &w1p(dx);
  Matrix H11p(N,N);
  Vector &dlamu1(tmpM3), &dlamu2(tmpM4);
  for (unsigned pditer=0; pditer<pdmaxiter; ++pditer) {
    // surrogate duality gap
    const REAL sdg(-(fu1.dot(lamu1) + fu2.dot(lamu2)));
    if (sdg < pdtol)
      break;
    const REAL tau(mu*2*M/sdg);
    const REAL inv_tau = REAL(-1)/tau;
    tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
    tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
    const REAL resnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());

    for (unsigned i=0; i<M; ++i) {
      REAL& tmpM3i = tmpM3(i);
      tmpM3i = inv_tau/fu1(i);
      REAL& tmpM4i = tmpM4(i);
      tmpM4i = inv_tau/fu2(i);
      w2(i) = tmpM3i + tmpM4i - REAL(1);
    }

    tmpM1 = lamu1.cwiseQuotient(fu1);
    tmpM2 = lamu2.cwiseQuotient(fu2);
    sig1 = -tmpM1 - tmpM2;
    sig2 = tmpM1 - tmpM2;
    sigx = sig1 - sig2.cwiseAbs2().cwiseQuotient(sig1);

    H11p = At*(Eigen::DiagonalMatrix<REAL,Eigen::Dynamic>(sigx)*A);
    w1p = At*(tmpM4 - tmpM3 - (sig2.cwiseQuotient(sig1).cwiseProduct(w2)));

    // optimized solver as A is positive definite and symmetric
    dx = H11p.ldlt().solve(w1p);

    Adx = A*dx;

    du = (w2 - sig2.cwiseProduct(Adx)).cwiseQuotient(sig1);

    dlamu1 = -tmpM1.cwiseProduct(Adx-du) - lamu1 + tmpM3;
    dlamu2 =  tmpM2.cwiseProduct(Adx+du) - lamu2 + tmpM4;
    Atdv = At*(dlamu1-dlamu2);

    // make sure that the step is feasible: keeps lamu1,lamu2 > 0, fu1,fu2 < 0
    REAL s(1);
    for (unsigned i=0; i<M; ++i) {
      REAL& dlamu1i = dlamu1(i);
      if (dlamu1i < 0) {
        const REAL tmp = -lamu1(i)/dlamu1i;
        if (s > tmp)
          s = tmp;
      }
      REAL& dlamu2i = dlamu2(i);
      if (dlamu2i < 0) {
        const REAL tmp = -lamu2(i)/dlamu2i;
        if (s > tmp)
          s = tmp;
      }
    }
    for (unsigned i=0; i<M; ++i) {
      REAL& Adxi = Adx(i);
      REAL& dui = du(i);
      const REAL Adx_du = Adxi-dui;
      if (Adx_du > 0) {
        const REAL tmp = -fu1(i)/Adx_du;
        if (s > tmp)
          s = tmp;
      }
      const REAL _Adx_du = -Adxi-dui;
      if (_Adx_du > 0) {
        const REAL tmp = -fu2(i)/_Adx_du;
        if (s > tmp)
          s = tmp;
      }
    }
    s *= REAL(0.99);

    // backtrack
    lamu1 += s*dlamu1;  lamu2 += s*dlamu2;
    rdual = (-lamu1-lamu2).array() + REAL(1);
    rdualNormSq = rdual.squaredNorm();
    bool suffdec = false;
    unsigned backiter = 0;
    do {
      xp = x + s*dx;  up = u + s*du;
      Axp = Ax + s*Adx;  Atvp = Atv + s*Atdv;
      fu1 = Axp - y - up;  fu2 = -Axp + y - up;
      AtvNormSq = Atvp.squaredNorm();
      tmpM1 = (-lamu1.cwiseProduct(fu1)).array() + inv_tau;
      tmpM2 = (-lamu2.cwiseProduct(fu2)).array() + inv_tau;
      const REAL newresnorm = sqrt(AtvNormSq + rdualNormSq + tmpM1.squaredNorm() + tmpM2.squaredNorm());
      suffdec = (newresnorm <= (REAL(1)-alpha*s)*resnorm);
      s = beta*s;
      if (++backiter > 32) {
        //("error: stuck backtracking, returning last iterate"); // see Section 4 of notes for more information
        xp.swap(x);
        return false;
      }
    } while (!suffdec);

    // next iteration
    x.swap(xp);  u.swap(up);
    Ax.swap(Axp);  Atv.swap(Atvp);
  }
  return true;
}

bool RobustRegressionL1PD(
  const Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
  REAL pdtol, unsigned pdmaxiter)
{
  return TRobustRegressionL1PD(A, b, x, pdtol, pdmaxiter);
}
bool RobustRegressionL1PD(
  const Eigen::SparseMatrix<REAL, Eigen::ColMajor>& A,
  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
  REAL pdtol, unsigned pdmaxiter)
{
  return TRobustRegressionL1PD(A, b, x, pdtol, pdmaxiter);
}

/*----------------------------------------------------------------*/

// Iteratively Re-weighted Least Squares (IRLS) implementation
template<typename MATRIX_TYPE>
inline bool TIterativelyReweightedLeastSquares(
  const MATRIX_TYPE& A,
  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
  REAL sigma, REAL eps)
{
  typedef Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
  typedef Eigen::Matrix<REAL, Eigen::Dynamic, 1> Vector;
  const unsigned m = (unsigned)b.size();
  const unsigned n = (unsigned)x.size();
  assert(A.rows() == m && A.cols() == n);

  // iterate optimization till the desired precision is reached
  Vector xp(n), e(m);
  const REAL sigmaSq(Square(sigma));
  unsigned iter = 0;
  REAL delta = std::numeric_limits<REAL>::max(), deltap;
  do {
    xp = x;
    // compute error vector
    e = A*x-b;
    // compute robust errors using the Huber-like loss function
    for (unsigned i=0; i<m; ++i) {
      REAL& err = e(i);
      const REAL errSq(Square(err));
      err = sigmaSq / (errSq + sigmaSq);
    }
    // solve the linear system using l2 norm
    const MATRIX_TYPE AtF(A.transpose()*e.asDiagonal());
    const Eigen::LDLT<Matrix> solver(AtF*A); // compute the Cholesky decomposition
    if (solver.info() != Eigen::Success) {
      std::cerr << "error: decomposing linear system failed" << std::endl;
      return false;
    }
    x = solver.solve(AtF*b);
    if (solver.info() != Eigen::Success) {
      std::cerr << "error: solving linear system failed" << std::endl;
      return false;
    }
    if (++iter > 32)
      break;
    deltap = delta; delta = (xp-x).norm();
  } while (delta > eps && (deltap-delta)/delta > 1e-2);
  return true;
}

bool IterativelyReweightedLeastSquares(
  const Eigen::Matrix<REAL, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
  REAL sigma, REAL eps)
{
  return TIterativelyReweightedLeastSquares(A, b, x, sigma, eps);
}
bool IterativelyReweightedLeastSquares(
  const Eigen::SparseMatrix<REAL, Eigen::ColMajor>& A,
  const Eigen::Matrix<REAL, Eigen::Dynamic, 1>& b,
  Eigen::Matrix<REAL, Eigen::Dynamic, 1>& x,
  REAL sigma, REAL eps)
{
  return TIterativelyReweightedLeastSquares(A, b, x, sigma, eps);
}

/////////////////////////

// given an array of values, compute the X84 threshold as in:
// Hampel FR, Rousseeuw PJ, Ronchetti EM, Stahel WA
// "Robust Statistics: the Approach Based on Influence Functions"
// Wiley Series in Probability and Mathematical Statistics, John Wiley & Sons, 1986
// returns the pair(median,trust_region)
// upper-bound threshold = median+trust_region
// lower-bound threshold = median-trust_region
template<typename TYPE>
inline std::pair<TYPE, TYPE>
ComputeX84Threshold(const TYPE* const values, size_t size, TYPE mul=TYPE(5.2))
{
  assert(size > 0);
  typename std::vector<TYPE> data(values, values+size);
  typename std::vector<TYPE>::iterator mid = data.begin() + size / 2;
  std::nth_element(data.begin(), mid, data.end());
  const TYPE median = *mid;
  // threshold = 5.2 * MEDIAN(ABS(values-median));
  for (size_t i=0; i<size; ++i)
    data[i] = std::abs(values[i]-median);
  std::nth_element(data.begin(), mid, data.end());
  return std::make_pair(median, mul*(*mid));
} // ComputeX84Threshold

/////////////////////////

typedef i23dSFM::Mat3 Matrix3x3;
typedef std::vector<size_t> IndexArr;

// find the shortest cycle for the given graph and starting vertex
struct Node {
  typedef IndexArr InternalType;
  InternalType edges; // array of vertex indices
};
typedef std::vector<Node> NodeArr;

struct Link {
  size_t ID; // node index
  size_t parentID;// parent link
  inline Link(size_t _ID=0, size_t _parentID=0) : ID(_ID), parentID(_parentID) {}
};
typedef std::queue<Link> LinkQue;

#ifdef HAVE_BOOST
typedef boost::property<boost::edge_weight_t, float> edge_property_t;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, size_t, edge_property_t> graph_t;
typedef graph_t::vertex_descriptor vertex_t;
typedef graph_t::edge_descriptor edge_t;
typedef boost::graph_traits<graph_t>::edge_iterator edge_iter;
#else
typedef lemon::ListGraph graph_t;
typedef graph_t::EdgeMap<double> map_EdgeMap;
#endif
typedef std::map<std::pair<size_t,size_t>, Matrix3x3> MapEdgeIJ2R;

// Look for the maximum spanning tree along the graph of relative rotations
// since we look for the maximum spanning tree using a minimum spanning tree algorithm
// weight are negated.
size_t FindMaximumSpanningTree(const RelativeRotations& RelRs, graph_t& g, MapEdgeIJ2R& mapIJ2R, NodeArr& minGraph)
{
  assert(!RelRs.empty());
#ifdef HAVE_BOOST
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    boost::add_edge(relR.i, relR.j, - relR.weight, g);
    mapIJ2R[std::make_pair(relR.i, relR.j)] = relR.Rij;
    mapIJ2R[std::make_pair(relR.j, relR.i)] = relR.Rij.transpose();
  }
  // find the minimum spanning tree
  const size_t nViews = boost::num_vertices(g);
  minGraph.resize(nViews);
  std::vector<edge_t> spanningTree;
  boost::kruskal_minimum_spanning_tree(g, std::back_inserter(spanningTree));
  for (std::vector<edge_t>::const_iterator ei=spanningTree.begin(); ei!=spanningTree.end(); ++ei) {
    const edge_t& edge = *ei;
    minGraph[edge.m_source].edges.push_back(edge.m_target);
    minGraph[edge.m_target].edges.push_back(edge.m_source);
  }
  const size_t nEdges = spanningTree.size();
  return nEdges;
#else

  //A-- Compute the number of node we need
  std::set<size_t> setNodes;
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    setNodes.insert(relR.i);
    setNodes.insert(relR.j);
  }

  //B-- Create a node graph for each element of the set
  typedef std::map<size_t, graph_t::Node> map_Size_t_Node;
  map_Size_t_Node map_size_t_to_node;
  for (std::set<size_t>::const_iterator iter = setNodes.begin();
    iter != setNodes.end();
    ++iter)
  {
    map_size_t_to_node[*iter] = g.addNode();
  }

  //C-- Create a graph from RelRs with weighted edges
  map_EdgeMap map_edgeMap(g);
  for (size_t p = 0; p < RelRs.size(); ++p) {
    const RelativeRotation& relR = RelRs[p];
    mapIJ2R[std::make_pair(relR.i, relR.j)] = relR.Rij;
    mapIJ2R[std::make_pair(relR.j, relR.i)] = relR.Rij.transpose();

    // add edge to the graph
    graph_t::Edge edge =  g.addEdge(map_size_t_to_node[relR.i], map_size_t_to_node[relR.j]);
    map_edgeMap[ edge ] = - relR.weight;
  }

  //D-- Compute the MST of the graph
  std::vector<graph_t::Edge> tree_edge_vec;
  lemon::kruskal(g, map_edgeMap, std::back_inserter(tree_edge_vec));

  const size_t nViews = lemon::countNodes(g);
  minGraph.resize(nViews);

  //E-- Export compute MST
  for(size_t i= 0 ; i < tree_edge_vec.size(); i++)
  {
    minGraph[g.id(g.u(tree_edge_vec[i]))].edges.push_back(g.id(g.v(tree_edge_vec[i])));
    minGraph[g.id(g.v(tree_edge_vec[i]))].edges.push_back(g.id(g.u(tree_edge_vec[i])));
  }
  return tree_edge_vec.size();
#endif
}
//----------------------------------------------------------------

// Filter the given relative rotations using the known global rotations
// returns the number of inliers
unsigned int FilterRelativeRotations(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  float threshold,
  std::vector<bool> * vec_inliers)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(threshold >= 0);
  // compute errors for each relative rotation
  std::vector<float> errors(RelRs.size());
  for(int r= 0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);
    const i23dSFM::Vec3 erij;
    ceres::RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    errors[r] = (float)erij.norm();
  }
  if (threshold == 0) {
    // estimate threshold
    const std::pair<float,float> res = ComputeX84Threshold(&errors[0], errors.size());
    threshold = res.first+res.second;
  }
  if (vec_inliers)  {
    vec_inliers->resize(RelRs.size());
  }
  // mark outliers
  unsigned int nInliers = 0;
  for(int r=0; r<errors.size(); ++r) {
    bool bInlier = (errors[r] < threshold);
    if (vec_inliers)
      (*vec_inliers)[r] = bInlier;
    if (bInlier)
      ++nInliers;
  }
  return nInliers;
} // FilterRelativeRotations
//----------------------------------------------------------------

REAL RelRotationAvgError(const RelativeRotations& RelRs, const Matrix3x3Arr& Rs, REAL* pMin=NULL, REAL* pMax=NULL)
{
#ifdef HAVE_BOOST
  boost::accumulators::accumulator_set<REAL,
    boost::accumulators::stats<
      boost::accumulators::tag::min,
      boost::accumulators::tag::mean,
      boost::accumulators::tag::max> > acc;

  for(int i=0; i < RelRs.size(); ++i) {
    const RelativeRotation& relR = RelRs[i];
    acc(i23dSFM::FrobeniusNorm(relR.Rij  - (Rs[relR.j]*Rs[relR.i].transpose())));
  }
  if (pMin)
    *pMin = boost::accumulators::min(acc);
  if (pMax)
    *pMax = boost::accumulators::max(acc);
  return boost::accumulators::mean(acc);
#else
  std::vector<REAL> vec_err(RelRs.size(), REAL(0.0));
  for(int i=0; i < RelRs.size(); ++i) {
    const RelativeRotation& relR = RelRs[i];
    vec_err[i] = i23dSFM::FrobeniusNorm(relR.Rij  - (Rs[relR.j]*Rs[relR.i].transpose()));
  }
  float min, max, mean, median;
  minMaxMeanMedian(vec_err.begin(), vec_err.end(), min, max, mean, median);
  if (pMin)
    *pMin = min;
  if (pMax)
    *pMax = max;
  return mean;
#endif
}
//----------------------------------------------------------------

// Robustly estimate global rotations from relative rotations as in:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// and detect outliers relative rotations and return them with 0 in arrInliers
bool GlobalRotationsRobust(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const size_t nMainViewID,
  float threshold,
  std::vector<bool> * vec_Inliers)
{
  assert(!Rs.empty());

  // -- Compute coarse global rotation estimates:
  //   - by finding the maximum spanning tree and linking the relative rotations
  //   - Initial solution is driven by relative rotations data confidence.
  graph_t g;
  MapEdgeIJ2R mapIJ2R;
  NodeArr minGraph;
  // find the Maximum Spanning Tree
  FindMaximumSpanningTree(RelRs, g, mapIJ2R, minGraph);
  g.clear();

  // start from the main view and link all views using the relative rotation estimates
  LinkQue stack;
  stack.push(Link(nMainViewID, size_t(0)));
  Rs[nMainViewID] = Matrix3x3::Identity();
  do {
    const Link& link = stack.front();
    const Node& node = minGraph[link.ID];

    for(Node::InternalType::const_iterator pEdge = node.edges.begin();
      pEdge != node.edges.end(); ++pEdge) {
        const size_t edge = *pEdge;
        if (edge == link.parentID) {
          // compute the global rotation for the current node
          assert(mapIJ2R.find(std::make_pair(link.parentID, link.ID)) != mapIJ2R.end());
          const Matrix3x3& Rij = mapIJ2R[std::make_pair(link.parentID, link.ID)];
          Rs[link.ID] = Rij * Rs[link.parentID];
        } else {
          // add edge to the processing queue
          stack.push(Link(edge, link.ID));
        }
    }
    stack.pop();
  } while(!stack.empty());
  minGraph.clear();
  mapIJ2R.clear();

  bool bOk = true;

  // refine global rotations based on the relative rotations
  bOk &= RefineRotationsAvgL1IRLS(RelRs, Rs, nMainViewID);

  // find outlier relative rotations
  if (threshold>=0 && vec_Inliers)  {
    FilterRelativeRotations(RelRs, Rs, threshold, vec_Inliers);
  }

  return bOk;
} // GlobalRotationsRobust
//----------------------------------------------------------------

// build A in Ax=b
inline void _FillMappingMatrix(
  const RelativeRotations& RelRs,
  const size_t nMainViewID,
  Eigen::SparseMatrix<REAL,Eigen::ColMajor>& A)
{
  A.reserve(A.rows()*2); // estimate of the number of non-zeros (optional)
  Eigen::SparseMatrix<REAL,Eigen::ColMajor>::Index i = 0, j = 0;
  for(int r=0; r<RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    if (relR.i != nMainViewID) {
      j = 3*(relR.i<nMainViewID ? relR.i : relR.i-1);
      A.insert(i+0,j+0) = REAL(-1);
      A.insert(i+1,j+1) = REAL(-1);
      A.insert(i+2,j+2) = REAL(-1);
    }
    if (relR.j != nMainViewID) {
      j = 3*(relR.j<nMainViewID ? relR.j : relR.j-1);
      A.insert(i+0,j+0) = REAL(1);
      A.insert(i+1,j+1) = REAL(1);
      A.insert(i+2,j+2) = REAL(1);
    }
    i+=3;
  }
  A.makeCompressed();
}

// compute errors for each relative rotation
inline void _FillErrorMatrix(
  const RelativeRotations& RelRs,
  const Matrix3x3Arr& Rs,
  Eigen::Matrix<REAL,Eigen::Dynamic,1>& b)
{
  for (size_t r = 0; r < RelRs.size(); ++r) {
    const RelativeRotation& relR = RelRs[r];
    const Matrix3x3& Ri = Rs[relR.i];
    const Matrix3x3& Rj = Rs[relR.j];
    const Matrix3x3& Rij = relR.Rij;
    const Mat3 eRij(Rj.transpose()*Rij*Ri);
    const i23dSFM::Vec3 erij;
    ceres::RotationMatrixToAngleAxis((const double*)eRij.data(), (double*)erij.data());
    b.block<3,1>(3*r,0) = i23dSFM::Vec3(erij*relR.weight);
  }
}

// apply correction to global rotations
inline void _CorrectMatrix(
  const Eigen::Matrix<REAL,Eigen::Dynamic,1>& x,
  const size_t nMainViewID,
  Matrix3x3Arr& Rs)
{
  for (size_t r = 0; r < Rs.size(); ++r) {
    if (r == nMainViewID)
      continue;
    Matrix3x3& Ri = Rs[r];
    const size_t i = (r<nMainViewID ? r : r-1);
    i23dSFM::Vec3 eRid = i23dSFM::Vec3(x.block<3,1>(3*i,0));
    const Mat3 eRi;
    ceres::AngleAxisToRotationMatrix((const double*)eRid.data(), (double*)eRi.data());
    Ri = Ri*eRi;
  }
}

// Refine the global rotations using to the given relative rotations, similar to:
// "Efficient and Robust Large-Scale Rotation Averaging", Chatterjee and Govindu, 2013
// L1 Rotation Averaging (L1RA) and Iteratively Reweighted Least Squares (IRLS) implementations combined
bool RefineRotationsAvgL1IRLS(
  const RelativeRotations& RelRs,
  Matrix3x3Arr& Rs,
  const size_t nMainViewID,
  REAL sigma)
{
  assert(!RelRs.empty() && !Rs.empty());
  assert(Rs[nMainViewID] == Matrix3x3::Identity());

  REAL fMinBefore, fMaxBefore, fMeanBefore = RelRotationAvgError(RelRs, Rs, &fMinBefore, &fMaxBefore);

  const unsigned nObss = (unsigned)RelRs.size();
  const unsigned nVars = (unsigned)Rs.size()-1; // main view is kept constant
  const unsigned m = nObss*3;
  const unsigned n = nVars*3;

  // build mapping matrix A in Ax=b
  Eigen::SparseMatrix<REAL,Eigen::ColMajor> A(m, n);
  _FillMappingMatrix(RelRs, nMainViewID, A);

  // init x with 0 that corresponds to trusting completely the initial Ri guess
  Vec x(Vec::Zero(n)), b(m);

  // L1RA iterate optimization till the desired precision is reached
  REAL e = std::numeric_limits<REAL>::max(), ep;
  unsigned iter1 = 0;
  do {
    // compute errors for each relative rotation
    _FillErrorMatrix(RelRs, Rs, b);
    // solve the linear system using l1 norm
    if (!RobustRegressionL1PD(A, b, x)) {
      std::cerr << "error: l1 robust regression failed." << std::endl;
      return false;
    }
    ep = e; e = x.norm();
    if (ep < e)
      break;
    // apply correction to global rotations
    _CorrectMatrix(x, nMainViewID, Rs);
  } while (++iter1 < 32 && e > 1e-5 && (ep-e)/e > 1e-2);
  // IRLS iterate optimization till the desired precision is reached
  x.setZero();
  e = std::numeric_limits<REAL>::max();
  unsigned iter2 = 0;
  do {
    // compute errors for each relative rotation
    _FillErrorMatrix(RelRs, Rs, b);
    // solve the linear system using l2 norm
    if (!IterativelyReweightedLeastSquares(A, b, x, sigma)) {
      std::cerr << "error: l2 iterative regression failed" << std::endl;
      return false;
    }
    ep = e; e = x.norm();
    if (ep < e)
      break;
    // apply correction to global rotations
    _CorrectMatrix(x, nMainViewID, Rs);
  } while (++iter2 < 32 && e > 1e-5 && (ep-e)/e > 1e-2);

  REAL fMinAfter, fMaxAfter, fMeanAfter = RelRotationAvgError(RelRs, Rs, &fMinAfter, &fMaxAfter);

  std::cout << "Refine global rotations using L1RA-IRLS and " << nObss << " relative rotations:\n"
    << " error reduced from " << fMeanBefore << "(" <<fMinBefore << " min, " << fMaxBefore << " max)\n"
    << " to " << fMeanAfter << "(" << fMinAfter << "min,"<< fMaxAfter<< "max)\n"
    << " in " << iter1 << "+" << iter2 << "=" << iter1+iter2 << " iterations" << std::endl;

  return true;
} // RefineRotationsAvgL1IRLS

} // namespace l1
} // namespace rotation_averaging
} // namespace i23dSFM


#include <gtest/gtest.h>
#include "pitts_multivector_random.hpp"
#include "pitts_multivector_eigen_adaptor.hpp"
#include <Eigen/Dense>

namespace
{
  // helper function for copy with a return instead of an argument
  template<typename T>
  auto copy_return(const PITTS::MultiVector<T>& a)
  {
    PITTS::MultiVector<T> b(a.rows(), a.cols());
    for(int j = 0; j < a.cols(); j++)
      for(int i = 0; i < a.rows(); i++)
        b(i,j) = a(i,j);
    return b;
  }
}

TEST(PITTS_MultiVector_EigenAdaptor, simple_const)
{
  using MultiVector_double = PITTS::MultiVector<double>;

  MultiVector_double mv(3,10);

  randomize(mv);
  const MultiVector_double mv_const = copy_return(mv);
  const auto mvMap = ConstEigenMap(mv_const);
  ASSERT_EQ(mv.rows(), mvMap.rows());
  ASSERT_EQ(mv.cols(), mvMap.cols());

  for(int i = 0; i < mv.rows(); i++)
    for(int j = 0; j < mv.cols(); j++)
    {
      EXPECT_EQ(mv(i,j), mvMap(i,j));
    }
}

TEST(PITTS_MultiVector_EigenAdaptor, simple_modify)
{
  using MultiVector_double = PITTS::MultiVector<double>;

  MultiVector_double mv(3,10);

  randomize(mv);
  auto mvMap = EigenMap(mv);
  ASSERT_EQ(mv.rows(), mvMap.rows());
  ASSERT_EQ(mv.cols(), mvMap.cols());

  const MultiVector_double oldmv = copy_return(mv);
  mvMap = Eigen::MatrixXd::Random(3,10);

  for(int i = 0; i < mv.rows(); i++)
    for(int j = 0; j < mv.cols(); j++)
    {
      EXPECT_EQ(mv(i,j), mvMap(i,j));
      EXPECT_NE(oldmv(i,j), mv(i,j));
    }
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <Eigen/Dense>

namespace py=pybind11;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

Vector mmul(
  Eigen::Ref<const Matrix>& mat,
  Eigen::Ref<const Vector>& vec)
{
  return mat*vec;
}

PYBIND11_MODULE(example, m)
{
  m.def("mmul", &mmul, "multiply matrix vector");
}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/1/diff-drive-kinematics_ex.cpp
 *  @author Jeferson Lima
 *  @brief  Differential drive kinematics Example
 *  @date   Mar 14, 2022
 **/

#include <iostream>
#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>
#include <robocore.hpp>
#ifdef USE_MATPLOTLIB
 #include <matplotlibcpp.h>
 namespace plt = matplotlibcpp;
#endif

using namespace boost::numeric::odeint;

const double vR = 0.05; // right wheel
const double vL = 0.06; // left  wheel
const double L  = 0.1;  // distance between wheels
typedef boost::array< double , 3 > state;

std::vector<double> xPos{0.0}, yPos{0.0}, theta{0.0};

void car_model(const state& x, state& dxdt, double t)
{
  double v = (vR + vL)/2;
  double w = (vR - vL)/L;

  dxdt[0] = std::cos(x[2])*v;
  dxdt[1] = std::sin(x[2])*v;
  dxdt[2] = w;
}

void log_model(const state& x, const double t)
{
  xPos.push_back(x[0]);
  yPos.push_back(x[1]);
  theta.push_back(x[2]);
  std::cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2] << std::endl;
}

int main(int argc, char** argv)
{
    state x = { 0.0 , 0.0 , 0.0 }; // initial conditions

#ifdef USE_MATPLOTLIB
    plt::figure();
    plt::named_plot("Initial Position", xPos, yPos, "o");
#endif
    runge_kutta4< state > stepper;
    integrate_const(stepper, car_model, x, 0.0, 20.0, 0.1, log_model);

#ifdef USE_MATPLOTLIB
    plt::named_plot("Car position", xPos, yPos);
    plt::legend();
    plt::show();
#endif

#ifdef SAVE_OUTPUT_CSV
  columns vals = {{"theta", theta}, {"xpos", xPos}, {"ypos", yPos}};
  RoboCore::write_csv("/tmp/diff_kinematic_output.csv", vals);
  std::cout << "Output file was saved in /tmp" << std::endl;
#endif

  return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ADDSCHWARZ_HH
#define ADDSCHWARZ_HH

#include <iostream>
#include <vector>
#include <boost/timer/timer.hpp>
#include <dune/istl/preconditioners.hh>

#include "linalg/triplet.hh"

namespace Kaskade
{
  enum schwarzType { DIAGONAL, COMPLETE };

  extern "C" void dgetrf_(int *m, int *n, double *a, int *lda, int *ipiv, int *info);
  extern "C" void dgetrs_(char *trans, int *n, int *nrhs, double *a, int *lda, int *ipiv,
      double *b, int *ldb, int *info);

  //---------------------------------------------------------------------

  // TODO: document this
  template <class Op>
  class AdditiveSchwarzPreconditioner: public Dune::Preconditioner<typename Op::domain_type, typename Op::range_type>
  {
    typedef typename Op::domain_type domain_type;
    typedef typename Op::domain_type range_type;
    typedef typename domain_type::field_type field_type;

  public:
    static int const category = Dune::SolverCategory::sequential;

    /**
     * \arg op the assembled operator
     */
    AdditiveSchwarzPreconditioner(Op const& op, schwarzType select, int verbosity=2)
    {
      MatrixAsTriplet<field_type> A = op.template get<MatrixAsTriplet<field_type> >();
      size_t k, n = A.nrows();
      switch (select)
      {
      case DIAGONAL:
        iBlocks.resize(n+1);
        iBlocksColIndices.resize(n);
        for (k=0; k<n; k++)
        {
          iBlocks[k] = k;
          iBlocksColIndices[k] = k;
        }
        iBlocks[n] = n;
        break;
      case COMPLETE:
        iBlocks.resize(2);
        iBlocksColIndices.resize(n);
        iBlocks[0] = 0;
        iBlocks[1] = n;
        for (k=0; k<n; k++)
        {
          iBlocksColIndices[k] = k;
        }
        break;
      }
      init(A,verbosity);
    }

    AdditiveSchwarzPreconditioner(Op const& op, size_t first, size_t last, int verbosity=2)
    {
      MatrixAsTriplet<field_type> A = op.template get<MatrixAsTriplet<field_type> >();
      size_t i, k, nBlocks = last-first, cnt, nnz = A.nnz();
      iBlocks.resize(nBlocks+1);
      // printf("AdditiveSchwarzPreconditioner: n=%ld, nBlocks=%ld\n", n, nBlocks);
      std::vector<size_t> nCounts(nBlocks);
      for (k=0; k<nBlocks; k++)
        nCounts[k] = 1;
      cnt = nBlocks;

      for (i=0; i<nnz; i++)
      {
        size_t ri = A.ridx[i];
        size_t ci = A.cidx[i];
        if ((ri>=first)&&(ri<last))
        {
          if ((ci<first)||(ci>=last))
          {
            nCounts[ri-first]++;
            cnt++;
          }
        }
        //             else
          //              {
          //                 if ((ci>=first)||(ci<last))
        //                   {
        //                     nCounts[ci-first]++;
        //                     cnt++;
        //                   }
        //               }
      }
      // printf("nCounts:  cnt=%ld ", cnt);
      // for(k=0; k<nBlocks; k++) printf("%3ld ", nCounts[k]);
      // printf("\n");

      iBlocksColIndices.resize(cnt);
      iBlocks[0] = 0;
      for (k=1; k<=nBlocks; k++)
      {
        iBlocks[k] = iBlocks[k-1]+nCounts[k-1];
        iBlocksColIndices[iBlocks[k-1]] = first+k-1;
        nCounts[k-1] = 1;
      }
      assert(iBlocks[nBlocks]==cnt);
      // printf("iBlocks:  ");
      // for(k=0; k<=nBlocks; k++) printf("%3ld ", iBlocks[k]);
      // printf("\n");
      for (i=0; i<nnz; i++)
      {
        size_t ri = A.ridx[i];
        size_t ci = A.cidx[i];
        if ((ri>=first)&&(ri<last))
        {
          if ((ci<first)||(ci>=last))
          {
            iBlocksColIndices[iBlocks[ri-first]+nCounts[ri-first]] = ci;
            nCounts[ri-first]++;
          }
        }
        //             else
          //              {
          //                 if ((ci>=first)||(ci<last))
        //                   {
        //                     iBlocksColIndices[iBlocks[ci-first]+nCounts[ci-first]] = ri;
        //                     nCounts[ci-first]++;
        //                   }
        //               }
      }

      init(A,verbosity);
    }

    void init(MatrixAsTriplet<field_type> &A, int verbosity=2)
    {
      size_t i, k, n = A.nrows(), nnz = A.nnz();
      // computing the amount of storage needed vo all sub-matrices

      size_t nBlocks = iBlocks.size()-1;
      // printf("AdditiveSchwarzPreconditioner: n=%ld, nBlocks=%ld\n", n, nBlocks);
      // for (k=0; k<nBlocks; k++) {
      // printf("%3ld: ", k);
      // for (i=iBlocks[k]; i<iBlocks[k+1]; i++) printf("%3ld ", iBlocksColIndices[i]);
      // printf("\n");
      // }
      blocks.resize(nBlocks);
      ipiv.resize(nBlocks);
      size_t maxBlockSize = -1;
      size_t memSize = 0, inverseIndexSize = 0;
      for (k=0; k<nBlocks; k++)
      {
        int blockSize = iBlocks[k+1]-iBlocks[k];
        if (blockSize>maxBlockSize)
          maxBlockSize = blockSize;
        memSize += blockSize*blockSize;
        inverseIndexSize += blockSize;
      }
      // printf("    memSize=%ld, inverseIndexSize=%ld\n", memSize, inverseIndexSize);

      // allocating the memory

      blocks[0] = (field_type*)malloc(memSize*sizeof(field_type));
      ipiv[0] = (int*)malloc(inverseIndexSize*sizeof(int));
      for (k=1; k<nBlocks; k++)
      {
        int blockSize = iBlocks[k]-iBlocks[k-1];
        blocks[k] = blocks[k-1]+blockSize*blockSize;
        ipiv[k] = ipiv[k-1]+blockSize;
      }
      // printf("blocks allocated: \n");
      // for (k=0; k<nBlocks; k++) {
      // int blockSize = iBlocks[k+1]-iBlocks[k];
      // printf("%8p %d ", blocks[k], blockSize*blockSize);
      // }
      // printf("\n");

      // which index is used in which submatrix? (inverse)

      std::vector<size_t> usedCnts(n);
      std::vector<size_t> usedIn(n+1);
      std::vector<size_t> usedInIndices(inverseIndexSize);

      for (k=0; k<n; k++)
        usedCnts[k] = 0;
      for (k=0; k<nBlocks; k++)
      {
        for (i=iBlocks[k]; i<iBlocks[k+1]; i++)
        {
          usedCnts[iBlocksColIndices[i]]++;
        }
      }
      // printf("usedCnts: ");
      // for (k=0; k<n; k++) printf(" %3ld", usedCnts[k]);
      // printf("\n");
      size_t cnt = 0;
      for (k=0; k<n; k++)
      {
        usedIn[k] = cnt;
        cnt += usedCnts[k];
      }
      if (cnt!=inverseIndexSize) printf("Assert cnt=%ld!=%ld=inverseIndexSize\n", cnt, inverseIndexSize);
      usedIn[n] = inverseIndexSize;

      for (k=0; k<n; k++)
        usedCnts[k] = 0;
      for (k=0; k<nBlocks; k++)
      {
        for (i=iBlocks[k]; i<iBlocks[k+1]; i++)
        {
          size_t ii = iBlocksColIndices[i];
          usedInIndices[usedIn[ii]+usedCnts[ii]] = k;
          usedCnts[ii]++;
        }
      }
      // printf("Inverse index: n=%ld, nBlocks=%ld\n", n, nBlocks);
      // for (k=0; k<n; k++) {
      // printf("%3ld: ", k);
      // for (i=usedIn[k]; i<usedIn[k+1]; i++) printf("%3ld ", usedInIndices[i]);
      // printf("\n");
      // }

      // filling the submatrices

      for (i=0; i<nnz; i++)
      {
        size_t ri = A.ridx[i];
        size_t ci = A.cidx[i];
        field_type vali = A.data[i];
        for (k=usedIn[ri]; k<usedIn[ri+1]; k++)
        {
          size_t kk = usedInIndices[k];
          // printf("A(%ld,%ld)=%14.5e in Block %ld\n", ri, ci, vali, kk);
          size_t subi, subk, bsize = iBlocks[kk+1]-iBlocks[kk];
          // now we have found the blocknummer kk
          for (subi=iBlocks[kk]; subi<iBlocks[kk+1]; subi++)
            if (ri==iBlocksColIndices[subi]) break;
          if (subi==iBlocks[kk+1]) continue;
          subi -= iBlocks[kk];
          for (subk=iBlocks[kk]; subk<iBlocks[kk+1]; subk++)
            if (ci==iBlocksColIndices[subk]) break;
          if (subk==iBlocks[kk+1]) continue;
          subk -= iBlocks[kk];
          // now we have the indices for the submatrix
          // printf("subA(%ld,%ld) %8p set to%14.5e\n", subi, subk, &blocks[kk][subi*bsize+subk], vali);
          blocks[kk][subi*bsize+subk] = vali;    // or adding? preset blocks with zero!
        }
      }

      // calling lapacks to compute the inverses inverseIndexSize

      for (k=0; k<nBlocks; k++)
      {
        size_t bsize = iBlocks[k+1]-iBlocks[k];
        //             printf("block=%ld %8p, bsize=%ld, ipiv=%8p\n", k, blocks[k], bsize, ipiv[k]);
        // 			for (int j=0; j<bsize; j++)
        // 			  {
        // 				printf("%8p:", &blocks[k][j*bsize]);
        // 				for (int l=0; l<bsize; l++)
        // 				  printf("%14.5e ", blocks[k][j*bsize+l]);
        // 				printf("\n");
        // 			  }
        if (bsize==1)
        {
          if (fabs(blocks[k][0])<1.0e-20)
          {
            printf("error diagonal element to small %e\n", blocks[k][0]);
            exit(1);
          }
          blocks[k][0] = 1.0/blocks[k][0];
        }
        else
        {
          int info, f_n = bsize;
          dgetrf_(&f_n, &f_n, blocks[k], &f_n, ipiv[k], &info);
          if (info!=0)
          {
            printf("error from dgetrf_: info = %d, block=%ld %8p, bsize=%ld\n", info,
                k, blocks[k], bsize);
            exit(1);
          }
        }
      }

      if ( verbosity>=2 )
      {
        std::cout << "PrecondType::ADDITIVESCHWARZ: n=" << n << ", nnz=" << nnz << ", time=" << boost::timer::format(timer.elapsed()) << "\n";
      }
    }

    ~AdditiveSchwarzPreconditioner()
    {
      free(blocks[0]);
      free(ipiv[0]);
    }

    virtual void pre(domain_type&, range_type&) {}
    virtual void post (domain_type&) {}

    virtual void apply (domain_type& x, range_type const& y)
    {
      size_t i, k;
      int n = y.dim();
      std::vector<field_type> b(n), bb(n), bbb(n);
      y.write(b.begin());
      int nBlocks = iBlocks.size()-1;
      // printf("apply: n=%d, nBlock=%d\n", n, nBlocks);
      for (k=0; k<nBlocks; k++)
      {
        int nn, nrhs = 1, lda, ldb, info, cnt;
        char trans[1] = {'N'};
        cnt = 0;
        for (i=iBlocks[k]; i<iBlocks[k+1]; i++)
        {
          size_t ii = iBlocksColIndices[i];
          // printf("bb[%d] %14.5e = b[%ld] %14.5e\n", cnt, bb[cnt], ii, b[ii]);
          bb[cnt] = b[ii];
          cnt++;
        }
        nn = lda = ldb = iBlocks[k+1]-iBlocks[k];
        // printf("apply%3ld: cnt=%d %8p, lng=%d\n", k, cnt, ipiv[k], lda);
        if (nn==1)
        {
          bb[0] = bb[0]*blocks[k][0];
        }
        else
        {
          dgetrs_(trans, &nn, &nrhs, blocks[k], &lda, ipiv[k], &bb[0], &ldb, &info);
          if (info!=0)
          {
            printf("error from dgetrs_: info =%d\n", info);
            exit(1);
          }
        }
        cnt = 0;
        for (i=iBlocks[k]; i<iBlocks[k+1]; i++)
        {
          size_t ii = iBlocksColIndices[i];
          // printf("b[%ld] %14.5e = bb[%d] %14.5e\n", ii, b[ii], cnt, b[cnt]);
          bbb[ii] += bb[cnt];
          cnt++;
        }
      }
      x.read(bbb.begin());
    }

  private:
    std::vector<size_t> iBlocks, iBlocksColIndices;
    std::vector<field_type*> blocks;
    std::vector<int*> ipiv;
    boost::timer::cpu_timer timer;
  };
} // namespace Kaskade
#endif


// Copyright (c) 2017, University of Minnesota
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// 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.
// THIS SOFTWARE IS PROVIDED "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 UNIVERSITY OF MINNESOTA, DULUTH 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.

#ifndef ADMM_FORCE_H
#define ADMM_FORCE_H 1

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <memory>
#include "cppoptlib/solver/lbfgssolver.h"

namespace admm {

//
//	Force base class
//
//	Weight should be calculated in the initialize function, then retrieved by the system
//	in System::initialize. For certain forces, optimized w_i is known.
//
class Force {
public:
	int global_idx; // Global index is the position of this force in the global u and z vectors
	double weight; // Weight is computed BY the force based on stiffness

	Force() : weight(0.f) {}
	virtual ~Force() {}

	// Called in System::initialize to compute local variables
	virtual void initialize( const Eigen::VectorXd &x, const Eigen::VectorXd &v, const Eigen::VectorXd &masses, const double timestep ){}

	// Get triplets for the selector (D) matrix, called after initialize
	virtual void get_selector( const Eigen::VectorXd &x, std::vector< Eigen::Triplet<double> > &triplets, std::vector<double> &weights ) = 0;

	// Called in System::step
	virtual void project( double dt, const Eigen::VectorXd &Dx, Eigen::VectorXd &u, Eigen::VectorXd &z ) const = 0;

	// Set an epsilon for collision/sliding/etc...
	virtual void set_eps( double eps ){}

}; // end class force

//
//	Spring Force
//
class Spring : public Force {
public:
	Spring( int idx0_, int idx1_, double stiffness_ ) : idx0(idx0_), idx1(idx1_), stiffness(stiffness_) {}
	void initialize( const Eigen::VectorXd &x, const Eigen::VectorXd &v, const Eigen::VectorXd &masses, const double timestep );
	void get_selector( const Eigen::VectorXd &x, std::vector< Eigen::Triplet<double> > &triplets, std::vector<double> &weights );
	void project( double dt, const Eigen::VectorXd &Dx, Eigen::VectorXd &u, Eigen::VectorXd &z ) const;
	int idx0, idx1;
	double stiffness, rest_length;

}; // end class spring

} // end namespace admm

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_ACCESS_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_ACCESS_HPP

#include <boost/geometry/core/access.hpp>

#include <boost/geometry/extensions/algebra/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace core_dispatch
{

template <typename Vector, typename CoordinateType, std::size_t Dimension>
struct access<vector_tag, Vector, CoordinateType, Dimension, std::false_type>
{
    static inline CoordinateType get(Vector const& v)
    {
        return traits::access<Vector, Dimension>::get(v);
    }
    static inline void set(Vector& v, CoordinateType const& value)
    {
        traits::access<Vector, Dimension>::set(v, value);
    }
};

template <typename Vector, typename CoordinateType, std::size_t Dimension>
struct access<vector_tag, Vector, CoordinateType, Dimension, std::true_type>
{
    static inline CoordinateType get(Vector const* v)
    {
        return traits::access<typename std::remove_pointer<Vector>::type, Dimension>::get(*v);
    }
    static inline void set(Vector* v, CoordinateType const& value)
    {
        traits::access<typename std::remove_pointer<Vector>::type, Dimension>::set(*v, value);
    }
};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<quaternion_tag, Q, CoordinateType, Dimension, std::false_type>
{
    static inline CoordinateType get(Q const& v)
    {
        return traits::access<Q, Dimension>::get(v);
    }
    static inline void set(Q& v, CoordinateType const& value)
    {
        traits::access<Q, Dimension>::set(v, value);
    }
};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<quaternion_tag, Q, CoordinateType, Dimension, std::true_type>
{
    static inline CoordinateType get(Q const* v)
    {
        return traits::access<typename std::remove_pointer<Q>::type, Dimension>::get(*v);
    }
    static inline void set(Q* v, CoordinateType const& value)
    {
        traits::access<typename std::remove_pointer<Q>::type, Dimension>::set(*v, value);
    }
};

template<typename M, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<matrix_tag, M, CoordinateType, I, J, std::false_type>
    : detail::indexed_access_non_pointer<M, CoordinateType, I, J>
{};

template<typename M, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<matrix_tag, M, CoordinateType, I, J, std::true_type>
    : detail::indexed_access_pointer<M, CoordinateType, I, J>
{};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<rotation_quaternion_tag, Q, CoordinateType, Dimension, std::false_type>
{
    static inline CoordinateType get(Q const& v)
    {
        return traits::access<Q, Dimension>::get(v);
    }
    static inline void set(Q& v, CoordinateType const& value)
    {
        traits::access<Q, Dimension>::set(v, value);
    }
};

template <typename Q, typename CoordinateType, std::size_t Dimension>
struct access<rotation_quaternion_tag, Q, CoordinateType, Dimension, std::true_type>
{
    static inline CoordinateType get(Q const* v)
    {
        return traits::access<typename std::remove_pointer<Q>::type, Dimension>::get(*v);
    }
    static inline void set(Q* v, CoordinateType const& value)
    {
        traits::access<typename std::remove_pointer<Q>::type, Dimension>::set(*v, value);
    }
};

template<typename RM, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<rotation_matrix_tag, RM, CoordinateType, I, J, std::false_type>
    : detail::indexed_access_non_pointer<RM, CoordinateType, I, J>
{};

template<typename RM, typename CoordinateType, std::size_t I, std::size_t J>
struct indexed_access<rotation_matrix_tag, RM, CoordinateType, I, J, std::true_type>
    : detail::indexed_access_pointer<RM, CoordinateType, I, J>
{};

} // namespace core_dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_CORE_ACCESS_HPP


#include <boost/compute/random/discrete_distribution.hpp>


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <armadillo>
using arma::datum;

namespace armadilloJava {

  class ExpectedDatum: public Expected {
    public:
      ExpectedDatum() {
        cout << "Compute ExpectedDatum() ... ";

        Mat<double> expected = {
          datum::pi,
          datum::e,
          datum::euler,
          datum::gratio,
          datum::sqrt2,
          datum::eps,
          datum::log_min,
          datum::log_max,
          datum::nan,
          datum::inf,
          datum::m_u,
          datum::N_A,
          datum::k,
          datum::k_evk,
          datum::a_0,
          datum::mu_B,
          datum::Z_0,
          datum::G_0,
          datum::k_e,
          datum::eps_0,
          datum::m_e,
          datum::eV,
          datum::ec,
          datum::F,
          datum::alpha,
          datum::alpha_inv,
          datum::K_J,
          datum::mu_0,
          datum::phi_0,
          datum::R,
          datum::G,
          datum::h,
          datum::h_bar,
          datum::m_p,
          datum::R_inf,
          datum::c_0,
          datum::sigma,
          datum::R_k,
          datum::b
        };
        save<double>("Datum", expected);

        cout << "done." << endl;
      }
  };
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/core/utility/config.hpp>
#include <nt2/core/utility/mt19937stream.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

namespace nt2
{
  void mt19937stream_::seed(uint32_t s)
  {
    generator_.seed(s);
  }

  void mt19937stream_::rand(double* data, std::size_t i0, std::size_t i1)
  {
    boost::random::uniform_real_distribution<double> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void mt19937stream_::rand(float*  data, std::size_t i0, std::size_t i1)
  {
    boost::random::uniform_real_distribution<float> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void mt19937stream_::randn(double* data, std::size_t i0, std::size_t i1)
  {
    boost::random::normal_distribution<double> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  void mt19937stream_::randn(float*  data, std::size_t i0, std::size_t i1)
  {
    boost::random::normal_distribution<float> dist;
    for(std::size_t i=i0;i<i1;++i) data[i] = dist(generator_);
  }

  mt19937stream_::~mt19937stream_()
  {
  }

  nt2::randstream_* mt19937stream()
  {
    static nt2::mt19937stream_ instance;
    return &instance;
  }
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/transpose_graph.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, bidirectionalS,
    property < vertex_name_t, char > > graph_t;

  enum { a, b, c, d, e, f, g, N };
  graph_t G(N);
  property_map < graph_t, vertex_name_t >::type
    name_map = get(vertex_name, G);
  char name = 'a';
  graph_traits < graph_t >::vertex_iterator v, v_end;
  for (boost::tie(v, v_end) = vertices(G); v != v_end; ++v, ++name)
    name_map[*v] = name;

  typedef std::pair < int, int >E;
  E edge_array[] = { E(a, c), E(a, d), E(b, a), E(b, d), E(c, f),
    E(d, c), E(d, e), E(d, f), E(e, b), E(e, g), E(f, e), E(f, g)
  };
  for (int i = 0; i < 12; ++i)
    add_edge(edge_array[i].first, edge_array[i].second, G);

  print_graph(G, name_map);
  std::cout << std::endl;

  graph_t G_T;
  transpose_graph(G, G_T);

  print_graph(G_T, name_map);

  graph_traits < graph_t >::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(G); ei != ei_end; ++ei)
    assert(edge(target(*ei, G), source(*ei, G), G_T).second == true);
  return 0;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 *    Notes
 *      Need to add bounds checking with respect to targetValue for
 *      computeNearestLeftNeighborUsingBinarySearch( ).
 */

#include <map>
#include <iterator>
#include <Eigen/Core>

namespace tudat
{
namespace basic_mathematics
{

//! Nearest left neighbor binary search.
int computeNearestLeftNeighborUsingBinarySearch(
        const Eigen::VectorXd& vectorOfSortedData,
        const double targetValueInVectorOfSortedData )
{
    // Declare local variables.
    // Declare bounds of vector of sorted data and current position.
    int leftLimitOfVectorOfSortedData = 0;
    int rightLimitOfVectorOfSortedData = vectorOfSortedData.rows( ) - 1;
    int currentPositionInVectorOfSortedData;

    // Check if data is sorted in ascending order.
    // ( true if ascending, else false ).
    bool isVectorOfSortedDataAscending
            = ( vectorOfSortedData[ rightLimitOfVectorOfSortedData ]
                >= vectorOfSortedData[ leftLimitOfVectorOfSortedData ] );

    // Loop through vector of sorted data until left and right limits
    // are neighbours.
    while ( rightLimitOfVectorOfSortedData
            - leftLimitOfVectorOfSortedData > 1 )
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInVectorOfSortedData
                = ( rightLimitOfVectorOfSortedData
                    + leftLimitOfVectorOfSortedData ) >> 1;

        // Check which limit to replace ( if ascending and target datum
        // is in right half, replace left limit ).
        if ( targetValueInVectorOfSortedData
             >= vectorOfSortedData[ currentPositionInVectorOfSortedData ]
             && isVectorOfSortedDataAscending )
        {
            // Set left limit to current position in vector of sorted data.
            leftLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }

        else
        {
            // Set right limit to current position in vector of sorted data.
            rightLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }
    }

    // Set current position to left limit.
    currentPositionInVectorOfSortedData = leftLimitOfVectorOfSortedData;

    // Return current position in vector.
    return currentPositionInVectorOfSortedData;
}

//! Nearest neighbor binary search.
int computeNearestNeighborUsingBinarySearch(
        const Eigen::VectorXd& vectorOfSortedData,
        const double targetValueInVectorOfSortedData )
{
    // Declare local variables.
    // Declare bounds of vector of sorted data and current position.
    int leftLimitOfVectorOfSortedData = 0;
    int rightLimitOfVectorOfSortedData = vectorOfSortedData.rows( ) - 1;
    int currentPositionInVectorOfSortedData;

    // Check if data is sorted in ascending order.
    // ( true if ascending, else false ).
    bool isVectorOfSortedDataAscending
            = ( vectorOfSortedData[ rightLimitOfVectorOfSortedData ]
                >= vectorOfSortedData[ leftLimitOfVectorOfSortedData ] );

    // Loop through vector of sorted data until left and right limits
    // are neighbours.
    while ( rightLimitOfVectorOfSortedData
            - leftLimitOfVectorOfSortedData > 1 ) // No rounding off errors because limits are integers
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInVectorOfSortedData
                = ( rightLimitOfVectorOfSortedData
                    + leftLimitOfVectorOfSortedData ) >> 1;

        // Check which limit to replace ( if ascending and target datum
        // is in right half, replace left limit ).
        if ( targetValueInVectorOfSortedData
             >= vectorOfSortedData[ currentPositionInVectorOfSortedData ]
             && isVectorOfSortedDataAscending )
        {
            // Set left limit to current position in vector of sorted data.
            leftLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }

        else
        {
            // Set right limit to current position in vector of sorted data.
            rightLimitOfVectorOfSortedData = currentPositionInVectorOfSortedData;
        }
    }

    // Set current position to left or right limit, whichever is closer.
    currentPositionInVectorOfSortedData = leftLimitOfVectorOfSortedData;
    if( ( ( vectorOfSortedData[ rightLimitOfVectorOfSortedData ] +
              vectorOfSortedData[ leftLimitOfVectorOfSortedData ] ) / 2.0 ) < targetValueInVectorOfSortedData )
    {
        currentPositionInVectorOfSortedData = rightLimitOfVectorOfSortedData;
    }

    else
    {
        currentPositionInVectorOfSortedData = leftLimitOfVectorOfSortedData;
    }

    // Return current position in vector.
    return currentPositionInVectorOfSortedData;
}

//! Nearest left neighbor binary search.
int computeNearestLeftNeighborUsingBinarySearch(
        const std::map< double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetValueInMapOfData )
{
    // Declare local variables.
    // Declare bounds of key of map of data and current position.
    int leftLimitOfKeyOfMapOfData = 0;
    int rightLimitOfKeyOfMapOfData = sortedIndepedentAndDependentVariables
                                     .size( ) - 1;
    int currentPositionInKeyOfMapOfData;

    // Declare map iterator
    std::map < double, Eigen::VectorXd >::const_iterator mapIterator;

    // Loop through vector of sorted data until left and right limits
    // are neighbours
    while ( rightLimitOfKeyOfMapOfData - leftLimitOfKeyOfMapOfData > 1 )
    {
        // Compute midpoint ( bitshift is same as division by 2.0 ).
        currentPositionInKeyOfMapOfData
                = ( rightLimitOfKeyOfMapOfData
                   + leftLimitOfKeyOfMapOfData ) >> 1;

        // Set map iterator to begin begin of map of sorted independent and
        // dependent variables.
        mapIterator = sortedIndepedentAndDependentVariables.begin( );

        // Advance iterator to location of current position in key of map of
        // data.
        advance( mapIterator, currentPositionInKeyOfMapOfData );

        // Check that target value lies to the right of lower bound.
        if ( targetValueInMapOfData
             >= mapIterator->first )
        {
            // Set left limit to current position in map of data.
            leftLimitOfKeyOfMapOfData = currentPositionInKeyOfMapOfData;
        }

        else
        {
            // Set right limit to current position in map of data.
            rightLimitOfKeyOfMapOfData = currentPositionInKeyOfMapOfData;
        }
    }

    // Set current position to left limit.
    currentPositionInKeyOfMapOfData = leftLimitOfKeyOfMapOfData;

    // Return current position in map of data.
    return currentPositionInKeyOfMapOfData;
}

} // namespace basic_mathematics
} // namespace tudat


// Boost.Geometry

// Copyright (c) 2019-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include "common.hpp"

#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/simplify.hpp>

template <typename G>
inline void simplify(G const& g1, G & g2)
{
    bg::simplify(g1, g2, 1, bg::strategy::simplify::douglas_peucker<geom::point, bg::strategy::distance::projected_point<> >());
    bg::simplify(g1, g2, 1, bg::strategy::simplify::douglas_peucker<geom::point, bg::strategy::distance::cross_track<> >());
    bg::simplify(g1, g2, 1, bg::strategy::simplify::douglas_peucker<geom::point, bg::strategy::distance::geographic_cross_track<> >());

    bg::simplify(g1, g2, 1, bg::strategies::simplify::cartesian<>());
    bg::simplify(g1, g2, 1, bg::strategies::simplify::spherical<>());
    bg::simplify(g1, g2, 1, bg::strategies::simplify::geographic<>());
}

template <typename G, typename P>
inline void centroid(G const& g, P & p)
{
    bg::centroid(g, p, bg::strategies::centroid::cartesian<>());
    bg::centroid(g, p, bg::strategies::centroid::spherical<>());
    bg::centroid(g, p, bg::strategies::centroid::geographic<>());
}

int test_main(int, char*[])
{
    geom g, o;

    ::centroid(g.b, o.pt);

    ::simplify(g.pt, o.pt);
    ::simplify(g.ls, o.ls);
    ::simplify(g.mls, o.mls);
    ::simplify(g.r, o.r);
    ::simplify(g.po, o.po);
    ::simplify(g.mpo, o.mpo);

    return 0;
}


#include "tools/partition_graph_edit_distance.hpp"

#include <set>
#include <deque>
#include <algorithm>
#include <boost/foreach.hpp>
#include <utility> // for std::pair
#include "tools/debug.hpp"

namespace biggles {

std::ostream& operator<<(std::ostream& os, const link_t& l) {
    os
        << "(" << x(l.first) << ", " << y(l.first) << ", " << t(l.first) << ")"
        << " -> "
        << "(" << x(l.second) << ", " << y(l.second) << ", " << t(l.second) << ")";
    return os;
}

bool operator<(const link_t& l1, const link_t& l2) {
    return l1.first < l2.first or (l1.first == l2.first and l1.second < l2.second);
}
bool operator==(const link_t& l1, const link_t& l2) {
    return l1.first == l2.first and l1.second == l2.second;
}

link_set_t get_links(const partition& part) {
    link_set_t result;
    BOOST_FOREACH (const shared_const_track_ptr& tp, part.tracks()) {
        const observation_collection& obs_coll = tp->observations();
        BOOST_ASSERT(obs_coll.size() > 1);
        observation_collection::const_iterator fir = obs_coll.begin();
        observation_collection::const_iterator sec = fir;
        ++sec;
        for (; sec != obs_coll.end(); ++fir, ++sec) {
            insert_result_t insert_result = result.insert(link_t(*fir, *sec));
            BOOST_ASSERT(insert_result.second);
        }
    }
    return result;
}

float partition_ged(const partition& part1, const partition& part2) {
    link_set_t links1 = get_links(part1);
    link_set_t links2 = get_links(part2);
    std::deque<link_t> sym_diff(links1.size() + links2.size());
    std::deque<link_t>::iterator end_iter = std::set_symmetric_difference(
        links1.begin(), links1.end(), links2.begin(), links2.end(), sym_diff.begin());
    return std::distance(sym_diff.begin(), end_iter);
}

float map_dot_product(numeric_partition_t::iterator b1, numeric_partition_t::iterator e1,
    numeric_partition_t::iterator b2)
{
    float sum = 0.f;
    while (b1 != e1) {
        sum += (b1++->second)*(b2++->second);
    }
    return sum;
}

void PartitionMeanVariance::add_partition(const partition& part1) {
    numeric_partition_t npart1;
    BOOST_FOREACH (const link_t link, get_links(part1)) {
        npart1.insert(npart1.end(), std::make_pair(link, 1.f));
    }
    add_partition(npart1);
}

void PartitionMeanVariance::rem_partition(const partition& part1) {
    numeric_partition_t npart1;
    BOOST_FOREACH (const link_t link, get_links(part1)) {
        npart1.insert(npart1.end(), std::make_pair(link, 1.f));
    }
    rem_partition(npart1);
}

void PartitionMeanVariance::add_partition(const numeric_partition_t& part1) {
    _n++;
    numeric_partition_t delta(part1);
    for (numpart_iter it = _mean.begin(); it != _mean.end(); ++it)
        delta[it->first] -= it->second; // missing keys will be created;
    for (numpart_iter it = delta.begin(); it != delta.end(); ++it)
        _mean[it->first] += (it->second)/_n;
    numeric_partition_t delta2(part1);
    for (numpart_iter it = _mean.begin(); it != _mean.end(); ++it)
        delta2[it->first] -= it->second; // missing keys will be created
    _m2 += map_dot_product(delta.begin(), delta.end(), delta2.begin());
}

void PartitionMeanVariance::rem_partition(const numeric_partition_t& part1) {
    numeric_partition_t delta2(part1);
    for (numpart_iter it = _mean.begin(); it != _mean.end(); ++it) {
        delta2[it->first] -= it->second; // missing keys will be created
        it->second *= _n; // n*mean
    }
    for (numpart_citer it = part1.begin(); it != part1.end(); ++it)
        _mean[it->first] -= it->second;
    _n--;
    numeric_partition_t delta(part1);
    for (numpart_iter it = _mean.begin(); it != _mean.end(); ++it) {
        it->second /= _n;
        delta[it->first] -= it->second;
    }
    _m2 -= map_dot_product(delta.begin(), delta.end(), delta2.begin());
}

float PartitionGelmanRubin::get() const {
    PartitionMeanVariance between_chain;
    float m_chains = _within_chain_mv.size();
    float n_samples = _within_chain_mv[0].n();
    for (size_t i = 1; i < m_chains; ++i)
        BOOST_ASSERT(n_samples == _within_chain_mv[i].n());
    float sum = 0.f;
    for (pmv_iter it = _within_chain_mv.begin(); it != _within_chain_mv.end(); ++it) {
        between_chain.add_partition(it->mean());
        sum += it->var();
    }
    float W = sum/m_chains;
    float B_by_n = between_chain.var();
    float var_estimate = (n_samples - 1.f)/n_samples * W + B_by_n;
    return sqrt(var_estimate/W);
}

} // namespace biggles


#include <iostream>
#include <vector>
#include <memory>
#include <string>
#include <Eigen/Dense>
#include "../include/trainer.h"
#include "../include/optimizer.h"
#include "../include/model.h"
#include "../include/utils.h"
#include "../datasets/include/mnist.h"
#include <matplotlibcpp.h>
#include <picojson.h>

int main()
{
    using namespace Eigen;
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::string;
    using std::vector;
    using std::iota;
    using std::shared_ptr;
    using std::make_shared;
    namespace plt = matplotlibcpp;

    string filepath = "./test/test_trainer.json";

    picojson::object obj;

    int err;

    err = load_json(filepath, obj);

    int input_size;
    int hidden_size;
    int output_size;
    int batch_size;
    double learning_rate;
    int epochs;

    // リストを読み込む形になっていないのでそこは再度作成する
    if (err)
    {
        input_size = 28 * 28;
        hidden_size = 50;
        output_size = 10;
        batch_size = 100;
        learning_rate = 0.1;
        epochs = 2;
    }
    else // こっちが正常系
    {
        input_size = obj["input_size"].get<double>();
        hidden_size = obj["hidden_size"].get<double>();
        output_size = obj["output_size"].get<double>();
        batch_size = obj["batch_size"].get<double>();
        learning_rate = obj["learning_rate"].get<double>();
        epochs = obj["epochs"].get<double>();
    }

    cout << "====== Training Parameters ======" << endl;
    cout << "Input Size: " << input_size << endl;
    cout << "Hidden Size: " << hidden_size << endl;
    cout << "Output Size: " << output_size << endl;
    cout << "Batch Size: " << batch_size << endl;
    cout << "Leaning Rate: " << learning_rate << endl;
    cout << "Number of Epochs: " << epochs << endl;

    // model_creator()とか実装して、それに基づいてshared_ptrを返すようにした方が作りやすい？(I/Fを気にしなくて済む)
    // 可変長引数の仕組みとテンプレート関数の仕組みを確認して作成する。一旦無視してやってみる。
    // auto model = make_shared<TwoLayerMLP>(input_size, hidden_size, output_size);
    vector<int> hidden_list; // MultiLayerModelに差し替えるとき必要
    hidden_list.push_back(hidden_size);
    hidden_list.push_back(hidden_size);
    hidden_list.push_back(hidden_size);
    // auto model = make_shared<MultiLayerModel>(input_size, hidden_list, output_size, 0.1, "sigmoid", "sigmoid");
    auto model = make_shared<MultiLayerModel>(input_size,
                                              hidden_list,
                                              output_size,
                                              0.01,
                                              "relu",
                                              "relu",
                                              true,
                                              0.5,
                                              true);
    auto optimizer = make_shared<SGD>(learning_rate);
    // auto optimizer = make_shared<Adam>(learning_rate*0.01);
    auto dataset = make_shared<MnistEigenDataset>(batch_size);

    Trainer trainer(model, optimizer, dataset, epochs=epochs);

    // cout << *(model->params["b1"]) << endl;

    // // model->get_params()の返り値を利用している場合、ちゃんとモデルのパラメータが更新されるかの確認
    // for (int i=0; i < 100; i++){
    //     trainer.train_step();
    // }

    cout << "======= Training Start! =======" << endl;

    trainer.train();

    cout << "Train acc history: ";
    for (const auto &item : trainer.train_acc_history)
    {
        cout << item << " ";
    }
    cout << endl;

    cout << "Test acc history: ";
    for (const auto &item : trainer.test_acc_history)
    {
        cout << item << " ";
    }
    cout << endl;

    // ------------------------------------------
    //              Visualization
    // ------------------------------------------
    vector<int> loss_counter(trainer.train_loss_history.size());
    iota(loss_counter.begin(), loss_counter.end(), 1);
    plt::title("Loss History");
    plt::plot(loss_counter, trainer.train_loss_history, "b");
    plt::grid(true);
    plt::save("Mnist_Learning_curve_by_Trainer_Class");

    return 0;
}


#include <boost/graph/adjacency_matrix.hpp>


#include <boost/graph/adjacency_list.hpp>
#include <string>
typedef std::string vertex_t;

typedef boost::adjacency_list<
    boost::vecS
    , boost::vecS
    , boost::bidirectionalS
    , vertex_t
> graph_type;

#include <boost/utility/string_ref.hpp>
#include <iostream>

inline void find_and_print(
    const graph_type& graph, boost::string_ref name)
{
    typedef typename boost::graph_traits<
        graph_type
    >::vertex_iterator vert_it_t;

    vert_it_t it, end;
    boost::tie(it, end) = boost::vertices(graph);

    typedef typename boost::graph_traits<
        graph_type
    >::vertex_descriptor desc_t;

    for (; it != end; ++ it) {
        const desc_t desc = *it;
        const vertex_t& vertex = boost::get(
            boost::vertex_bundle, graph
        )[desc];

        if (vertex == name.data()) {
            break;
        }
    }

    assert(it != end);
    std::cout << name << '\n';
} /* find_and_print */

int main() {
    graph_type graph;

    static const std::size_t vertex_count = 5;
    graph.m_vertices.reserve(vertex_count);
    /*
    C++ -> STL -> Boost -> C++ guru <- C
    */

    typedef boost::graph_traits<
        graph_type
    >::vertex_descriptor descriptor_t;

    descriptor_t cpp
        = boost::add_vertex(vertex_t("C++"), graph);
    descriptor_t stl
        = boost::add_vertex(vertex_t("STL"), graph);
    descriptor_t boost
        = boost::add_vertex(vertex_t("Boost"), graph);
    descriptor_t guru
        = boost::add_vertex(vertex_t("C++ guru"), graph);
    descriptor_t ansic
        = boost::add_vertex(vertex_t("C"), graph);

    BOOST_STATIC_ASSERT((boost::is_same<descriptor_t, std::size_t>::value));

    boost::add_edge(cpp, stl, graph);
    boost::add_edge(stl, boost, graph);
    boost::add_edge(boost, guru, graph);
    boost::add_edge(ansic, guru, graph);

    find_and_print(graph, "Boost");
    find_and_print(graph, "C++ guru");
} // end of main


#pragma once

#include "IViaPoints.hpp"

#include <boost/numeric/odeint.hpp>

using namespace gazebo;

namespace cardsflow_gazebo
{
	class ITendon;

	class MeshWrapping : public IViaPoints
	{

    public:
        MeshWrapping();

        virtual void UpdateForcePoints();

        virtual void CalculateForce();

    public:
        ignition::math::Vector3d prevCoord;
        ignition::math::Vector3d nextCoord;
        ignition::math::Vector3d wrappingDir;
        std::map<int, ignition::math::Vector3d> vertices;
        std::set<ignition::math::Vector3d> facets;
        ignition::math::Vector3d x_axis;
        ignition::math::Vector3d y_axis;
        ignition::math::Vector3d z_axis;
        std::map<int, ignition::math::Vector3d> geodesicPathPoints;
        std::map<int, ignition::math::Vector3d> halfSpace;
        std::map<int, ignition::math::Vector3d> convexHullVertices;
        std::map<int, ignition::math::Vector3d> convexHullFacets;
        std::map<int, ignition::math::Vector3d> facetNormals;
        double alphaMax;

        void CoordinateFrame();
        void HalfSpace();
        void ConvexEnvelope();
        void GeodesicPath();

	};
}