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

/*
 Copyright (C) 2015 Sebastian Schlenkrich

*/

#ifndef quantlib_templatetdstochvolmodel_hpp
#define quantlib_templatetdstochvolmodel_hpp

#include <complex>
#include <ql/shared_ptr.hpp>
#include <boost/function.hpp>
#include <ql/errors.hpp>
#include <ql/experimental/templatemodels/auxilliaries/auxilliariesT.hpp>
#include <ql/experimental/templatemodels/auxilliaries/gausslobattoT.hpp>
#include <ql/experimental/templatemodels/auxilliaries/complexT.hpp>
#include <ql/experimental/templatemodels/auxilliaries/solver1dT.hpp>
#include <ql/experimental/templatemodels/stochasticprocessT.hpp>
#include <ql/experimental/templatemodels/stochvol/hestonmodelT.hpp>

#define _MIN_( a, b ) ( (a) < (b) ? (a) : (b) )
#define _MAX_( a, b ) ( (a) > (b) ? (a) : (b) )

namespace QuantLib {

    // general stochastic volatility model interface with time-dependent parameters and parameter averaging pricing formula
    //
    //    dS(t) = lambda(t) [ b(t) S(t) + (1-b(t)) L ] sqrt[z(t)] dW(t)
    //    dz(t) = theta [ m - z(t) ] dt + eta(t) sqrt[z(t)] dZ(t)
    //    dW(t) dZ(t) = rho dt
    //
    template <class DateType, class PassiveType, class ActiveType>
    class TimeDependentStochVolModelT : public StochasticProcessT<DateType,PassiveType,ActiveType> {
    public:
        // abstract inspectors
        virtual ActiveType  lambda( const DateType t) = 0;
        virtual ActiveType  b(      const DateType t) = 0;
        virtual ActiveType  L()                       = 0;
        virtual ActiveType  theta()                   = 0;
        virtual ActiveType  m()                       = 0;
        virtual ActiveType  eta(    const DateType t) = 0;
        virtual ActiveType  z0()                      = 0;
        virtual ActiveType  rho()                     = 0;
        virtual ActiveType  S0()                      = 0;

        // from base class
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VecD;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VecP;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VecA;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::MatA;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VolEvolv;
        using StochasticProcessT<DateType, PassiveType, ActiveType>::volEvolv;

        // helper functions for vol averaging, Piterbarg, 10.2.4
        inline static ActiveType A_CIR ( ActiveType c1, ActiveType c2, ActiveType z0, ActiveType theta, ActiveType eta, DateType dt ) {
            ActiveType gamma = sqrt((theta*theta + 2.0 * eta*eta * c2));
            ActiveType t1 = theta*z0/eta/eta * (theta + gamma)*dt;
            ActiveType t2 = 1.0 + (theta + gamma + c1 * eta*eta) * (exp(gamma * dt) - 1.0) / 2.0 / gamma;
            QL_REQUIRE(t2>0,"A_CIR: require positive log()-argument");
            return t1 - 2.0*theta*z0/eta/eta*log(t2);
        }

        inline static ActiveType B_CIR ( ActiveType c1, ActiveType c2, ActiveType z0, ActiveType theta, ActiveType eta, DateType dt ) {
            ActiveType gamma = sqrt((theta*theta + 2.0 * eta*eta * c2));
            ActiveType emGdt = exp(-gamma * dt);
            ActiveType numer = (2.0*c2 - theta*c1)*(1.0 - emGdt) + gamma*c1*(1.0 + emGdt);
            ActiveType denum = (theta + gamma + c1*eta*eta) * (1.0 - emGdt) + 2.0*gamma*emGdt;
            return numer / denum;
        }

        // abstract averaging formula definitions
        inline virtual ActiveType  averageLambda( const DateType T) = 0;
        inline virtual ActiveType  averageB     ( const DateType T) = 0;
        inline virtual ActiveType  averageEta   ( const DateType T) = 0;

        // undiscounted expectation of vanilla payoff
        inline ActiveType vanillaOption(const PassiveType forwardPrice,
                                        const PassiveType strikePrice,
                                        const DateType    term,
                                        const int         callOrPut,
                                        const PassiveType accuracy,
                                        const size_t      maxEvaluations) {
                StochVolModelT<DateType,PassiveType,ActiveType> model(averageLambda(term), averageB(term),L(),theta(),m(),averageEta(term),z0(),rho());
                return model.vanillaOption( forwardPrice, strikePrice, term, callOrPut, accuracy, maxEvaluations );
        }

        // conditional moments of vol process used for z-integration, Piterbarg, 8.3.3.
        // E[ z(T) | z(t) ]
        inline virtual ActiveType expectationZ( DateType t, ActiveType zt, DateType dT ) {
            return z0() + (zt - z0())*exp(-theta()*dT);
        }
        // Var[ z(T) | z(t) ]
        inline virtual ActiveType varianceZ( DateType t, ActiveType zt, DateType dT ) {
            ActiveType expmThDT = exp(-theta()*dT);
            ActiveType onemETDT = 1 - expmThDT;
            ActiveType eta2oThe = eta(t+dT/2.0)*eta(t+dT/2.0)/theta();  // approx eta(t)=eta for s \in [t, t+dT]
            return zt*eta2oThe*expmThDT*onemETDT + z0()*eta2oThe/2.0*onemETDT*onemETDT;
        }

        // stochastic process interface
        // dimension of X
        inline virtual size_t size() { return 2; }
        // stochastic factors of x and z (maybe distinguish if trivially eta=0)
        inline virtual size_t factors() { return 2; }
        // initial values for simulation
        inline virtual VecP initialValues() {
            VecP X(2);
            X[0] = S0();
            X[1] = z0();
            return X;
        }
        // a[t,X(t)]
        inline virtual VecA drift( const DateType t, const VecA& X) {
            VecA a(2);
            // S-variable drift-less
            a[0] = 0.0;
            // z-variable theta [ m - z(t)^+ ]  (full truncation)
            a[1] = theta()*(m() - ((X[1]>0)?(X[1]):(0.0)));
            return a;
        }
        // b[t,X(t)]
        inline virtual MatA diffusion( const DateType t, const VecA& X) {
            MatA B(2);
            B[0].resize(2);
            B[1].resize(2);
            ActiveType sqrtz = ( (X[1]>0) ? (sqrt(X[1])) : (0.0) );   // full truncation
            // S-variable lambda(t) [ b(t) S(t) + (1-b(t)) L ] sqrt[z(t)] dW(t)
            B[0][0] = lambda(t) * (b(t) * X[0] + (1.0-b(t)) * L()) * sqrtz;
            B[0][1] = 0.0;
            // z-variable
            B[1][0] = rho()*eta(t)*sqrtz;
            B[1][1] = sqrt(1-rho()*rho())*eta(t)*sqrtz;
            // finished
            return B;
        }

        // integrate X1 = X0 + drift()*dt + diffusion()*dW*sqrt(dt)
        inline virtual void evolve( const DateType t0, const VecA& X0, const DateType dt, const VecD& dW, VecA& X1 ) {
            // ensure X1 has size of X0
            VecA a = drift(t0, X0);
            MatA b = diffusion(t0, X0);
            // S-variable
            X1[0] = X0[0] + a[0]*dt + b[0][0]*dW[0]*sqrt(dt);
            // z-variable
            if (volEvolv()==VolEvolv::FullTruncation) {
                X1[1] = X0[1] + a[1]*dt + (b[1][0]*dW[0]+b[1][1]*dW[1])*sqrt(dt);
                return;
            }
            if (volEvolv()==VolEvolv::LogNormalApproximation) {
                ActiveType e = expectationZ(t0, X0[1], dt);
                ActiveType v = varianceZ(t0, X0[1], dt);
                ActiveType dZ = rho()*dW[0] + sqrt(1-rho()*rho())*dW[1];
                ActiveType si = sqrt(log(1.0 + v/e/e));
                ActiveType mu = log(e) - si*si/2.0;
                X1[1] = exp(mu + si*dZ);
            }
            return;
        }

        // embeded classes

        class PieceWiseConstant  {
        protected:
            // check for dimensions
            bool isConsistent_;
            // time grid
            std::vector<DateType>     times_;
            // time-dependent model parameters
            std::vector<ActiveType>   lambda_;
            std::vector<ActiveType>   b_;
            std::vector<ActiveType>   eta_;
            // time-homogeneous model parameters
            ActiveType                L_;
            ActiveType                theta_;
            ActiveType                m_;
            ActiveType                z0_;
            ActiveType                rho_;
            ActiveType                S0_;
            // helper
            inline size_t idx( const DateType t ) { return TemplateAuxilliaries::idx(times_,t); }
        public:
            // constructor
            PieceWiseConstant( const std::vector<DateType>&    times,
                               const std::vector<ActiveType>&  lambda,
                               const std::vector<ActiveType>&  b,
                               const std::vector<ActiveType>&  eta,
                               const ActiveType                L,
                               const ActiveType                theta,
                               const ActiveType                m,
                               const ActiveType                z0,
                               const ActiveType                rho,
                               const ActiveType                S0 )
                : times_(times), lambda_(lambda), b_(b), eta_(eta), L_(L), theta_(theta), m_(m), z0_(z0), rho_(rho), S0_(S0) {
                isConsistent_ = true;
                QL_REQUIRE(times_.size()>0,"TemplateTimeDependentStochVolModel::PieceWiseConstant: non-empty times required");
                for (size_t k=0; k<times_.size()-1; ++k) QL_REQUIRE(times_[k]<times_[k+1],"TemplateTimeDependentStochVolModel::PieceWiseConstant: ascending time-grid required");
                QL_REQUIRE(lambda_.size()==times_.size(),"TemplateTimeDependentStochVolModel::PieceWiseConstant: lambda dimension mismatch");
                QL_REQUIRE(b_.size()     ==times_.size(),"TemplateTimeDependentStochVolModel::PieceWiseConstant: b dimension mismatch");
                QL_REQUIRE(eta_.size()   ==times_.size(),"TemplateTimeDependentStochVolModel::PieceWiseConstant: eta dimension mismatch");
            }
            // inspectors
            virtual ActiveType  lambda( const DateType t)  { return lambda_[idx(t)]; }
            virtual ActiveType  b(      const DateType t)  { return b_[idx(t)];      }
            virtual ActiveType  eta(    const DateType t)  { return eta_[idx(t)];    }
            virtual ActiveType  L()                        { return L_;              }
            virtual ActiveType  theta()                    { return theta_;          }
            virtual ActiveType  m()                        { return m_;              }
            virtual ActiveType  z0()                       { return z0_;             }
            virtual ActiveType  rho()                      { return rho_;            }
            virtual ActiveType  S0()                       { return S0_;            }

        };

        // averaging assuming piecewise constant parameters on ( T_k-1, T_k ]
        class MidPointIntegration {
        protected:
            // reference to model
            TimeDependentStochVolModelT* model_;
            // time grid for integration
            std::vector<DateType>               times_;
            inline std::vector<DateType> getTimes( const DateType T ) {
                std::vector<DateType> times;
                times.push_back(0.0);
                for (size_t k=0; k<times_.size(); ++k) {
                    if (times_[k]>0.0 && times_[k]<T) times.push_back(times_[k]);
                    if (times_[k]>=T) break;
                }
                times.push_back(T);
                return times;
            }

            class AverageLambdaObjective {
            protected:
                ActiveType z0_;
                ActiveType theta_;
                ActiveType eta_;
                DateType   dt_;
                ActiveType target_;
            public:
                AverageLambdaObjective ( const ActiveType z0,
                                         const ActiveType theta,
                                         const ActiveType eta,
                                         const DateType   dt,
                                         const ActiveType target )
                                         : z0_(z0), theta_(theta), eta_(eta), dt_(dt), target_(target) {}
                ActiveType operator() ( const ActiveType avLambda2c ) {
                    ActiveType A = A_CIR(0, -avLambda2c, z0_, theta_, eta_, dt_);
                    ActiveType B = B_CIR(0, -avLambda2c, z0_, theta_, eta_, dt_);
                    ActiveType res = A - B*z0_ - target_;
                    return res;
                }
            };

            // Prop. 9.1.2
            class RiccatiODE {
                TimeDependentStochVolModelT* model_;
                ActiveType v_;
                ActiveType u_;
                ActiveType averageB_;
            public:
                RiccatiODE (TimeDependentStochVolModelT* model, ActiveType v, ActiveType u, ActiveType averageB)
                    : model_(model), v_(v), u_(u), averageB_(averageB) {}
                void operator() (const DateType t, const std::vector<ActiveType>& y, std::vector<ActiveType>& fy) {
                    fy[0] = - model_->theta() * model_->z0() * y[1];
                    fy[1] = (model_->theta() - model_->rho()*model_->eta(t)*averageB_*u_*model_->lambda(t))*y[1]
                            - model_->eta(t)*model_->eta(t)/2.0*y[1]*y[1] - v_*model_->lambda(t)*model_->lambda(t);
                }
            };

        public:
            // constructor
            MidPointIntegration ( TimeDependentStochVolModelT* model,
                                  std::vector<DateType>        times )
                                  : model_(model), times_(times) { }

            virtual ~MidPointIntegration() = default;

            // averaging formula implementations
            virtual ActiveType  averageEta   ( const DateType T) {
                std::vector<DateType> times(getTimes(T));
                std::vector<ActiveType> f(times.size());
                std::vector<ActiveType> w(times.size()-1);
                f[f.size()-1] = 0.0;
                for (size_t k=f.size()-1; k>0; --k) {
                    ActiveType lambda = model_->lambda((times[k-1]+times[k])/2.0);
                    ActiveType tmp    = exp(-model_->theta()*(times[k]-times[k-1]));
                    f[k-1] = lambda*lambda/model_->theta()*(1.0 - tmp) + tmp*f[k];
                }
                ActiveType sum = 0.0;
                for (size_t k=0; k<w.size(); ++k) {
                    ActiveType lambda  = model_->lambda((times[k]+times[k+1])/2.0);
                    ActiveType lambda2 = lambda*lambda;
                    ActiveType theta   = model_->theta();
                    ActiveType theta2  = theta*theta;
                    w[k] = (f[k+1]*f[k+1] - f[k]*f[k])/2.0/theta +
                           (f[k+1] - f[k])*lambda2/theta2 +
                           (times[k+1]-times[k])*lambda2*lambda2/theta2;
                    w[k] *= 0.5;
                    sum  += w[k];
                }
                ActiveType eta2=0.0;
                for (size_t k=0; k<w.size(); ++k) {
                    ActiveType eta = model_->eta((times[k]+times[k+1])/2.0);
                    eta2 += w[k] * eta * eta;
                }
                eta2 = eta2 / sum;
                return sqrt(eta2);
            }

            virtual ActiveType  averageB( const DateType T) {
                std::vector<DateType> times(getTimes(T));
                std::vector<ActiveType> w(times.size()-1);
                ActiveType theta = model_->theta();
                ActiveType z0 = model_->z0();
                ActiveType sumLambda2dT = 0.0;
                ActiveType S1=0.0, S2=0.0, S3=0.0;
                ActiveType sum = 0.0;
                for (size_t k=0; k<w.size(); ++k) {
                    // v1
                    ActiveType lambda    = model_->lambda((times[k]+times[k+1])/2.0);
                    ActiveType lambda2   = lambda*lambda;
                    ActiveType eta       = model_->eta((times[k]+times[k+1])/2.0);
                    ActiveType lambda2dT = lambda2*(times[k+1]-times[k]);
                    ActiveType v1        = z0*z0*(times[k+1]-times[k])*(lambda2dT/2.0 + sumLambda2dT);
                    sumLambda2dT        += lambda2dT;
                    // v3, v4, v5
                    ActiveType expmThdT  = exp(-theta*(times[k+1]-times[k]));
                    ActiveType v3        = z0/theta*(S2+S3)*(1.0-expmThdT);
                    ActiveType v4        = (times[k+1]-times[k]) - (1.0-expmThdT)/theta -
                                           (1.0-expmThdT)*(1.0-expmThdT)/2.0/theta;
                    ActiveType laEtaTh   = lambda*eta/theta;
                    v4                  *= z0*laEtaTh*laEtaTh/2.0;
                    ActiveType theta2    = theta*theta;
                    ActiveType v5        = z0*lambda2/theta2/2.0*S1*(1.0-expmThdT)*(1.0-expmThdT);
                    // updating S1, S2, S3
                    S3                   = expmThdT * ( S3 + S1*lambda2/theta*(1.0-expmThdT) );
                    S2                   = expmThdT*S2 + laEtaTh*laEtaTh/2.0*(1.0-expmThdT)*(1.0-expmThdT);
                    S1                   = expmThdT*expmThdT*S1 + eta*eta/theta/2.0*(1.0 - expmThdT*expmThdT);
                    // gathering things together...
                    w[k]                 = lambda2 * ( v1 + v3 + v4 + v5 );
                    sum                 += w[k];
                }
                ActiveType b=0;
                for (size_t k=0; k<w.size(); ++k) {
                    b += w[k] * model_->b((times[k]+times[k+1])/2.0);
                }
                b = b / sum;
                return b;
            }

            virtual ActiveType  averageLambda     ( const DateType T) {
                ActiveType b   = averageB(T);
                ActiveType eta = averageEta(T);  // maybe better use time-dep eta
                std::vector<DateType> times(getTimes(T));
                // c = h''(zeta) / h'(zeta)
                ActiveType zeta = 0.0;
                for (size_t k=0; k<times.size()-1; ++k) {
                    ActiveType lambda    = model_->lambda((times[k]+times[k+1])/2.0);
                    zeta += lambda*lambda*(times[k+1]-times[k]);
                }
                ActiveType avLambda2 = zeta / (times[times.size()-1] - times[0]);
                zeta *= model_->z0();
                ActiveType c = -(b*b/4.0 + 1.0/zeta)/2.0;
                // Psi_{z lambda^2}
                ActiveType A = 0.0, B = 0.0;
                //std::vector<ActiveType> y1(2,0.0), y0(2,0.0);
                //RiccatiODE ode(model_,c,0.0,b);
                for (size_t k=times.size()-1; k>0; --k) {
                    DateType  t = (times[k]+times[k-1])/2.0;
                    DateType dt = (times[k]-times[k-1]);
                    ActiveType lambda = model_->lambda(t);
                    A = A + A_CIR(B, -c*lambda*lambda, model_->z0(), model_->theta(), eta, dt);
                    B = B_CIR(B, -c*lambda*lambda, model_->z0(), model_->theta(), eta, dt);
                    // Riccati ODE via Runge Kutta Method
                    //y1 = y0;
                    //TemplateAuxilliaries::rungeKuttaStep<DateType,ActiveType>(y1,times[k],ode,times[k-1]-times[k],y0);
                }
                ActiveType target = A - B*model_->z0();
                //ActiveType target = y0[0] + y0[1]*model_->z0();
                AverageLambdaObjective f(model_->z0(), model_->theta(), eta, T, target);
                ActiveType avLambda2c = avLambda2 * c;
                avLambda2c = TemplateAuxilliaries::solve1d<ActiveType>(f, 1.0e-8, avLambda2c, avLambda2c, 10);
                ActiveType avLambda = sqrt(avLambda2c / c);
                return avLambda;
            };
        };
    };

    // piecewise constant parameters and numerical integration
    template <class DateType, class PassiveType, class ActiveType>
    class PWCAnalytical : public TimeDependentStochVolModelT<DateType,PassiveType,ActiveType> {
        // from base class
        using typename TimeDependentStochVolModelT<DateType, PassiveType, ActiveType>::PieceWiseConstant;
        using typename TimeDependentStochVolModelT<DateType, PassiveType, ActiveType>::MidPointIntegration;
        using typename TimeDependentStochVolModelT<DateType, PassiveType, ActiveType>::VolEvolv;

    private:
        ext::shared_ptr<PieceWiseConstant>    pwc_;
        ext::shared_ptr<MidPointIntegration>  mp_;
        VolEvolv volEvolv_;
    public:
        PWCAnalytical(const std::vector<DateType>&    times,
                        const std::vector<ActiveType>&  lambda,
                        const std::vector<ActiveType>&  b,
                        const std::vector<ActiveType>&  eta,
                        const ActiveType                L,
                        const ActiveType                theta,
                        const ActiveType                m,
                        const ActiveType                z0,
                        const ActiveType                rho,
                        const ActiveType                S0,
                        const VolEvolv                  volEvolv = VolEvolv::FullTruncation )
            :    pwc_(new PieceWiseConstant(times,lambda,b,eta,L,theta,m,z0,rho,S0)),
                    mp_(new MidPointIntegration(this,times)), volEvolv_(volEvolv) {}
        // inspectors
        virtual ActiveType  lambda( const DateType t)  { return pwc_->lambda(t);    }
        virtual ActiveType  b(      const DateType t)  { return pwc_->b(t) ;        }
        virtual ActiveType  eta(    const DateType t)  { return pwc_->eta(t);       }
        virtual ActiveType  L()                        { return pwc_->L();          }
        virtual ActiveType  theta()                    { return pwc_->theta();      }
        virtual ActiveType  m()                        { return pwc_->m();          }
        virtual ActiveType  z0()                       { return pwc_->z0();         }
        virtual ActiveType  rho()                      { return pwc_->rho();        }
        virtual ActiveType  S0()                       { return pwc_->S0();         }
        virtual VolEvolv    volEvolv()                 { return volEvolv_;          }
        // averaging formula implementations
        virtual ActiveType  averageLambda( const DateType T) { return mp_->averageLambda( T ); }
        virtual ActiveType  averageB     ( const DateType T) { return mp_->averageB( T );      }
        virtual ActiveType  averageEta   ( const DateType T) { return mp_->averageEta( T );    }
    };

}

#undef _MIN_
#undef _MAX_

#endif  /* ifndef quantlib_templatetdstochvolmodel_hpp */


#include <fstream>
#include <bits/stdc++.h>
#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <algorithm>
#include <math.h>
#include <cstring>
#include <queue>
#include <stack>
#include <map>
#include <Eigen/Dense>
using namespace std;
#define INF 0x3f3f3f3f
typedef Eigen::Vector3i beacon;
typedef Eigen::Matrix3i Perm;
typedef tuple<int,int,int> triplet;

vector<beacon> generatePerm(beacon input){

    auto roll = [](beacon in){
        beacon res = {in(0),in(2),-in(3)};
        return res;
    };
    auto turn = [](beacon in){
        beacon res = {-in(1),in(0),in(2)};
        return res;
    };
    vector<beacon> res;
    for(int c = 0; c < 2; c++){
        for(int s = 0; s < 3; s++){
            input = roll(input);
            res.push_back(input);
            for(int i = 0; i < 3; i++){
                input = turn(input);
                res.push_back(input);
            }
        }
        input = roll(turn(roll(input)));
    }
    return res;
}

bool read_input(vector<vector<beacon>> &input){
    string filename = "inputPartOne.txt";
    ifstream input_file(filename);
    int i = 0;
    string temp1;
    getline(input_file,temp1);
    vector<beacon> row;
    for(string line;getline(input_file, line);){
        int n = line.size();
        char arr[n+1];
        strcpy(arr,line.c_str());
        if(arr[0] == '-' && arr[1] == '-' && arr[2] == '-'){
            i++;
            continue;
        }
        if(n == 0){
            input.push_back(row);
            row.clear();
            continue;
        }
        stringstream ss(line);
        std::string s;

        getline(ss,s,',');
        int x = stoi(s);
        getline(ss,s,',');
        int y = stoi(s);
        getline(ss,s,',');
        int z = stoi(s);
        beacon t = {x,y,z};
        row.push_back(t);
    }
    return false;
}

triplet convBeacon(beacon in){
    return make_tuple(in(0),in(1),in(2));
}
beacon convTriplet(triplet in){
    int x,y,z;
    std::tie(x,y,z) = in;
    beacon res;
    res << x,y,z;
    return res;
}
beacon calc(beacon a, beacon b){
    beacon res;
    res = a + b ;

    return res;
}
void solve(vector<vector<beacon>> input){
    vector<Perm> rot;
    beacon p;
    vector<beacon> flips;
    Perm one, two, three, four, five , six;
    beacon a,b,c,d,e,f,g,h;
    a << 1,1,1;
    b << 1,1,-1; c << 1,-1,1; d << -1,1,1; e << 1,-1,-1; f << -1,-1,1;
    g << -1,1,-1; h << -1,-1,-1;
    flips.push_back(a);flips.push_back(b);flips.push_back(c);flips.push_back(d);
    flips.push_back(e);flips.push_back(f);flips.push_back(g);flips.push_back(h);
    one << 1, 0, 0 , 0 , 1 , 0, 0 , 0 , 1;
    two << 1,0,0,0,0,1,0,1,0;
    three << 0,1,0,1,0,0,0,0,1;
    four << 0,1,0,0,0,1,1,0,0;
    five << 0,0,1,0,1,0,1,0,0;
    six << 0,0,1,1,0,0,0,1,0;
    //cout << one << endl << two << endl << three << endl << four << endl << five << endl << six << endl;
    rot.push_back(one);rot.push_back(two);rot.push_back(three);rot.push_back(four);
    rot.push_back(five);rot.push_back(six);
    vector<Perm> perms;
    for(Perm A: rot){
        for(beacon B: flips){

            Perm res = B.asDiagonal() * A;
            perms.push_back(res);
        }
    }
    int n = input.size();
    vector<beacon> pos(n);
    vector<int> rotation(n, -1);
    vector<bool> vis(n, false);
    pos[0] = {0,0,0};
    rotation[0] = 0;
    vis[0] = true;
    for(int scanner1 = 0; scanner1 < n; scanner1++){
        if(!vis[scanner1])
            continue;
        for(int scanner2 = scanner1+1; scanner2 < n; scanner2++){
            bool foundWinnter = false;
                for(int rot2 = 0; rot2 < 48; rot2++){
                    map<triplet, int> diffCounter;
                    for(int beacon1 = 0; beacon1 < input[scanner1].size(); beacon1++){
                        for(int beacon2 = 0; beacon2 < input[scanner2].size(); beacon2++){
                            beacon b1 = input[scanner1][beacon1];
                            beacon b2 = perms[rot2] * input[scanner2][beacon2];
                            beacon diff = (b1 - b2);
                            triplet diffForMap = convBeacon(diff);
                            diffCounter[diffForMap]++;
                            if(diffCounter[diffForMap] >= 12){
                                foundWinnter = true;
                                beacon relDist = pos[scanner1] - diff;
                                beacon relDist2 = pos[scanner1] + diff;
                                beacon relDist3 = diff - pos[scanner1] ;
                                cout << " found match: " << scanner1 << " " << scanner2 << endl;
                                cout << pos[scanner1].transpose() << " pos Scanner 1 "<< endl;
                                cout << diff.transpose() << " diff" << endl;
                                cout << (relDist).transpose() << " relDist"  << endl;
                                cout << (relDist2).transpose() << " relDist2"  << endl;
                                cout << (relDist3).transpose() << " relDist3"  << endl;
                                cout << endl;
                                //cout << perms[rot2]<< endl;
                                rotation[scanner2] = rot2;
                                pos[scanner2] = diff;

                            }
                        }
                        if(foundWinnter)
                            break;
                    }
                    if(foundWinnter)
                    break;
                }
            if(!foundWinnter)
                continue;
            vector<beacon> curr = input[scanner2];
            int rot = rotation[scanner2];
            beacon posMe = pos[scanner2];
            for(int i = 0; i < curr.size(); i++){
                curr[i] = perms[rot] * curr[i];
                curr[i] += posMe;
                cout << " " << curr[i].transpose() << endl;
            }
            vis[scanner2] = true;
        }
    }

}

int main(){
    vector<vector<beacon>> input;
    read_input(input);
    solve(input);

}


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

#include "Feature.h"

#include <Eigen/Dense>
#include <Core/Utility/Console.h>
#include <Core/Geometry/PointCloud.h>
#include <Core/Geometry/KDTreeFlann.h>

namespace three {

namespace {

Eigen::Vector4d ComputePairFeatures(const Eigen::Vector3d &p1,
		const Eigen::Vector3d &n1, const Eigen::Vector3d &p2,
		const Eigen::Vector3d &n2)
{
	Eigen::Vector4d result;
	Eigen::Vector3d dp2p1 = p2 - p1;
	result(3) = dp2p1.norm();
	if (result(3) == 0.0) {
		return Eigen::Vector4d::Zero();
	}
	auto n1_copy = n1;
	auto n2_copy = n2;
	double angle1 = n1_copy.dot(dp2p1) / result(3);
	double angle2 = n2_copy.dot(dp2p1) / result(3);
	if (acos(fabs(angle1)) > acos(fabs(angle2))) {
		n1_copy = n2;
		n2_copy = n1;
		dp2p1 *= -1.0;
		result(2) = -angle2;
	} else {
		result(2) = angle1;
	}
	auto v = dp2p1.cross(n1_copy);
	double v_norm = v.norm();
	if (v_norm == 0.0) {
		return Eigen::Vector4d::Zero();
	}
	v /= v_norm;
	auto w = n1_copy.cross(v);
	result(1) = v.dot(n2_copy);
	result(0) = atan2(w.dot(n2_copy), n1_copy.dot(n2_copy));
	return result;
}

std::shared_ptr<Feature> ComputeSPFHFeature(const PointCloud &input,
		const KDTreeFlann &kdtree, const KDTreeSearchParam &search_param)
{
	auto feature = std::make_shared<Feature>();
	feature->Resize(33, (int)input.points_.size());
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)input.points_.size(); i++) {
		const auto &point = input.points_[i];
		const auto &normal = input.normals_[i];
		std::vector<int> indices;
		std::vector<double> distance2;
		if (kdtree.Search(point, search_param, indices, distance2) > 1) {
			// only compute SPFH feature when a point has neighbors
			double hist_incr = 100.0 / (double)(indices.size() - 1);
			for (size_t k = 1; k < indices.size(); k++) {
				// skip the point itself, compute histogram
				auto pf = ComputePairFeatures(point, normal,
						input.points_[indices[k]], input.normals_[indices[k]]);
				int h_index = (int)(floor(11 * (pf(0) + M_PI) / (2.0 * M_PI)));
				if (h_index < 0) h_index = 0;
				if (h_index >= 11) h_index = 10;
				feature->data_(h_index, i) += hist_incr;
				h_index = (int)(floor(11 * (pf(1) + 1.0) * 0.5));
				if (h_index < 0) h_index = 0;
				if (h_index >= 11) h_index = 10;
				feature->data_(h_index + 11, i) += hist_incr;
				h_index = (int)(floor(11 * (pf(2) + 1.0) * 0.5));
				if (h_index < 0) h_index = 0;
				if (h_index >= 11) h_index = 10;
				feature->data_(h_index + 22, i) += hist_incr;
			}
		}
	}
	return feature;
}

class ProjectedPoints
{
public:
	int id;		// global point ID
	double u;	// u coordinate on the projected patch
	double v;	// v coordinate on the projected patch
	double depth;
};

std::vector<ProjectedPoints> PlanarParameterizationForOnePoint(
		size_t point_id, const PointCloud &cloud, const KDTreeFlann &kdtree,
		const KDTreeSearchParamHybrid &search_param,
		const PlanarParameterizationOption &option)
{
	const int patch_half_size = option.half_patch_size_;
	const int patch_size = patch_half_size * 2 + 1;
	const double patch_half_diag_length = sqrt((2.0 * pow(patch_half_size, 2.0)));

	std::vector<int> indices;
	std::vector<double> distance2;
	Eigen::Matrix3d tangential_axis;
	Eigen::Vector3d vt, vt_n, vt_t1, vt_t2, vt_adj, vt_proj;
	Eigen::Vector2d vt_val = Eigen::Vector2d::Zero();
	Eigen::Vector2i vt_val_descrete = Eigen::Vector2i::Zero();
	Eigen::MatrixXd patch = Eigen::MatrixXd::Zero(patch_size, patch_size);
	double depth = -1.0;
	int adj_id = -1;
	vt = cloud.points_[point_id];

	// giving enough search radius to give enough space in the boundry pixels
	KDTreeSearchParamHybrid search_param_buffer = search_param;
	search_param_buffer.radius_ = search_param.radius_ * 2.0;
	int number_of_searched_neighbors =
			kdtree.Search(vt, search_param_buffer, indices, distance2);
	std::vector<ProjectedPoints> output;
	output.resize(number_of_searched_neighbors);

	if (number_of_searched_neighbors >= 1) {
		tangential_axis = ComputeTangentialAxis(cloud, indices);
		vt_n = tangential_axis.col(0);
		vt_t1 = tangential_axis.col(1);
		vt_t2 = tangential_axis.col(2);
		// project points onto tangential PlanarParameterization
		for (int j = 0; j < number_of_searched_neighbors; j++){
			adj_id = indices[j];
			vt_adj = cloud.points_[adj_id];
			depth = (vt_adj - vt).dot(vt_n);
			vt_proj = vt_adj - depth * vt_n;
			vt_val(0) = (vt_proj - vt).dot(vt_t1);
			vt_val(1) = (vt_proj - vt).dot(vt_t2);
			vt_val = vt_val / search_param.radius_ * patch_half_diag_length;
			output[j].u = vt_val(0);
			output[j].v = vt_val(1);
			output[j].depth = depth;
			output[j].id = adj_id;
		}
	}
	return std::move(output);
}

class ProcessedPatch
{
public:
	// kernel x kernel size, local depth map
	Eigen::MatrixXd depth_map;
	// kernel x kernel size, point cloud index
	std::vector<Eigen::MatrixXi> index_map;
	// kernel x kernel size, point cloud weight to adjacent index
	std::vector<Eigen::MatrixXd> weight_map;
};

std::shared_ptr<ProcessedPatch> PlanarParameterizationForOnePatch(
		const std::vector<ProjectedPoints> &projected_points,
		const PlanarParameterizationOption &option)
{
	const int number_of_neighbors = option.number_of_neighbors_;
	const int patch_half_size = option.half_patch_size_;
	const int patch_size = patch_half_size * 2 + 1;
	const double sigma_pow2 = option.sigma_ * option.sigma_;

	auto output = std::make_shared<ProcessedPatch>();
	output->index_map.resize(number_of_neighbors);
	output->weight_map.resize(number_of_neighbors);
	for (int i = 0; i < number_of_neighbors; i++){
		output->index_map[i].resize(patch_size, patch_size);
		output->weight_map[i].resize(patch_size, patch_size);
		output->index_map[i].setConstant(-1);
		output->weight_map[i].setZero();
	}
	output->depth_map.resize(patch_size, patch_size);

	const int number_of_points = int(projected_points.size());
	Eigen::MatrixXd data = Eigen::MatrixXd::Zero(2, number_of_points);
	int cnt = 0;
	for (int i = 0; i < (int)projected_points.size(); i++){
		data(0,i) = projected_points[i].u;
		data(1,i) = projected_points[i].v;
	}
	KDTreeFlann kdtree;
	kdtree.SetMatrixData(data);
	std::vector<int> indices;
	std::vector<double> distance2;
	for (int u = 0; u < (int)patch_size; u++){
		for (int v = 0; v < (int)patch_size; v++){
			Eigen::Vector2d query;
			query(0) = u - patch_half_size;
			query(1) = v - patch_half_size;
			// todo: should we use SearchKNN here? maybe fill in blank
			int number_of_searched_neighbors = kdtree.SearchKNN(
					query, number_of_neighbors, indices, distance2);
			double sum_weight = 0.0, sum_weighted_val = 0.0;
			for (int i = 0; i < number_of_searched_neighbors; i++) {
				int i_adj = indices[i];
				double dist_adj = distance2[i];
				double weight_i_th_adj = exp(-dist_adj/sigma_pow2);
				double depth_adj = projected_points[i_adj].depth;
				sum_weight += weight_i_th_adj;
				sum_weighted_val += weight_i_th_adj * depth_adj;
				output->index_map[i](v, u) = projected_points[i_adj].id;
				output->weight_map[i](v, u) = weight_i_th_adj;
			}
			// normalize weight_map
			if (sum_weight != 0.0){
				for (int i = 0; i < number_of_searched_neighbors; i++) {
					output->weight_map[i](v, u) /= sum_weight;
				}
			}
			if (number_of_searched_neighbors >= 1){
				if (option.depth_densify_method_ ==
					depth_densify_gaussian_kernel && sum_weight != 0.0)
					output->depth_map(v, u) = sum_weighted_val / sum_weight;
				else if (option.depth_densify_method_ ==
						depth_densify_nearest_neighbor)
					output->depth_map(v, u) =
					projected_points[indices[0]].depth;
			}
		}
	}
	return output;
}

}	// unnamed namespace

std::shared_ptr<Feature> ComputeFPFHFeature(const PointCloud &input,
		const KDTreeSearchParam &search_param/* = KDTreeSearchParamKNN()*/)
{
	auto feature = std::make_shared<Feature>();
	feature->Resize(33, (int)input.points_.size());
	if (input.HasNormals() == false) {
		PrintDebug("[ComputeFPFHFeature] Failed because input point cloud has no normal.\n");
		return feature;
	}
	KDTreeFlann kdtree(input);
	auto spfh = ComputeSPFHFeature(input, kdtree, search_param);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)input.points_.size(); i++) {
		const auto &point = input.points_[i];
		std::vector<int> indices;
		std::vector<double> distance2;
		if (kdtree.Search(point, search_param, indices, distance2) > 1) {
			double sum[3] = {0.0, 0.0, 0.0};
			for (size_t k = 1; k < indices.size(); k++) {
				// skip the point itself
				double dist = distance2[k];
				if (dist == 0.0)
					continue;
				for (int j = 0; j < 33; j++) {
					double val = spfh->data_(j, indices[k]) / dist;
					sum[j / 11] += val;
					feature->data_(j, i) += val;
				}
			}
			for (int j = 0; j < 3; j++)
				if (sum[j] != 0.0) sum[j] = 100.0 / sum[j];
			for (int j = 0; j < 33; j++) {
				feature->data_(j, i) *= sum[j / 11];
				// The commented line is the fpfh function in the paper.
				// But according to PCL implementation, it is skipped.
				// Our initial test shows that the full fpfh function in the
				// paper seems to be better than PCL implementation. Further
				// test required.
				feature->data_(j, i) += spfh->data_(j, i);
			}
		}
	}
	return feature;
}

std::shared_ptr<PlanarParameterizationOutput> PlanarParameterization(
		const PointCloud &cloud,
		const KDTreeSearchParamHybrid &search_param,
		const PlanarParameterizationOption &option)
{
	// parameters
	const int patch_size = option.half_patch_size_ * 2 + 1;
	const int patch_size_pow2 = patch_size * patch_size;
	const int number_of_points = (int)cloud.points_.size();

	auto output = std::make_shared<PlanarParameterizationOutput>();
	output->depth_.Resize(patch_size_pow2, number_of_points);
	for (int i = 0; i < option.number_of_neighbors_; i++){
		Feature data_weight;
		data_weight.Resize(patch_size_pow2, number_of_points);
		output->weight_.push_back(data_weight);
		Eigen::MatrixXi data_index;
		data_index.resize(patch_size_pow2, number_of_points);
		output->index_.push_back(data_index);
	}
	KDTreeFlann kdtree;
	kdtree.SetGeometry(cloud);

#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < number_of_points; i++) {
		auto projected_points = PlanarParameterizationForOnePoint(
				i, cloud, kdtree, search_param, option);
		auto information_patch = PlanarParameterizationForOnePatch(
				projected_points, option);
		for (int j = 0; j < option.number_of_neighbors_; j++){
			for (int k = 0; k < patch_size_pow2; k++){
				output->weight_[j].data_(k, i) =
						information_patch->weight_map[j](k);
				output->index_[j](k, i) =
						information_patch->index_map[j](k);
			}
		}
		for (size_t k = 0; k < patch_size_pow2; k++){
			output->depth_.data_(k, i) = information_patch->depth_map(k);
		}
	}
	return output;
}

}	// namespace three


#ifndef RICPAD_DIFFERENTIATE
#define RICPAD_DIFFERENTIATE

#include <Eigen/Dense>

namespace solver {

template <typename T, int N>
T differentiate(
        // A function which takes an Eigen Matrix
        const std::function<T(Eigen::Matrix<T,N,1>&)>& f,
        // The variables contained in an Eigen matrix
        const Eigen::Matrix<T, N, 1>& x,
        // With respect to which variable we differentiate
        const int k,
        // Step size
        const T& h
      )
{
    Eigen::Matrix<T, N, 1> xp(x), xm(x);

    xp(k) += h;
    xm(k) -= h;

    T ans = f(xp) - f(xm);
    ans /= (h*T(2));

    return ans;
};

}; // namespace differentiate
#endif


#include <mex.h>
#include <Eigen/Core>
#include "splineGeneration.h"
#include "drakeUtil.h"
#include <iostream>

using namespace std;
using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
  string usage = "[coefs, objval] = twoWaypointCubicSplinemex(ts, xs, xd0, xdf)";
  if (nrhs != 4)
    mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplinemex:WrongNumberOfInputs", usage.c_str());
  if (nlhs > 2)
    mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplinemex:WrongNumberOfOutputs", usage.c_str());

  const std::vector<double> segment_times = matlabToStdVector(prhs[0]);
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[1]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[2]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[3]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = 3;
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
  plhs[0] = mxCreateNumericArray(num_segments, dims, mxDOUBLE_CLASS, mxREAL);
  double objective_value = 0.0;
  for (mwSize dof = 0; dof < ndof; dof++) {
    PiecewisePolynomial<double> spline = twoWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, 3), xdf[dof], xs(dof, 1), xs(dof, 2));

    PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
    acceleration_squared *= acceleration_squared;
    PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
    objective_value += acceleration_squared_integral.value(spline.getEndTime()) - acceleration_squared_integral.value(spline.getStartTime());

    for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
      for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
        mwSize sub[] = {dof, segment_index, num_coeffs_per_segment - coefficient_index - 1}; // Matlab's reverse coefficient indexing...
        *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(segment_index).getCoefficients()[coefficient_index];
      }
    }
  }

  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleScalar(objective_value);
  }
}


#include "mex.h"
#include <Eigen/Dense>
#include "../radialpose.h"

void print_usage() {
	mexPrintf("[R,t,f,params] = radialpose_mex(x,X,solver,[upgrade_only]);\n");
	mexPrintf(" Solvers:\n");
	mexPrintf("  1 - D(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  2 - D(2,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  3 - D(3,0) - 5p -- Larsson et al.  ICCV 2019  (Minimal)\n");
	mexPrintf("  4 - D(3,3) - 8p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  5 - U(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  6 - U(0,1) - 4p -- Larsson et al.  ICCV 2017  (Minimal, Non-planar)\n");
	mexPrintf("  7 - U(0,1) - 4p -- Bujnak et al.   ACCV 2010  (Minimal, Non-planar)\n");
	mexPrintf("  8 - U(0,1) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf("  9 - U(0,2) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf(" 10 - U(0,3) - 5p -- Kukelova et al. ICCV 2013  (Minimal)\n");
	mexPrintf(" 11 - U(0,1) - 4p -- Oskarsson       arxiv 2018 (Minimal, Planar)\n");
	mexPrintf(" 12 - N/A    - 5p -- Kukelova et al. ICCV 2013  (Minimal, 1D Radial)\n\n");
	mexPrintf("If upgrade_only=true we don't solve for the 1D radial camera and only try to upgrade.\n This is only for the two-step solvers.\n");
}

void save_poses(int nlhs, mxArray *plhs[], std::vector<radialpose::Camera> &poses) {
	int n_sols = poses.size();
	int n_params = 0;
	if (n_sols > 0)
		n_params = poses[0].dist_params.size();

	if (nlhs >= 1) {
		plhs[0] = mxCreateDoubleMatrix(9, n_sols, mxREAL);
		double *p = mxGetPr(plhs[0]);
		for (int i = 0; i < n_sols; ++i)
			for (int j = 0; j < 9; ++j)
				p[9 * i + j] = poses[i].R(j);
	}
	if (nlhs >= 2) {
		plhs[1] = mxCreateDoubleMatrix(3, n_sols, mxREAL);
		double *p = mxGetPr(plhs[1]);
		for (int i = 0; i < n_sols; ++i)
			for (int j = 0; j < 3; ++j)
				p[3 * i + j] = poses[i].t(j);
	}
	if (nlhs >= 3) {
		plhs[2] = mxCreateDoubleMatrix(1, n_sols, mxREAL);
		double *p = mxGetPr(plhs[2]);
		for (int i = 0; i < n_sols; ++i)
			p[i] = poses[i].focal;

	}
	if (nlhs >= 4) {
		plhs[3] = mxCreateDoubleMatrix(n_params, n_sols, mxREAL);
		double *p = mxGetPr(plhs[3]);
		for (int i = 0; i < n_sols; ++i)
			for (int j = 0; j < n_params; ++j)
				p[n_params * i + j] = poses[i].dist_params[j];
	}
}

void mexFunction(int nlhs,mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs < 3 || nrhs > 5) {
		print_usage();
		mexErrMsgTxt("Incorrect number of input arguments.");
	}
	if (nlhs > 4) {
		print_usage();
		mexErrMsgTxt("Wrong number of output arguments.");
	}

	if (mxGetM(prhs[0]) != 2) {
		print_usage();
		mexErrMsgTxt("First input must be 2 x N matrix.");
	}
	if (mxGetM(prhs[1]) != 3) {
		print_usage();
		mexErrMsgTxt("Second input must be 3 x N matrix.");
	}
	if (mxGetN(prhs[0]) != mxGetN(prhs[1])) {
		print_usage();
		mexErrMsgTxt("Not the same number of 2D points and 3D points.");
	}

	bool use_radial_solver = true;
	if (nrhs >= 4 && mxGetScalar(prhs[3]) != 0.0) {
		use_radial_solver = false;
	}

	double damp_factor = 0.0;
	if (nrhs >= 5) {
		damp_factor = mxGetScalar(prhs[4]);
	}

	// If we don't call the radial solver we don't normalize world coord system
	bool center_world_coordinates = use_radial_solver;

	Eigen::Matrix<double, 2, Eigen::Dynamic> x = Eigen::Map<Eigen::Matrix<double, 2, Eigen::Dynamic>>(mxGetPr(prhs[0]), 2, mxGetN(prhs[0]));
	Eigen::Matrix<double, 3, Eigen::Dynamic> X = Eigen::Map<Eigen::Matrix<double, 3, Eigen::Dynamic>>(mxGetPr(prhs[1]), 3, mxGetN(prhs[1]));

	if(x.cols() > 8) {
		// This is to avoid crash when solver is called with more than 8 points
		x = x.block(0,0,2,8).eval();
		X = X.block(0,0,3,8).eval();
	}

	int solver_idx = static_cast<int>(mxGetScalar(prhs[2]));

	std::vector<radialpose::Camera> poses;

	if (solver_idx == 1) { // D(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 2) { // D(2,0)
		radialpose::larsson_iccv19::Solver<2, 0, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 3) { // D(3,0)
		radialpose::larsson_iccv19::Solver<3, 0, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 4) { // D(3,3)
		radialpose::larsson_iccv19::Solver<3, 3, true> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 5) { // U(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, false> estimator;
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.damp_factor = damp_factor;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 6) { // U(0,1)
		radialpose::larsson_iccv17::NonPlanarSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 7) {  // U(0,1)
		radialpose::bujnak_accv10::NonPlanarSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 8) {  // U(0,1)
		radialpose::kukelova_iccv13::Solver estimator(1);
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 9) {  // U(0,2)
		radialpose::kukelova_iccv13::Solver estimator(2);
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 10) { // U(0,3)
		radialpose::kukelova_iccv13::Solver estimator(3);
		estimator.use_radial_solver = use_radial_solver;
		estimator.center_world_coord = center_world_coordinates;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 11) {  // U(0,1)
		radialpose::oskarsson_arxiv18::PlanarSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else if (solver_idx == 12) {  // N/A
		radialpose::kukelova_iccv13::Radial1DSolver estimator;
		estimator.estimate(x, X, &poses);
		save_poses(nlhs, plhs, poses);

	} else {
		print_usage();
		mexErrMsgTxt("Solver NYI.\n");
	}
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "polar_dec.h"
#include "polar_svd.h"
#ifdef _WIN32
#else
#  include <fenv.h>
#endif
#include <cmath>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <cfenv>

// From Olga's CGAL mentee's ARAP code
template <
  typename DerivedA,
  typename DerivedR,
  typename DerivedT,
  typename DerivedU,
  typename DerivedS,
  typename DerivedV>
IGL_INLINE void igl::polar_dec(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T,
  Eigen::PlainObjectBase<DerivedU> & U,
  Eigen::PlainObjectBase<DerivedS> & S,
  Eigen::PlainObjectBase<DerivedV> & V)
{
  using namespace std;
  using namespace Eigen;
  typedef typename DerivedA::Scalar Scalar;

  const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());

  Eigen::SelfAdjointEigenSolver<DerivedA> eig;
  feclearexcept(FE_UNDERFLOW);
  eig.computeDirect(A.transpose()*A);
  if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
  {
    cout<<"resorting to svd 1..."<<endl;
    return polar_svd(A,R,T,U,S,V);
  }

  S = eig.eigenvalues().cwiseSqrt();

  V = eig.eigenvectors();
  U = A * V;
  R = U * S.asDiagonal().inverse() * V.transpose();
  T = V * S.asDiagonal() * V.transpose();

  S = S.reverse().eval();
  V = V.rowwise().reverse().eval();
  U = U.rowwise().reverse().eval() * S.asDiagonal().inverse();

  if(R.determinant() < 0)
  {
    // Annoyingly the .eval() is necessary
    auto W = V.eval();
    const auto & SVT = S.asDiagonal() * V.adjoint();
    W.col(V.cols()-1) *= -1.;
    R = U*W.transpose();
    T = W*SVT;
  }

  if(std::fabs(R.squaredNorm()-3.) > th)
  {
    cout<<"resorting to svd 2..."<<endl;
    return polar_svd(A,R,T,U,S,V);
  }
}

template <
  typename DerivedA,
  typename DerivedR,
  typename DerivedT>
IGL_INLINE void igl::polar_dec(
  const Eigen::PlainObjectBase<DerivedA> & A,
  Eigen::PlainObjectBase<DerivedR> & R,
  Eigen::PlainObjectBase<DerivedT> & T)
{
  DerivedA U;
  DerivedA V;
  Eigen::Matrix<typename DerivedA::Scalar,DerivedA::RowsAtCompileTime,1> S;
  return igl::polar_dec(A,R,T,U,S,V);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template  void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::polar_dec<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 2, 2, 0, 2, 2>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


// Copyright (c)  Mingcheng Zuo, Dietmar Wolz.
//
// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory.

// Eigen based implementation of differential evolution (GCL-DE) derived from
// "A case learning-based differential evolution algorithm for global optimization of interplanetary trajectory design,
//  Mingcheng Zuo, Guangming Dai, Lei Peng, Maocai Wang, Zhengquan Liu", https://doi.org/10.1016/j.asoc.2020.106451

#include <Eigen/Core>
#include <iostream>
#include <float.h>
#include <ctime>
#include <random>
#include "pcg_random.hpp"

using namespace std;

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vec;
typedef Eigen::Matrix<int, Eigen::Dynamic, 1> ivec;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> mat;

typedef void (*callback_parallel)(int, int, double[], double[]);

namespace gcl_differential_evolution {

static uniform_real_distribution<> distr_01 = std::uniform_real_distribution<>(
        0, 1);

static normal_distribution<> gauss_01 = std::normal_distribution<>(0, 1);

static double normreal(pcg64 *rs, double mu, double sdev) {
    return gauss_01(*rs) * sdev + mu;
}

static vec zeros(int n) {
    return Eigen::MatrixXd::Zero(n, 1);
}

static vec constant(int n, double val) {
    return Eigen::MatrixXd::Constant(n, 1, val);
}

static Eigen::MatrixXd uniformVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return distr_01(rs);
    });
}

static Eigen::MatrixXd normalVec(int dim, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dim, 1, [&]() {
        return gauss_01(rs);
    });
}

static Eigen::MatrixXd uniform(int dx, int dy, pcg64 &rs) {
    return Eigen::MatrixXd::NullaryExpr(dx, dy, [&]() {
        return distr_01(rs);
    });
}

struct IndexVal {
    int index;
    double val;
};

static bool compareIndexVal(IndexVal i1, IndexVal i2) {
    return (i1.val < i2.val);
}

static ivec sort_index(const vec &x) {
    int size = x.size();
    IndexVal ivals[size];
    for (int i = 0; i < size; i++) {
        ivals[i].index = i;
        ivals[i].val = x[i];
    }
    std::sort(ivals, ivals + size, compareIndexVal);
    return Eigen::MatrixXi::NullaryExpr(size, 1, [&ivals](int i) {
        return ivals[i].index;
    });
}

// wrapper around the fitness function, scales according to boundaries

class Fitness {

public:

    Fitness(callback_parallel func_par_, int dim_, const vec &lower_limit,
            const vec &upper_limit) {
        func_par = func_par_;
        dim = dim_;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) // bounds defined
            scale = (upper - lower);
    }

    vec getClosestFeasible(const vec &X) const {
        if (lower.size() > 0) {
            return X.cwiseMin(upper).cwiseMax(lower);
        }
        return X;
    }

    void values(const mat &popX, vec &ys) {
        int popsize = popX.cols();
        int n = popX.rows();
        double pargs[popsize * n];
        double res[popsize];
        for (int p = 0; p < popX.cols(); p++) {
            for (int i = 0; i < n; i++)
                pargs[p * n + i] = popX(i, p);
        }
        func_par(popsize, n, pargs, res);
        for (int p = 0; p < popX.cols(); p++)
            ys[p] = res[p];
        evaluationCounter += popsize;
    }

    bool feasible(int i, double x) {
        return lower.size() == 0 || (x >= lower[i] && x <= upper[i]);
    }

    vec sample(pcg64 &rs) {
        if (lower.size() > 0) {
            vec rv = uniformVec(dim, rs);
            return (rv.array() * scale.array()).matrix() + lower;
        } else
            return normalVec(dim, rs);
    }

    double sample_i(int i, pcg64 &rs) {
        if (lower.size() > 0)
            return lower[i] + scale[i] * distr_01(rs);
        else
            return gauss_01(rs);
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
    callback_parallel func_par;
    int dim;
    vec lower;
    vec upper;
    long evaluationCounter;
    vec scale;
};

class GclDeOptimizer {

public:

    GclDeOptimizer(long runid_, Fitness *fitfun_, int dim_, int seed_,
            int popsize_, int maxEvaluations_, double pbest_,
            double stopfitness_, double F0_, double CR0_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : int(dim * 8.5 + 150);
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
        // use low value 0 < pbest <= 1 to narrow search.
        pbest = pbest_;
        // Limit for fitness value.
        stopfitness = stopfitness_;
        F0 = F0_;
        CR0 = CR0_;
        // stop criteria
        stop = 0;
        rs = new pcg64(seed_);
        init();
    }

    ~GclDeOptimizer() {
        delete rs;
    }

    double rnd01() {
        return distr_01(*rs);
    }

    int rndInt(int max) {
        return (int) (max * distr_01(*rs));
    }

    void doOptimize() {
        int gen_stuck = 0;
        vector<vec> sp;

        int maxIter = maxEvaluations / popsize + 1;
        double previous_best = DBL_MAX;
        double CR, F;

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1;; iterations++) {
            // sort population
            ivec sindex = sort_index(nextY);
            popY = nextY(sindex, Eigen::all);
            popX = nextX(Eigen::all, sindex);

            bestX = popX.col(0);
            bestY = popY[0];

            if (isfinite(stopfitness) && bestY < stopfitness) {
                stop = 1;
                return;
            }
            if (bestY == previous_best)
                gen_stuck++;
            else
                gen_stuck = 0;
            previous_best = bestY;

            if (fitfun->getEvaluations() >= maxEvaluations)
                return;
            for (int p = 0; p < popsize; p++) {
                int r1, r2, r3;
                do {
                    r1 = rndInt(popsize);
                } while (r1 == p);
                do {
                    r2 = rndInt(int(popsize * pbest));
                } while (r2 == p || r2 == r1);
                do {
                    r3 = rndInt(popsize + sp.size());
                } while (r3 == p || r3 == r2 || r3 == r1);
                int jr = rndInt(dim);
                //Produce the CR and F
                double mu = 1
                        - sqrt(float(iterations / maxIter))
                                * exp(float(-gen_stuck / iterations));
                if (iterations % 2 == 1) {
                    CR = normreal(rs, 0.95, 0.01);
                    F = normreal(rs, mu, 1);
                    if (F < 0 || F > 1)
                        F = rnd01();
                } else {
                    CR = abs(normreal(rs, CR0, 0.01));
                    F = F0;
                }
                vec ui = popX.col(p);
                for (int j = 0; j < dim; j++) {
                    if (j == jr || rnd01() < CR) {
                        if (r3 < popsize)
                            ui[j] = popX(j, r1)
                                    + F * (popX(j, r2) - popX(j, r3));
                        else
                            ui[j] = popX(j, r1)
                                    + F * ((popX)(j, r2) - sp[r3 - popsize][j]);
                        if (!fitfun->feasible(j, ui[j]))
                            ui[j] = fitfun->sample_i(j, *rs);
                    }
                }
                nextX.col(p) = ui;
            }
            fitfun->values(nextX, nextY);
            for (int p = 0; p < popsize; p++) {
                if (nextY[p] < popY[p]) {
                    if (sp.size() < popsize)
                        sp.push_back(popX.col(p));
                    else
                        sp[rndInt(popsize)] = popX.col(p);
                } else {        // no improvement, copy from parent
                    nextX.col(p) = popX.col(p);
                    nextY[p] = popY[p];
                }
            }
        }
    }

    void init() {
        popCR = zeros(popsize);
        popF = zeros(popsize);
        nextX = mat(dim, popsize);
        for (int p = 0; p < popsize; p++)
            nextX.col(p) = fitfun->sample(*rs);
        nextY = vec(popsize);
        fitfun->values(nextX, nextY);
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
    long runid;
    Fitness *fitfun;
    int popsize; // population size
    int dim;
    int maxEvaluations;
    double pbest;
    double stopfitness;
    int iterations;
    double bestY;
    vec bestX;
    int stop;
    double F0;
    double CR0;
    pcg64 *rs;
    mat popX;
    vec popY;
    mat nextX;
    vec nextY;
    vec popCR;
    vec popF;
};
}

using namespace gcl_differential_evolution;

extern "C" {
double* optimizeGCLDE_C(long runid, callback_parallel func_par, int dim,
        int seed, double *lower, double *upper, int maxEvals, double pbest,
        double stopfitness, int popsize, double F0, double CR0) {
    int n = dim;
    double *res = new double[n + 4];
    vec lower_limit(n), upper_limit(n);
    bool useLimit = false;
    for (int i = 0; i < n; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
        useLimit |= (lower[i] != 0);
        useLimit |= (upper[i] != 0);
    }
    if (useLimit == false) {
        lower_limit.resize(0);
        upper_limit.resize(0);
    }
    Fitness fitfun(func_par, n, lower_limit, upper_limit);
    GclDeOptimizer opt(runid, &fitfun, dim, seed, popsize, maxEvals, pbest,
            stopfitness, F0, CR0);
    try {
        opt.doOptimize();
        vec bestX = opt.getBestX();
        double bestY = opt.getBestValue();
        for (int i = 0; i < n; i++)
            res[i] = bestX[i];
        res[n] = bestY;
        res[n + 1] = fitfun.getEvaluations();
        res[n + 2] = opt.getIterations();
        res[n + 3] = opt.getStop();
        return res;
    } catch (std::exception &e) {
        cout << e.what() << endl;
        return res;
    }
}
}


// This file is part of:
// gnss-sim: A GNSS Signal Simulator
//
// Copyright (c) 2020 Damien Dusha
// SPDX-License-Identifier: MIT
//
// Derived from https://github.com/osqzss/gps-sdr-sim (MIT Licence):
// Copyright (c) 2015-2020 Takuji Ebinuma

#include "gpssim.h"
#include "gps_channel.h"
#include "gps_time.h"
#include "gps_ephem.h"
#include "gps_math.h"
#include "gps_subframe.h"
#include "geodesy.h"
#include "ionosphere.h"
#include "noise_generator.h"
#include "rinex2_reader.h"
#include "sin_table.h"
#include "sample_writer.h"
#include "satellite_gain.h"

#include <Eigen/Core>

#include <unistd.h>
#include <iostream>

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <ctime>

namespace {

static constexpr double kWavelengthGpsL1ca_m = 0.190293672798365;

}   // namespace

/*! \brief Compute range between a satellite and the receiver
 *  \param[out] rho The computed range
 *  \param[in] eph Ephemeris data of the satellite
 *  \param[in] g GPS time at time of receiving the signal
 *  \param[in] e_pos_e_a is the position of the receiver.
 */
range_t ComputeRange(const ephem_t &eph, const ionoutc_t &ionoutc,
                  gpstime_t g, const Eigen::Vector3d &e_pos_e_a)
{
	double pos[3],vel[3],clk[2];
	double los[3];
	double xrot,yrot;

	double neu[3];
	double tmat[3][3];

	// SV position at time of the pseudorange observation.
	satpos(eph, g, pos, vel, clk);

	// Receiver to satellite vector and light-time.
	subVect(los, pos, e_pos_e_a);
	const double tau = normVect(los)/SPEED_OF_LIGHT;

	// Extrapolate the satellite position backwards to the transmission time.
	pos[0] -= vel[0]*tau;
	pos[1] -= vel[1]*tau;
	pos[2] -= vel[2]*tau;

	// Earth rotation correction. The change in velocity can be neglected.
	xrot = pos[0] + pos[1]*OMEGA_EARTH*tau;
	yrot = pos[1] - pos[0]*OMEGA_EARTH*tau;
	pos[0] = xrot;
	pos[1] = yrot;

    range_t rho;

	// New observer to satellite vector and satellite range.
	subVect(los, pos, e_pos_e_a);
	const double range = normVect(los);
	rho.d = range;

	// Pseudorange.
	rho.range = range - SPEED_OF_LIGHT*clk[0];

	// Relative velocity of SV and receiver.
	const double rate = dotProd(vel, los)/range;

	// Pseudorange rate.
	rho.rate = rate; // - SPEED_OF_LIGHT*clk[1];

	// Time of application.
	rho.g = g;

	// Azimuth and elevation angles.
	const GeodeticPosition llh = xyz2llh(e_pos_e_a);
	ltcmat(llh, tmat);
	ecef2neu(los, tmat, neu);
	rho.azel = neu2azel(neu);

	// Add ionospheric delay
	rho.iono_delay = ionosphericDelay(ionoutc, g, llh, rho.azel);
	rho.range += rho.iono_delay;

	return rho;
}

AzimuthElevation ComputeSatelliteAzel(const ephem_t &eph, gpstime_t g,
                        const Eigen::Vector3d &e_pos_e_a)
{
	double neu[3];
	double pos[3],vel[3],clk[3],los[3];
	double tmat[3][3];

	const GeodeticPosition llh = xyz2llh(e_pos_e_a);
	ltcmat(llh, tmat);

	satpos(eph, g, pos, vel, clk);
	subVect(los, pos, e_pos_e_a);
	ecef2neu(los, tmat, neu);
	return neu2azel(neu);
}

int allocateChannel(GpsChannel *chan, ephem_t *eph, int* allocatedSat,
                    ionoutc_t ionoutc, gpstime_t current_simulation_time,
                    const Eigen::Vector3d &e_pos_e_a, double elevation_mask_deg)
{
    int num_visible_sats = 0;

	for (int sv = 0; sv < MAX_SAT; sv++)
	{
        AzimuthElevation azel;
        bool is_visible = false;
        if (eph[sv].valid) {
            azel = ComputeSatelliteAzel(eph[sv], current_simulation_time, e_pos_e_a);
            is_visible = azel.elevation_deg() > elevation_mask_deg;
        }

		if (is_visible)
		{
			num_visible_sats++;
			if (allocatedSat[sv] == -1) // Visible but not allocated
			{
				// Allocated new satellite
                int i;
				for (i = 0; i < MAX_CHAN; i++)
				{
					if (!chan[i].IsEnabled())
					{
						// Initialize channel
						chan[i] = GpsChannel(sv+1);
						chan[i].azel = azel;

						// Generate subframe
						chan[i].SetEphemeris(eph[sv], ionoutc);

						// Generate navigation message
						chan[i].GenerateNavMsg(current_simulation_time, 1);

						// Initialize pseudorange
                        range_t rho = ComputeRange(eph[sv], ionoutc,
                                     current_simulation_time, e_pos_e_a);
						chan[i].rho0 = rho;

						// Initialize carrier phase
						const double r_xyz = rho.range;

						rho = ComputeRange(eph[sv], ionoutc,
                                     current_simulation_time,
                                     Eigen::Vector3d::Zero());
						const double r_ref = rho.range;

						const double phase_ini =
                                (2.0*r_ref - r_xyz) * (1.0 / kWavelengthGpsL1ca_m);
						chan[i].carrier_phase_cycles = phase_ini - floor(phase_ini);

						// Done.
						break;
					}
				}

				// Set satellite allocation channel
				if (i < MAX_CHAN)
					allocatedSat[sv] = i;
			}
		}
		else if (allocatedSat[sv] >= 0) // Not visible but allocated
		{
			// Clear channel
			chan[allocatedSat[sv]].prn = 0;

			// Clear satellite allocation flag
			allocatedSat[sv] = -1;
		}
	}

	return num_visible_sats;
}

void usage(void)
{
	fprintf(stderr, "Usage: gps-sdr-sim [options]\n"
		"Options:\n"
		"  -e <gps_nav>     RINEX navigation file for GPS ephemerides (required)\n"
		"  -c <location>    ECEF X,Y,Z in meters (static mode) e.g. 3967283.154,1022538.181,4872414.484\n"
		"  -l <location>    Lat,Lon,Hgt (static mode) e.g. 35.681298,139.766247,10.0\n"
		"  -t <date,time>   Scenario start time YYYY/MM/DD,hh:mm:ss\n"
		"  -d <duration>    Duration [sec] (static mode max: %d)\n"
		"  -o <output>      I/Q sampling data file (default: gpssim.bin)\n"
		"  -s <frequency>   Sampling frequency [Hz] (default: 2600000)\n"
		"  -i               Disable ionospheric delay for spacecraft scenario\n"
		"  -v               Show details about simulated channels\n",
		STATIC_MAX_DURATION);

	return;
}

/// \brief Checks whether the sample frequency has an integer number of ns ticks.
bool SampleFrequencyIsValid(int sample_frequency) {
    constexpr int ticks_per_ns = 1000000000;
    int period_ns = ticks_per_ns / sample_frequency;
    return period_ns * sample_frequency == ticks_per_ns;
}

int main(int argc, char *argv[])
{
	clock_t tstart,tend;

	ephem_t eph[EPHEM_ARRAY_SIZE][MAX_SAT];

	gpstime_t simulation_start_gps_time;
    gpstime_t current_simulation_time;

    // Default static location; Tokyo
	GeodeticPosition llh = GeodeticPosition::FromDegrees(35.681298, 139.766247, 10.0);

	GpsChannel chan[MAX_CHAN];
	double elevation_mask_deg = 0.0;

    // User position in ECEF coordinates.
    Eigen::Vector3d e_pos_e_a;

    std::string rinex2_nav_file;
    std::string output_sample_filename = {"gpssim.bin"};

	int result;

	datetime_t t0,tmin,tmax;
	gpstime_t gmin,gmax;

	bool verbose = false;

	ionoutc_t ionoutc;

    int allocatedSat[MAX_SAT];

    NoiseGenerator noise_generator(1.0, 0.20, 0.5);

	////////////////////////////////////////////////////////////
	// Read options
	////////////////////////////////////////////////////////////

	// Default options
	double raw_samp_freq = 2.6e6;
	simulation_start_gps_time.week = -1; // Invalid start time
	double duration = 300;     // 5 minutes.
	ionoutc.enable = true;

	if (argc<3)
	{
		usage();
		exit(1);
	}

	while ((result=getopt(argc,argv,"e:u:g:c:l:o:s:t:d:iv"))!=-1)
	{
		switch (result)
		{
		case 'e':
            rinex2_nav_file.assign(optarg);
			break;
		case 'c':
        {
			// Static ECEF coordinates input mode
            double xyz[3];
			sscanf(optarg,"%lf,%lf,%lf", &xyz[0], &xyz[1], &xyz[2]);
            e_pos_e_a = {xyz[0], xyz[1], xyz[2]};
			break;
        }
		case 'l':
        {
			// Static geodetic coordinates input mode.
            double raw_lat, raw_lon, raw_height;
			sscanf(optarg,"%lf,%lf,%lf",&raw_lat, &raw_lon, &raw_height);
            llh = GeodeticPosition::FromDegrees(raw_lat, raw_lon, raw_height);
			e_pos_e_a = llh2xyz(llh);
			break;
        }
		case 'o':
			output_sample_filename.assign(optarg);
			break;
		case 's':
			raw_samp_freq = atof(optarg);
			if (raw_samp_freq < 1.0e6)
			{
				fprintf(stderr, "ERROR: Invalid sampling frequency.\n");
				exit(1);
			}
			break;
		case 't':
			sscanf(optarg, "%d/%d/%d,%d:%d:%lf", &t0.y, &t0.m, &t0.d, &t0.hh, &t0.mm, &t0.sec);
			if (t0.y<=1980 || t0.m<1 || t0.m>12 || t0.d<1 || t0.d>31 ||
				t0.hh<0 || t0.hh>23 || t0.mm<0 || t0.mm>59 || t0.sec<0.0 || t0.sec>=60.0)
			{
				fprintf(stderr, "ERROR: Invalid date and time.\n");
				exit(1);
			}
			t0.sec = floor(t0.sec);
			simulation_start_gps_time = date2gps(t0);
			break;
		case 'd':
			duration = atof(optarg);
			break;
		case 'i':
			ionoutc.enable = false; // Disable ionospheric correction
			break;
		case 'v':
			verbose = true;
			break;
		case ':':
		case '?':
			usage();
			exit(1);
		default:
			break;
		}
	}

	if (rinex2_nav_file.empty())
	{
		fprintf(stderr, "ERROR: GPS ephemeris file is not specified.\n");
		exit(1);
	}

	if (duration < 0.0 || duration > STATIC_MAX_DURATION)
	{
		fprintf(stderr, "ERROR: Invalid duration.\n");
		exit(1);
	}

    SampleWriter sample_writer;
    if (sample_writer.OpenFile(output_sample_filename))
    {
        fprintf(stderr, "ERROR: Failed to open output sample file \"%s\".\n", output_sample_filename.c_str());
    }

    const int sample_freq_hz = std::lround(raw_samp_freq);
    if (!SampleFrequencyIsValid(sample_freq_hz)) {
        fprintf(stderr, "ERROR: Sample frequency %f not an integer divisor of 1GHz\n",
                raw_samp_freq);
        exit(1);
    }

    const double sample_period_s = 1.0 / raw_samp_freq;
    const int sample_period_ns = 1000000000 / sample_freq_hz;

    // Currently, samples are processed in batches of 1ms
    const double batch_period_s = 0.001;
    const int num_sample_batches = std::lround(duration / batch_period_s);
    const int samples_per_batch = std::lround(
            (1e9 * batch_period_s) / sample_period_ns);

	////////////////////////////////////////////////////////////
	// Read ephemeris
	////////////////////////////////////////////////////////////

	const int neph = readRinexNavAll(eph, &ionoutc, rinex2_nav_file);

	if (neph==0)
	{
		fprintf(stderr, "ERROR: No ephemeris available.\n");
		exit(1);
	}

	if (verbose && ionoutc.valid)
	{
		fprintf(stderr, "  %12.3e %12.3e %12.3e %12.3e\n",
			ionoutc.alpha0, ionoutc.alpha1, ionoutc.alpha2, ionoutc.alpha3);
		fprintf(stderr, "  %12.3e %12.3e %12.3e %12.3e\n",
			ionoutc.beta0, ionoutc.beta1, ionoutc.beta2, ionoutc.beta3);
		fprintf(stderr, "   %19.11e %19.11e  %9d %9d\n",
			ionoutc.A0, ionoutc.A1, ionoutc.tot, ionoutc.wnt);
		fprintf(stderr, "%6d\n", ionoutc.dtls);
	}

	for (int sv = 0; sv < MAX_SAT; sv++)
	{
		if (eph[0][sv].valid)
		{
			gmin = eph[0][sv].toc;
			tmin = eph[0][sv].t;
			break;
		}
	}

	gmax.sec = 0;
	gmax.week = 0;
	tmax.sec = 0;
	tmax.mm = 0;
	tmax.hh = 0;
	tmax.d = 0;
	tmax.m = 0;
	tmax.y = 0;
	for (int sv = 0; sv < MAX_SAT; sv++)
	{
		if (eph[neph-1][sv].valid)
		{
			gmax = eph[neph-1][sv].toc;
			tmax = eph[neph-1][sv].t;
			break;
		}
	}

	// Scenario start time has been set.
	if (simulation_start_gps_time.week >= 0)
	{
		if (subGpsTime(simulation_start_gps_time, gmin) < 0.0 ||
            subGpsTime(gmax, simulation_start_gps_time) < 0.0)
		{
			fprintf(stderr, "ERROR: Invalid start time.\n");
			fprintf(stderr, "tmin = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n",
				tmin.y, tmin.m, tmin.d, tmin.hh, tmin.mm, tmin.sec,
				gmin.week, gmin.sec);
			fprintf(stderr, "tmax = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n",
				tmax.y, tmax.m, tmax.d, tmax.hh, tmax.mm, tmax.sec,
				gmax.week, gmax.sec);
			exit(1);
		}
	}
	else
	{
		simulation_start_gps_time = gmin;
		t0 = tmin;
	}

	fprintf(stderr, "Start time = %4d/%02d/%02d,%02d:%02d:%02.0f (%d:%.0f)\n",
		t0.y, t0.m, t0.d, t0.hh, t0.mm, t0.sec, simulation_start_gps_time.week,
         simulation_start_gps_time.sec);
	fprintf(stderr, "Duration = %.1f [sec]\n", duration);

	// Select the current set of ephemerides
	int ieph = -1;

	for (int i = 0; i < neph; i++)
	{
		for (int sv = 0; sv < MAX_SAT; sv++)
		{
			if (eph[i][sv].valid)
			{
				const double dt = subGpsTime(simulation_start_gps_time, eph[i][sv].toc);
				if (dt>=-SECONDS_IN_HOUR && dt<SECONDS_IN_HOUR)
				{
					ieph = i;
					break;
				}
			}
		}

		if (ieph>=0) // ieph has been set
			break;
	}

	if (ieph == -1)
	{
		fprintf(stderr, "ERROR: No current set of ephemerides has been found.\n");
		exit(1);
	}

	////////////////////////////////////////////////////////////
	// Initialize channels
	////////////////////////////////////////////////////////////

	// Clear satellite allocation flag
	for (int sv = 0; sv < MAX_SAT; sv++) {
		allocatedSat[sv] = -1;
    }

	// Initial reception time
	current_simulation_time = incGpsTime(simulation_start_gps_time, 0.0);

	// Allocate visible satellites
	allocateChannel(chan, eph[ieph], allocatedSat, ionoutc, current_simulation_time,
                    e_pos_e_a, elevation_mask_deg);

	for(int i = 0; i < MAX_CHAN; i++)
	{
		if (chan[i].IsEnabled()) {
			fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn,
				chan[i].azel.azimuth_deg(), chan[i].azel.elevation_deg(), chan[i].
				rho0.d, chan[i].rho0.iono_delay);
        }
	}

	////////////////////////////////////////////////////////////
	// Generate baseband signals
	////////////////////////////////////////////////////////////

	tstart = clock();

	// Update receiver time
	current_simulation_time = incGpsTime(current_simulation_time, batch_period_s);

    PeriodicFunctionTable<double, 1024> sin_table(
        [](double t) -> double {return std::sin(t);}, 1.0);
    PeriodicFunctionTable<double, 1024> cos_table(
        [](double t) -> double {return std::cos(t);}, 1.0);

    // Default all channels to a gain of 1.
    ConstellationGain constellation_gain;
    for (int prn = 0; prn <= 32; ++prn) {
        constellation_gain.SetSatelliteToConstantGain(prn, 1.0);
    }

    // Successively decrease the gain as the PRN increases.
    constellation_gain.SetSatelliteToConstantGain( 1, 1.0);
    constellation_gain.SetSatelliteToConstantGain( 2, 0.9);
    constellation_gain.SetSatelliteToConstantGain( 3, 0.7);
    constellation_gain.SetSatelliteToConstantGain( 6, 0.6);
    constellation_gain.SetSatelliteToConstantGain( 9, 0.5);
    constellation_gain.SetSatelliteToConstantGain(10, 0.4);
    constellation_gain.SetSatelliteToConstantGain(11, 0.3);
    constellation_gain.SetSatelliteToConstantGain(12, 0.2);
    constellation_gain.SetSatelliteToConstantGain(17, 0.17);
    constellation_gain.SetSatelliteToConstantGain(20, 0.15);
    constellation_gain.SetSatelliteToConstantGain(23, 0.13);
    constellation_gain.SetSatelliteToConstantGain(28, 0.10);
    constellation_gain.SetSatelliteToConstantGain(32, 0.07);

    std::cout << "Num batches = " << num_sample_batches << std::endl;
    std::cout << "samples_per_batch = " << samples_per_batch << std::endl;

	for (int batch = 1; batch < num_sample_batches; batch++)
	{
        // Per-channel gain, maximum value of 1.
        std::array<double, MAX_CHAN> gain;

		for (int i = 0; i < MAX_CHAN; i++)
		{
			if (chan[i].IsEnabled())
			{
				// Refresh code phase and data bit counters
				const int sv_index = chan[i].prn-1;

				// Current pseudorange
				const range_t rho = ComputeRange(eph[ieph][sv_index], ionoutc,
                             current_simulation_time, e_pos_e_a);

				chan[i].azel = rho.azel;

				// Update code phase and data bit counters
				chan[i].ComputeCodePhase(rho, batch_period_s);

				// Signal gain.
				gain[i] = constellation_gain.ComputeGain(
                    chan[i].prn, rho.d, rho.azel);
			}
		}

		// There is no intrinsic need to loop here, but we do so to avoid computing
		// the pseudorange too frequently.
		for (int sample = 0; sample < samples_per_batch; sample++)
		{
			double i_acc = 0;
			double q_acc = 0;

			for (int i = 0; i < MAX_CHAN; i++)
			{
				if (chan[i].IsEnabled())
				{
                    const double coeff =
                            chan[i].current_code_data_symbol() * gain[i];
                    const double ip = coeff *
                            cos_table.LookupValue(chan[i].carrier_phase_cycles);
                    const double qp = coeff *
                            sin_table.LookupValue(chan[i].carrier_phase_cycles);

					// Accumulate for all visible satellites.
					i_acc += ip;
					q_acc += qp;

					// Update code phase, including the code chip and data bit.
					chan[i].UpdateCodePhase(sample_period_s);

					// Update carrier phase
					chan[i].UpdateCarrierPhase(sample_period_s);
				}
			}

            // At this point, the min and max values for the I- and Q- samples
            // are -16 and +16 based on the number of satellites processed.
            //
            // We scale them to +/- 0.5 for the sample writer.
            constexpr double kSampleScale = 1.0 / (2.0 * 16.0);
            sample_writer.WriteSample(
                    noise_generator.ScaleAndAddNoise(i_acc * kSampleScale),
                    noise_generator.ScaleAndAddNoise(q_acc * kSampleScale));
        }

        //
        // Update navigation message and channel allocation on 30s boundaries.
        //

        if (current_simulation_time.On30sBoundary())
		{
			// Update navigation message
			for (int i = 0; i < MAX_CHAN; i++)
			{
                if (chan[i].prn>0) {
                    chan[i].GenerateNavMsg(current_simulation_time, 0);
                }
			}

			// Refresh ephemeris and subframes
			// Quick and dirty fix. Need more elegant way.
			for (int sv = 0; sv < MAX_SAT; sv++)
			{
				if (eph[ieph+1][sv].valid)
				{
					const double dt = subGpsTime(eph[ieph+1][sv].toc,
                                                 current_simulation_time);
					if (dt < SECONDS_IN_HOUR)
					{
						ieph++;
						for (int i = 0; i < MAX_CHAN; i++)
						{
							// Generate new subframes if allocated
							if (chan[i].IsEnabled()) {
								chan[i].SetEphemeris(eph[ieph][chan[i].prn-1], ionoutc);
                            }
						}
					}

					break;
				}
			}

			// Update channel allocation
			allocateChannel(chan, eph[ieph], allocatedSat, ionoutc,
                            current_simulation_time, e_pos_e_a, elevation_mask_deg);

			// Show details about simulated channels
			if (verbose)
			{
				fprintf(stderr, "\n");
				for (int i = 0; i < MAX_CHAN; i++)
				{
					if (chan[i].prn>0)
						fprintf(stderr, "%02d %6.1f %5.1f %11.1f %5.1f\n", chan[i].prn,
							chan[i].azel.azimuth_deg(), chan[i].azel.elevation_deg(),
                            chan[i].rho0.d, chan[i].rho0.iono_delay);
				}
			}
		}

		// Update receiver time
		current_simulation_time = incGpsTime(current_simulation_time, batch_period_s);

		// Update time counter
		fprintf(stderr, "\rTime into run = %4.2f",
                subGpsTime(current_simulation_time, simulation_start_gps_time));
		fflush(stdout);
	}

	tend = clock();

	fprintf(stderr, "\nDone!\n");

	// Process time
	fprintf(stderr, "Process time = %.1f [sec]\n", (double)(tend-tstart)/CLOCKS_PER_SEC);

	return(0);
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACSCD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse cosecant in degree:
    \f$(180/\pi) \arcsin(1/x)\f$.

    @par Header <boost/simd/function/acscd.hpp>

    @see acsc, accscpi

    @par Example:

      @snippet acscd.cpp acscd

    @par Possible output:

      @snippet acscd.txt acscd

  **/
  IEEEValue acscd(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/acscd.hpp>
#include <boost/simd/function/simd/acscd.hpp>

#endif


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file scattered-interpolation.hpp
 * @author Jakub Cerveny <jakub.cerveny@ext.citationtech.net>
 * @author Matyas Hollmann <matyas.hollmann@melowntech.com>
 *
 * Interpolation on 2D scattered data.
 */

#ifndef imgproc_scattered_interpolation_hpp_included_
#define imgproc_scattered_interpolation_hpp_included_

#include <vector>
#include <Eigen/Sparse>
#include <opencv2/core/core.hpp>

#include "dbglog/dbglog.hpp"
#include "utility/gccversion.hpp"
#include "rastermask.hpp"

namespace imgproc {

#if !defined(IMGPROC_HAS_OPENCV) || !defined(IMGPROC_HAS_EIGEN3)
    UTILITY_FUNCTION_ERROR("Laplace interpolation is available only when "
                           "compiled with both OpenCV and Eigen3 libraries.")
#endif

/** Solves the boundary value problem -\Delta u = 0 on elements in the matrix
 *  'data' that correspond to unset elements in 'mask'. Elements corresponding
 *  to set positions in 'mask' are regarded as given data.
 *
 *  The method is described in section 3.8 "Laplace Interpolation" of
 *  Numerical Recipes in C, Third Edition.
 *
 *  typename T_OPT:  floating-point numeric type in which Eigen solves the problem,
 *                   float(faster), double(more precise)
 *  typename T_DATA: numeric type of (per-channel) elements of 'data' matrix,
 *                   if it's an integral type results are rounded before storing
 *  int nChan:       number of channels of 'data' matrix
 *
 *  Example usage: for 'data' matrix of type CV_32FC2 use <float, 2, T_OPT>
 *                 for 'data' matrix of type  CV_8UC3 use <unsigned char, 3, T_OPT>
 */
template<typename T_DATA, int nChan, typename T_OPT = float>
void laplaceInterpolate(cv::Mat &data, const imgproc::RasterMask &mask, double tol = 1e-12)
{
    static_assert(std::is_floating_point<T_OPT>::value,
                  "Floating-point numeric type expected.");

    assert(sizeof(T_DATA) == data.elemSize1() && data.channels() == nChan);

    // round results if 'data' matrix elements are of integral type
    constexpr bool doRound = std::is_integral<T_DATA>::value;

    using SparseMatrix = Eigen::SparseMatrix<T_OPT>;
    using Triplet = Eigen::Triplet<T_OPT>;
    // used to access individual entries in the 'data' matrix
    using cvVec = cv::Vec<T_DATA, nChan>;
    // used to access individual entris in the 'rhs' vector,
    // should be a vector of floating-point types
    using rhsVecT = cv::Vec<T_OPT, nChan>;

    int w = data.cols, h = data.rows;
    // # of variables
    int n = 0;

    auto linearizeID([w](int x, int y) -> int
    {
        return y * w + x;
    });

    auto isInside([w, h](const cv::Point2i& pt) -> bool
    {
        return std::min(pt.x, pt.y) >= 0 && pt.x < w && pt.y < h;
    });

    // optimize only free points, given points should not be a part of the
    // optimization
    std::vector<int> pixelIDs(w * h, -1);
    for (int y = 0; y < h; ++y)
    for (int x = 0; x < w; ++x)
    {
        if (!mask.get(x, y)) // free point
        {
            // assing ID and increase value of the counter
            pixelIDs[linearizeID(x, y)] = n++;
        }
    }

    if (!n) // nothing to do here
    {
        LOG(debug) << "All points are given, nothing to do.";
        return;
    }

    // assemble the linear system
    LOG(debug) << "Assembling " << n << "x" << n
               << " sparse system. # of channels: " << nChan;

    std::vector<Triplet> coefs;
    // default constructor = initialize with zeros
    std::vector<rhsVecT> rhsVec(n, rhsVecT());
    coefs.reserve(5 * n);

    const static std::array<cv::Point2i, 4> dirs = {{{1, 0}, {-1, 0},
                                                     {0, 1}, {0, -1}}};

    for (int y = 0; y < h; ++y)
    for (int x = 0; x < w; ++x)
    {
        // index of the unknown
        int k = pixelIDs[linearizeID(x, y)];

        // given point
        if (k < 0) { continue; }

        cv::Point2i cur{x, y};
        // # of neighbors
        int nNeighs = 0;
        for (const auto& dir : dirs)
        {
            cv::Point2i tmp = cur + dir;
            if (!isInside(tmp)) { continue; }
            ++nNeighs;

            // index of the neighboring unknown
            int t = pixelIDs[linearizeID(tmp.x, tmp.y)];
            if (t < 0) // neighbor is a given point
            {
                // convert 'data' to rhsVecT
                rhsVec[k] += rhsVecT(data.at<cvVec>(tmp));
            }
            else // neighbor is a free point
            {
                coefs.emplace_back(k, t, -1.0);
            }
        }
        coefs.emplace_back(k, k, nNeighs);
    }

    SparseMatrix mat(n, n);
    mat.setFromTriplets(coefs.begin(), coefs.end());

    LOG(debug) << "Matrix constructed.";

    using Precond = Eigen::DiagonalPreconditioner<T_OPT>;
    using Solver = Eigen::BiCGSTAB<SparseMatrix, Precond>;
    using EigVecX = Eigen::Matrix<T_OPT, Eigen::Dynamic, 1>;

    // solve the system
    Solver solver(mat);
    solver.setTolerance(tol);

    EigVecX rhs(n), sln(n);
    for (int i = 0; i < nChan; ++i)
    {
        for (int j = 0; j < n; ++j) {
            rhs(j) = rhsVec[j](i);
        }

        LOG(debug) << "Solving system with rhs = channel " << (i + 1)
                   << " out of " << nChan;

        sln = solver.solve(rhs);

        LOG(debug) << "#iterations: " << solver.iterations();
        LOG(debug) << "estimated error: " << solver.error();
        LOG(debug) << "min: " << sln.minCoeff() << ", max: " << sln.maxCoeff();

        // for integral types we should round the results
        // TODO: in C++17 use if constexpr
        if (doRound)
        {
            for (int j = 0; j < n; ++j) {
               sln(j) = std::round(sln(j));
            }
        }

        // store solution of the i-th channel
        for (int y = 0; y < h; ++y)
        for (int x = 0; x < w; ++x)
        {
            int id = pixelIDs[linearizeID(x, y)];
            if (id >= 0) // free point
            {
                data.at<cvVec>(y, x)(i) = sln(id);
            }
        }
    }
}

} // imgproc

#endif // imgproc_scattered_interpolation_hpp_included_