#ifndef INTEGRALS_HPP_
#define INTEGRALS_HPP_
/**
 * @file integrals.hpp
 * @author Adam Lamson
 * @brief Probility Density Function integrals for KMC algorithm. Used to create
 * lookup tables
 * @version 0.1
 * @date 2019-04-15
 *
 * @copyright Copyright (c) 2019
 *
 */

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cassert>
#include <cstdio>
#include <iostream>
#include <string>

/*! \brief Integrate exponential factor with the form of
 * e^{-M * (\sqrt{ s^2 + lm^2}-ell0 -1)^2}
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lowerr limit of integral
 * \param sbound Upper limit of integral
 * \param M exponential constant factor. Physically, this is the product of
 (1-load_sensitivity)*spring_const/(k_B * Temperature)
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double integral(double lm, double sbound0, double sbound1, double M,
                       double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        // lambda capture variabls ell0 and M
        const double exponent = sqrt(s * s + lm * lm) - ell0 - 1;
        return exp(-M * exponent * exponent);
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

#endif /* INTEGRALS_HPP_ */


#include <math.h>
#include <cmath>
//#include <RcppEigen.h>
#include <RcppArmadillo.h>
//#include <iostream>
//#include <vector>
//#include <Eigen/Core>

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(RcppEigen)]]

typedef Eigen::Triplet<float> T; //tripleta para rellenar matrices sparse
typedef Eigen::MappedSparseMatrix<float> MSpMat; //alias para matriz sparse
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd; //alias para vector sparse
using namespace Rcpp;
//using namespace RcppArmadillo;

// [[Rcpp::export]]
float KernelGauss (arma::cube x, arma::cube y, int dimension, double sigJ, double sigd, float distancia)
{
  //Calculo individual del kernel entre dos observaciones
  // input: x observaciones
  //        y observaciones
  // dimension: numero de canales
  //sigJ: sd de la imagen
  //sigd: sd de la posicion
  //distancia: distancia L_2 entre los dos pixeles
  float kernel_value = 0;
  float siJ = (float)sigJ;
  float sid = (float)sigd;
  for (int i = 0; i < dimension; i++ )
  {
    kernel_value +=(float)pow( (float)(x[i] - y[i]),2);
  }
  kernel_value = exp(-(kernel_value/(2*pow(siJ,2)) ) - distancia/(2*pow(sid, 2)));
  return( kernel_value );
}

// [[Rcpp::export]]
Eigen::SparseMatrix<float> Kernel_RGB ( arma::cube  M, int h, int w, double distancia, double sigJ, double sigd, int dimension)
{
  // Funcion para calcular  la matriz de similaridades de todos los puntos de una imagen
  //  input: w: altura en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         h: ancho en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         distancia: limite de la vecindad del pixel a considerar
  //         M: imagen de 'dimension' dimensiones (numero de canales)
  //         sigJ: sigma para kernel gaussiano, de la imagen
  //         sigd: sigma para kernell gaussiano, de las distancias
  //         dimension: numero de canales
  //output: W, similaridad calculada
  int i, j, k, l = 0 ;
  float vecindad = distancia;
  float d2 = 0.;
  float calculo = 0.;
  std::vector<T> tripletList; //inicializamos tripleta
  tripletList.reserve((int)floor(h*h*w*w*.1));

  //comienza calculo de matriz de similaridades
  for(i=0; i < h; i++)
  {
    for(j=0; j< w; j++ )
    {
      for(k=0; k<h; k++)
      {
        for(l=0; l<w; l++)
        {
          d2 = (float)(pow(i-k,2)+pow(j-l,2));
          if(d2 <= vecindad)
          {
            calculo = KernelGauss( M.tube(j,i), M.tube(l, k), dimension, sigJ, sigd,  vecindad);
            tripletList.push_back(T((j)*h+i, (l)*h+k, calculo ));
          }
        }
      }
    }
  }
  //termina calculo de matriz de similaridades
  Eigen::SparseMatrix<float>  W(h*w, h*w); //construccion de matriz de similaridades
  W.setFromTriplets(tripletList.begin(), tripletList.end());
  return(W);
}

// [[Rcpp::export]]
Eigen::SparseMatrix<float> Kernel_float ( NumericMatrix M, int h, int w, double distancia, double sigJ, double sigd)
{
  // Funcion para calcular  la matriz de similaridades de todos los puntos de una imagen
  //  input: w: altura en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         h: ancho en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         distancia: limite de la vecindad del pixel a considerar
  //         M: imagen de 2 dimensiones (EN ESCALA DE GRISES)
  //         sigJ: sigma para kernel gaussiano, de la imagen
  //         sigd: sigma para kernell gaussiano, de las distancias
  //output: W, similaridad calculada, sparse
  int i, j, k, l = 0 ;
  float vecindad = (float)distancia;
  float d2 = 0.;
  float calculo = 0.;
  std::vector<T> tripletList; //inicializamos tripleta
  tripletList.reserve((int)(h*h*w*w*.1));
  //comienza calculo de matriz de similaridades
  for(i=0; i < h; i++)
  {
    for(j=0; j< w; j++ )
    {
      for(k=0; k<h; k++)
      {
        for(l=0; l<w; l++)
        {
          d2 = (float)(pow(i-k,2)+pow(j-l,2));
          if(d2 <= vecindad)
          {
            calculo = (float) (exp(-pow(M(j,i)-M(l,k),2)/(2*pow(sigJ,2))-d2/(2*pow(sigd,2))));
            tripletList.push_back(T((j)*h+i, (l)*h+k, calculo ));
          }
        }
      }
    }
  }
  //termina calculo de matriz de similaridades
  Eigen::SparseMatrix<float>  W(h*w, h*w); //construccion de matriz de similaridades
  W.setFromTriplets(tripletList.begin(), tripletList.end());
  return(W);
}


#include "iris/geometry.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>
#include "iris_cdd.h"

namespace iris {

int factorial(int n) {
  return n == 0 ? 1 : factorial(n - 1) * n;
}

double nSphereVolume(int dim, double radius) {
  double v;
  int k = std::floor(dim / 2);
  if (dim % 2 == 0) {
    v = std::pow(M_PI, k) / static_cast<double>(factorial(k));
  } else {
    v = (2.0 * factorial(k) * std::pow(4 * M_PI, k)) / static_cast<double>(factorial(2 * k + 1));
  }
  return v * std::pow(radius, dim);
}

Ellipsoid::Ellipsoid(int dim) :
  C_(Eigen::MatrixXd(dim, dim)),
  d_(Eigen::VectorXd(dim)) {}
Ellipsoid::Ellipsoid(Eigen::MatrixXd C, Eigen::VectorXd d):
  C_(C),
  d_(d) {}
const Eigen::MatrixXd& Ellipsoid::getC() const {
  return C_;
}
const Eigen::VectorXd& Ellipsoid::getD() const {
  return d_;
}
void Ellipsoid::setC(const Eigen::MatrixXd &C) {
  C_ = C;
}
void Ellipsoid::setCEntry(Eigen::DenseIndex row,
                          Eigen::DenseIndex col, double value) {
  C_(row, col) = value;
}
void Ellipsoid::setD(const Eigen::VectorXd &d) {
  d_ = d;
}
void Ellipsoid::setDEntry(Eigen::DenseIndex index, double value) {
  d_(index) = value;
}
int Ellipsoid::getDimension() const {
  return C_.cols();
}
double Ellipsoid::getVolume() const {
  return C_.determinant() * nSphereVolume(this->getDimension(), 1.0);
}
Ellipsoid Ellipsoid::fromNSphere(Eigen::VectorXd &center, double radius) {
  const int dim = center.size();
  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim, dim);
  C.diagonal().setConstant(radius);
  Ellipsoid ellipsoid(C, center);
  return ellipsoid;
}

Polyhedron::Polyhedron(int dim):
  A_(0, dim),
  b_(0, 1),
  dd_representation_dirty_(true) {}
Polyhedron::Polyhedron(Eigen::MatrixXd A, Eigen::VectorXd b):
    A_(A),
    b_(b),
    dd_representation_dirty_(true) {}
void Polyhedron::setA(const Eigen::MatrixXd &A) {
  A_ = A;
  dd_representation_dirty_ = true;
}
const Eigen::MatrixXd& Polyhedron::getA() const {
  return A_;
}
void Polyhedron::setB(const Eigen::VectorXd &b) {
  b_ = b;
  dd_representation_dirty_ = true;
}
const Eigen::VectorXd& Polyhedron::getB() const {
  return b_;
}
int Polyhedron::getDimension() const {
  return A_.cols();
}
int Polyhedron::getNumberOfConstraints() const {
  return A_.rows();
}
void Polyhedron::appendConstraints(const Polyhedron &other) {
  A_.conservativeResize(A_.rows() + other.getA().rows(), A_.cols());
  A_.bottomRows(other.getA().rows()) = other.getA();
  b_.conservativeResize(b_.rows() + other.getB().rows());
  b_.tail(other.getB().rows()) = other.getB();
  dd_representation_dirty_ = true;
}
void Polyhedron::updateDDRepresentation() {
  generator_points_.clear();
  generator_rays_.clear();
  getGenerators(A_, b_, generator_points_, generator_rays_);
  dd_representation_dirty_ = false;
}
std::vector<Eigen::VectorXd> Polyhedron::generatorPoints() {
  if (dd_representation_dirty_) {
    updateDDRepresentation();
  }
  return generator_points_;
}
std::vector<Eigen::VectorXd> Polyhedron::generatorRays() {
  if (dd_representation_dirty_) {
    updateDDRepresentation();
  }
  return generator_rays_;
}
bool Polyhedron::contains(Eigen::VectorXd point, double tolerance) {
  return (A_ * point - b_).maxCoeff() <= tolerance;
}

}


/**
 * Kalman Filter
 * Skeleton code for teaching
 * A3M33MKR
 * Czech Technical University
 * Faculty of Electrical Engineering
 * Intelligent and Mobile Robotics group
 *
 * Authors: Zdeněk Kasl, Karel Košnar kosnar@labe.felk.cvut.cz
 *
 * this code is inspired by tutorial on Kalman Filter by Greg Czerniak
 *
 * Licence: MIT (see LICENSE file)
 **/

#include<cmath>
#include<cassert>
#include<cstdlib>
#include<fstream>
#include<iostream>
#include <Eigen/Dense>

#include "gui/gui.h"
#include "systemSimulator/system.h"
using namespace imr;
using namespace gui;
using namespace Eigen;

Point KalmanFilter(Point measuredPosition);

//    float v0 = 149;
//    float alpha = (float) (M_PI / 4 - 0.007);
    float v0 = 150;
    float alpha = (float) (M_PI / 4);
    float dt = 0.1;
    float g = (float) (9.8 * dt);

    Matrix4f A,R,Sigma;
    Matrix<float,4,1> B;
    Matrix2f Q;
    Matrix<float,2,4> C;
    Vector4f X;

void help(char** argv)
{
    std::cout << "\nUsage of the program " << argv[0]+2 << std::endl
         << "Parameter [-h or -H] displays this message." <<std::endl
	 << " Parameter [-n or -N] number of simulation steps."
         << std::endl;
}

int main(int argc, char** argv)
{
    A <<    1,0,dt,0,
            0,1,0,dt,
            0,0,1,0,
            0,0,0,1;
    B <<    0,
            0,
            0,
            -1;
    C <<    1,0,0,0,
            0,1,0,0;
    X <<   0,
            0,
            cos(alpha) * v0,
            sin(alpha) * v0;
    R <<    3,0,0,0,
            0,3,0,0,
            0,0,3,0,
            0,0,0,3;
    Q <<    50,0,
            0,50;
    Sigma << 1,0,0,0,
            0,1,0,0,
            0,0,1,0,
            0,0,0,1;

    int nSteps = 1000;
    char *dataFile;

    // Parse all console parameters
    for (int i=0; i<argc; i++) {
        if (argv[i][0]=='-') {
            switch(argv[i][1]) {
                //> HELP
                case 'H' : case 'h' :
                    help(argv);
                    break;
		case 'N': case 'n':
                    assert(i+1 < argc);
                    assert(atoi(argv[i+1])>1);
                    nSteps = atoi(argv[i+1]);
		    break;
                default :
                    std::cout << "Parameter \033[1;31m" << argv[i] << "\033[0m is not valid!\n"
                        << "Use parameter -h or -H for help." << std::endl;
                    break;
            }
        }
    }
    // All parameters parsed

    Gui gui;
    System system;

    //> comment line below in order to let the program
    //> continue right away
//    gui.startInteractor();

    Point measurement;
    Point truth;
    Point kfPosition;

    for(int i=1; i<nSteps; i++) {
       system.makeStep();
       truth = system.getTruthPosition();
       measurement = system.getMeasurement();
       kfPosition = KalmanFilter(measurement);
       gui.setPoints(truth, measurement, kfPosition);

       //> comment line below in order to let the program
       //> continue right away
       if (i%40 == 0) gui.startInteractor();

    }
    gui.startInteractor();

    return EXIT_SUCCESS;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//                               implement Kalman Filter here
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

Point  KalmanFilter(const Point measuredPosition)
{
    Point ret;
    Vector2f Z;
    Z <<    measuredPosition.x,
            measuredPosition.y;
    Matrix<float,4,1> input = g * B;
    Vector4f X_ = A * X + input;
    Matrix4f Sigma_ = A * Sigma * A.transpose() + R;
    Matrix<float,4,2> K =  Sigma_ * C.transpose() * (C * Sigma_ * C.transpose() + Q).inverse();
    X = X_ + 1*K * (Z - C * X_);

    ret.x = X(0);
    ret.y = X(1);
    return ret;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Free energy, energy, and specific heat of square lattice Ising model

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/simpson.hpp>

namespace ising {
namespace free_energy {
namespace square {

namespace {

template<typename T, typename FVAR>
struct functor {
  functor(T Jx, T Jy, FVAR beta) {
    using std::cosh; using std::sinh;
    c_ = 1 / (2 * boost::math::constants::pi<T>());
    chab_ = cosh(2 * beta * Jx) * cosh(2 * beta * Jy);
    k_ = 1.0 / (sinh(2 * beta * Jx) * sinh(2 * beta * Jy));
  }
  FVAR operator()(T t) const {
    using std::cos; using std::log; using std::sqrt;
    return c_ * log(chab_ + sqrt(1 + k_ * k_ - 2 * k_ * cos(2 * t)) / k_);
  }
  T c_;
  FVAR chab_, k_;
};

template<typename T, typename FVAR>
functor<T, FVAR> func(T Jx, T Jy, FVAR beta) { return functor<T, FVAR>(Jx, Jy, beta); }

}

template<typename T>
inline std::tuple<T, T, T> infinite(T Jx, T Jy, T beta) {
  typedef T real_t;
  using std::log;
  if (Jx <= 0 || Jy <= 0) throw(std::invalid_argument("Jx and Jy should be positive"));
  if (beta <= 0) throw(std::invalid_argument("beta should be positive"));
  const real_t pi = boost::math::constants::pi<real_t>();
  auto beta_fvar = boost::math::differentiation::make_fvar<real_t, 2>(beta);
  auto logZ = log(real_t(2)) / 2 + standards::simpson_1d(func(Jx, Jy, beta_fvar), real_t(0), pi, 8);
  real_t d2 = logZ.derivative(2);
  for (unsigned long n = 16; n < 1024; n *= 2) {
    logZ = log(real_t(2)) / 2 + standards::simpson_1d(func(Jx, Jy, beta_fvar), real_t(0), pi, n);
    if (beta * beta * abs((logZ.derivative(2) - d2) / logZ.derivative(0)) <
        2 * std::numeric_limits<real_t>::epsilon()) break;
    d2 = logZ.derivative(2);
  }
  real_t free_energy = - logZ.derivative(0) / beta;
  real_t energy = - logZ.derivative(1);
  real_t specific_heat = beta * beta * logZ.derivative(2);
  return std::make_tuple(free_energy, energy, specific_heat);
}

template<typename I, typename T>
inline std::tuple<T, T, T> finite(I Lx, I Ly, T Jx, T Jy, T beta) {
  typedef T real_t;
  using std::exp; using std::log;
  if (Lx <= 0 || Ly <= 0) throw(std::invalid_argument("Lx and Ly should be positive"));
  if (Jx <= 0 || Jy <= 0) throw(std::invalid_argument("Jx and Jy should be positive"));
  if (beta <= 0) throw(std::invalid_argument("beta should be positive"));
  const real_t pi = boost::math::constants::pi<real_t>();
  auto beta_fvar = boost::math::differentiation::make_fvar<real_t, 2>(beta);
  auto a = beta_fvar * Jx;
  auto b = beta_fvar * Jy;
  decltype(beta_fvar) lp0(0), lp1(0), lp2(0), lp3(0);
  for (std::size_t k = 0; k < 2 * Lx; ++k) {
    auto cosh_g = (cosh(2*a) * cosh(2*b) - cos(pi * k / Lx) * sinh(2*b)) / sinh(2*a);
    auto gamma_abs = log(cosh_g + sqrt(cosh_g * cosh_g - 1));
    if ((k & 1) == 1) {
      lp0 += (Ly * gamma_abs/2) + log(1 + exp(-(Ly*gamma_abs)));
      lp1 += (Ly * gamma_abs/2) + log(1 - exp(-(Ly*gamma_abs)));
    } else {
      lp2 += (Ly * gamma_abs/2) + log(1 + exp(-(Ly*gamma_abs)));
      lp3 += (Ly * gamma_abs/2) + log(1 - exp(-(Ly*gamma_abs)));
    }
  }
  auto logZ = -log(real_t(2)) / (Lx * Ly) + a + real_t(1)/2 * log(1 - exp(-4*a));
  if (sinh(2*a) * sinh(2*b) < real_t(1)) {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) - exp(lp3-lp0))) / (Lx * Ly);
  } else {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) + exp(lp3-lp0))) / (Lx * Ly);
  }
  real_t free_energy = - logZ.derivative(0) / beta;
  real_t energy = - logZ.derivative(1);
  real_t specific_heat = beta * beta * logZ.derivative(2);
  return std::make_tuple(free_energy, energy, specific_heat);
}

} // end namespace square
} // end namespace free_energy
} // end namespace ising


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

////////////////////////////////////////////////////////////////////////////////
/// @file
/// Solution of the Euler Problem #3
/// (link).
////////////////////////////////////////////////////////////////////////////////

#include <stapl/utility/do_once.hpp>
#include <stapl/array.hpp>
#include <stapl/algorithm.hpp>
#include <boost/lexical_cast.hpp>

////////////////////////////////////////////////////////////////////////////////
/// @brief Returns truncated integer square root of given number.
/// @tparam IntType Integral type of the number.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
inline IntType sqrt_int(IntType n)
{
  return static_cast<IntType>( std::sqrt(n) );
}

////////////////////////////////////////////////////////////////////////////////
/// @brief Functor that returns the larger of @p n and @p x/n that is both a
///        factor of @p x and a prime number (0 if neither satisfies these
///        conditions).
/// @tparam IntType Integral type of the dividend @p x.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
struct largest_prime_factors_filter
{
  static_assert(std::is_integral<IntType>::value, "Integer required.");

  typedef IntType result_type;

  largest_prime_factors_filter(IntType x)
    : m_x(x)
  { }

  IntType operator()(IntType n)
  {
    if ((m_x % n) != 0)   // n is not a factor of x
      return 0;

    if (is_prime(m_x/n))  // x/n (which is larger than n) is prime
      return m_x/n;

    return is_prime(n) ? n : 0;
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_x);
  }

private:
  IntType m_x;

  static bool is_prime(IntType n)
  {
    IntType sqrt_n = sqrt_int(n);

    for (IntType i = 2; i <= sqrt_n; ++i)
    {
      if ((n % i) == 0)
        return false;
    }

    return true;
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  using value_t = std::size_t;

  if (argc < 2)
  {
    stapl::do_once( [] {
      std::cout << "Run as: mpirun -n <ncpu> pe_3b <number>" << std::endl;
    });
    return EXIT_FAILURE;
  }

  stapl::counter<stapl::default_timer> exec_timer;
  exec_timer.start();

  // Read the dividend from command line.
  value_t num = boost::lexical_cast<value_t> (argv[1]);

  // Create a generator of consecutive values from 2 to sqrt(num).
  auto c_vw = stapl::counting_view(sqrt_int(num)-1, 2);

  // Apply the filter that effectively replaces each generated number by zero
  // if it is not prime factor of <num> and accumulate a maximum of these
  // numbers.
  value_t max_prime_factor = stapl::map_reduce(
      largest_prime_factors_filter<value_t>(num),
      stapl::max<value_t>(),
      c_vw);

  double t = exec_timer.stop();

  stapl::do_once( [&] {
    std::cout << "Computation finished (time taken: " << t << " s)."
              << std::endl
              << "Largest prime factor of " << num << " is: "
              << max_prime_factor << std::endl;
  });

  return EXIT_SUCCESS;
}


#include "resection.h"

#include <Eigen/SVD>
#include <iostream>
#include "sfm_utils.h"
#include "defines.h"

namespace {

    matrix34d canonicalizeP(const matrix34d &P)
    {
        matrix3d RR = P.get_minor<3, 3>(0, 0);
        vector3d tt;
        tt[0] = P(0, 3);
        tt[1] = P(1, 3);
        tt[2] = P(2, 3);

        if (cv::determinant(RR) < 0) {
            RR *= -1;
            tt *= -1;
        }

        double sc = 0;
        for (int i = 0; i < 9; i++) {
            sc += RR.val[i] * RR.val[i];
        }
        sc = std::sqrt(3 / sc);

        Eigen::MatrixXd RRe;
        copy(RR, RRe);
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(RRe, Eigen::ComputeFullU | Eigen::ComputeFullV);
        RRe = svd.matrixU() * svd.matrixV().transpose();
        copy(RRe, RR);

        tt *= sc;

        matrix34d result;
        for (int i = 0; i < 9; ++i) {
            result(i / 3, i % 3) = RR(i / 3, i % 3);
        }
        result(0, 3) = tt(0);
        result(1, 3) = tt(1);
        result(2, 3) = tt(2);

        return result;
    }

    // (см. Hartley & Zisserman p.178)
    cv::Matx34d estimateCameraMatrixDLT(const cv::Vec3d *Xs, const cv::Vec3d *xs, int count)
    {
        using mat = Eigen::MatrixXd;
        using vec = Eigen::VectorXd;

        mat A(2 * count, 12);

        for (int i = 0; i < count; ++i) {

            double x = xs[i][0];
            double y = xs[i][1];
            double w = xs[i][2];

            double X = Xs[i][0];
            double Y = Xs[i][1];
            double Z = Xs[i][2];
            double W = 1.0;

            A.row(i * 2 + 0) << 0, 0, 0, 0, -w*X, -w*Y, -w*Z, -w*W, y*X, y*Y, y*Z, y*W;
            A.row(i * 2 + 1) << w*X, w*Y, w*Z, w*W, 0, 0, 0, 0, -x*X, -x*Y, -x*Z, -x*W;
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::VectorXd null_space = svd.matrixV().col(11);

        matrix34d result;
        for (int i = 0; i < 12; ++i) {
            result(i / 4, i % 4) = null_space(i);
        }

        return canonicalizeP(result);
    }

    cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector<cv::Vec3d> &X, const std::vector<cv::Vec2d> &x, bool verbose)
    {
        if (X.size() != x.size()) {
            throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()");
        }

        const int n_points = X.size();

        // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
        // будет отличаться от случая с гомографией
        const int n_trials = 10000;

        const double threshold_px = 3;

        const int n_samples = 6;
        uint64_t seed = 1;

        int best_support = 0;
        cv::Matx34d best_P;

        std::vector<int> sample;
        for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
            phg::randomSample(sample, n_points, n_samples, &seed);

            cv::Vec3d ms0[n_samples];
            cv::Vec3d ms1[n_samples];
            for (int i = 0; i < n_samples; ++i) {
                ms0[i] = X[sample[i]];
                ms1[i] = calib.unproject(x[sample[i]]);
            }

            cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples);

            int support = 0;
            for (int i = 0; i < n_points; ++i) {
                cv::Vec3d pt = calib.project(P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1.0));
                if (pt[2] == 0) {
                    continue;
                }
                cv::Vec2d px = {pt[0] / pt[2], pt[1] / pt[2]};
                if (cv::norm(px - x[i]) < threshold_px) {
                    ++support;
                }
            }

            if (support > best_support) {
                best_support = support;
                best_P = P;

                if (verbose) std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl;

                if (best_support == n_points) {
                    break;
                }
            }
        }

        if (verbose) std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl;

        if (best_support == 0) {
            throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix");
        }

        return best_P;
    }

}

cv::Matx34d phg::findCameraMatrix(const Calibration &calib, const std::vector <cv::Vec3d> &X, const std::vector <cv::Vec2d> &x, bool verbose) {
    return estimateCameraMatrixRANSAC(calib, X, x, verbose);
}


#pragma once
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include "serializer.hpp"

namespace tomoto
{
	namespace math
	{
		template<typename _Ty = float>
		struct MultiNormalDistribution
		{
			static constexpr _Ty log2pi = (_Ty)1.83787706641;
			Eigen::Matrix<_Ty, -1, 1> mean;
			Eigen::Matrix<_Ty, -1, -1> cov, l;
			_Ty logDet = 0;

			MultiNormalDistribution(size_t k = 0) :
				mean{ Eigen::Matrix<_Ty, -1, 1>::Zero(k) },
				cov{ Eigen::Matrix<_Ty, -1, -1>::Identity(k, k) },
				l{ Eigen::Matrix<_Ty, -1, -1>::Identity(k, k) }
			{
			}

			_Ty getLL(const Eigen::Matrix<_Ty, -1, 1>& x) const
			{
				_Ty ll = -((x - mean).transpose() * cov.inverse() * (x - mean))[0] / 2;
				ll -= log2pi * mean.size() / 2 + logDet;
				return ll;
			}

			const Eigen::Matrix<_Ty, -1, -1>& getCovL() const
			{
				return l;
			}

			template<typename _List>
			static MultiNormalDistribution<_Ty> estimate(_List list, size_t len)
			{
				MultiNormalDistribution<_Ty> newDist;
				if (len)
				{
					newDist.mean = list(0);
					for (size_t i = 1; i < len; ++i) newDist.mean += list(i);
					newDist.mean /= len;
					newDist.cov = Eigen::Matrix<_Ty, -1, -1>::Identity(newDist.mean.size(), newDist.mean.size());
					for (size_t i = 0; i < len; ++i)
					{
						Eigen::Matrix<_Ty, -1, 1> o = list(i) - newDist.mean;
						newDist.cov += o * o.transpose();
					}
					if (len > 1) newDist.cov /= len - 1;
				}
				Eigen::MatrixXd l = newDist.cov.template cast<double>().llt().matrixL();
				newDist.l = l.template cast<float>();
				newDist.logDet = l.diagonal().array().log().sum();
				return newDist;
			}

			DEFINE_SERIALIZER_CALLBACK(onRead, mean, cov);
		private:
			void onRead()
			{
				l = cov.llt().matrixL();
				logDet = l.diagonal().array().log().sum();
			}
		};

	}
}


#ifndef POLYTOPE_H
#define POLYTOPE_H

#include <cassert>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

namespace integrator_chains {

/** Basic half-space representation of polytopes.

   \ingroup integrator_chains */
class Polytope {
  public:
    Polytope( Eigen::MatrixXd incoming_H, Eigen::VectorXd incoming_K );
    Polytope( const Polytope &other );

    /** Check that defining matrices have consistent dimensions.

       The "defining matrices" are those involved in the inequality providing
       the half-space representation, i.e., H and K in {x | Hx <= K}.
       N.B., this check does not occur during instantiation of Polytope. */
    bool is_consistent() const;

    /** Is X contained in this polytope?

       The polytope is a closed set. No numerical tolerance is used. */
    bool is_in( Eigen::VectorXd X ) const;

    /** Intersect two polytopes that are defined in the same dimension space. */
    Polytope operator&( const Polytope &P2 );

    // Factory functions
    /** Create random Polytope in R^n using Eigen::MatrixXd::Random().

       \param n dimension of the containing space.

       Random matrices as provided by Eigen::MatrixXd::Random() are used to
       instantiate Polytope in half-space representation. The matrices have
       \c n+1 rows, corresponding to \c n+1 inequalities.
    */
    static Polytope * randomH( int n );

    /** Construct Polytope that is an axis-aligned rectangle.

       \param bounds an array of the form
       \code
       [x1_min, x1_max, x2_min, x2_max, ..., xn_min, xn_max],
       \endcode
       which defines a polytope in terms of intervals along each axis in R^n.
       The interval along the first axis is [x1_min, x1_max], the interval along
       the second axis is [x2_min, x2_max], etc. Thus the length of the given
       array is 2n.

       E.g., a unit square in R^2 can be created using
       \code
           Eigen::Vector4d bounds;
           bounds << 0, 1,
                     0, 1;
           Polytope *square = Polytope::box( bounds );
       \endcode
    */
    static Polytope * box( const Eigen::VectorXd &bounds );

    /** Output this polytope in JSON to given stream. */
    friend std::ostream & operator<<( std::ostream &out, const Polytope &P );

    /** Output this polytope in JSON excluding opening { and closing }.

       This method facilitates inheritance from Polytope without crowding the
       JSON representations of other classes. */
    void dumpJSONcore( std::ostream &out ) const;

  private:
    // { x \in R^n | Hx \leq K }
    Eigen::MatrixXd H;
    Eigen::VectorXd K;
};

std::ostream & operator<<( std::ostream &out, const Polytope &P )
{
    out << "{ ";
    P.dumpJSONcore( out );
    out << " }";
    return out;
}

void Polytope::dumpJSONcore( std::ostream &out ) const
{
    int i, j;
    out << "\"H\": [";
    for (i = 0; i < H.rows(); i++) {
        if (i > 0) {
            out << ", ";
        }
        out << "[";
        for (j = 0; j < H.cols(); j++) {
            if (j > 0)
                out << ", ";
            out << H(i,j);
        }
        out << "]";
    }
    out << "], \"K\": [";
    for (i = 0; i < K.rows(); i++) {
        if (i > 0) {
            out << ", ";
        }
        out << K(i);
    }
    out << "]";
}

Polytope * Polytope::randomH( int n )
{
    return new Polytope( Eigen::MatrixXd::Random( n+1, n ),
                         Eigen::VectorXd::Random( n+1 ) );
}

Polytope * Polytope::box( const Eigen::VectorXd &bounds )
{
    assert( bounds.size() % 2 == 0 );

    int n = bounds.size()/2;
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero( 2*n, n );
    Eigen::VectorXd K( 2*n );
    for (int i = 0; i < n; i++) {
        H(2*i, i) = -1;
        K(2*i) = -bounds(2*i);
        H(2*i+1, i) = 1;
        K(2*i+1) = bounds(2*i+1);
    }
    return new Polytope( H, K );
}

bool Polytope::is_consistent() const
{
    if (H.rows() == K.size()) {
        return true;
    } else {
        return false;
    }
}

bool Polytope::is_in( Eigen::VectorXd X ) const
{
    assert( H.cols() == X.size() );

    if ((H*X-K).maxCoeff() <= 0) {
        return true;
    } else {
        return false;
    }
}

Polytope Polytope::operator&( const Polytope &P2 )
{
    assert( this->H.cols() == P2.H.cols() );

    Eigen::MatrixXd H( this->H.rows() + P2.H.rows(), this->H.cols() );
    Eigen::VectorXd K( this->H.rows() + P2.H.rows(), 1 );

    H.topRows( this->H.rows() ) = this->H;
    K.topRows( this->H.rows() ) = this->K;
    H.bottomRows( P2.H.rows() ) = P2.H;
    K.bottomRows( P2.H.rows() ) = P2.K;

    return Polytope( H, K );
}

Polytope::Polytope( Eigen::MatrixXd incoming_H, Eigen::VectorXd incoming_K )
    : H(incoming_H), K(incoming_K)
{ }

Polytope::Polytope( const Polytope &other )
    : H(other.H), K(other.K)
{ }

/** Extension of Polytope to have label (string).

   This class provides a basis for labeling trajectories. E.g., the labeling of
   the state can be a set of strings corresponding to the labels of polytopes
   containing the state.

   \ingroup integrator_chains */
class LabeledPolytope : public Polytope {
public:
    std::string label;

    LabeledPolytope( const Polytope &other );

    /** Polytope::box() but including a label. */
    static LabeledPolytope * box( const Eigen::VectorXd &bounds, std::string label="" );

    /** Output this labeled polytope in JSON to given stream. */
    friend std::ostream & operator<<( std::ostream &out, const LabeledPolytope &P );
};

LabeledPolytope::LabeledPolytope( const Polytope &other )
    : label(""), Polytope( other )
{ }

LabeledPolytope * LabeledPolytope::box( const Eigen::VectorXd &bounds, std::string label )
{
    Polytope *P = Polytope::box( bounds );
    LabeledPolytope *Q = new LabeledPolytope( *P );
    delete P;
    Q->label = label;
    return Q;
}

std::ostream & operator<<( std::ostream &out, const LabeledPolytope &P )
{
    out << "{ \"label\": \"" << P.label << "\", ";
    P.dumpJSONcore( out );
    out << " }";
    return out;
}

} // namespace integrator_chains

#endif  // ifndef POLYTOPE_H


#include "vio/eigen_utils.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace vio {

Eigen::Vector3d rotro2eu(Eigen::Matrix3d R) {
  Eigen::Vector3d euler;
  euler[0] = atan2(R(2, 1), R(2, 2));
  euler[1] = -(atan2(R(2, 0), sqrt(1 - R(2, 0) * R(2, 0))));
  euler[2] = atan2(R(1, 0), R(0, 0));
  return euler;
}

Eigen::Matrix3d roteu2ro(Eigen::Vector3d eul) {
  double cr = cos(eul[0]);
  double sr = sin(eul[0]);  // roll
  double cp = cos(eul[1]);
  double sp = sin(eul[1]);  // pitch
  double ch = cos(eul[2]);
  double sh = sin(eul[2]);  // heading
  Eigen::Matrix3d dcm;
  dcm(0, 0) = cp * ch;
  dcm(0, 1) = (sp * sr * ch) - (cr * sh);
  dcm(0, 2) = (cr * sp * ch) + (sh * sr);

  dcm(1, 0) = cp * sh;
  dcm(1, 1) = (sr * sp * sh) + (cr * ch);
  dcm(1, 2) = (cr * sp * sh) - (sr * ch);

  dcm(2, 0) = -sp;
  dcm(2, 1) = sr * cp;
  dcm(2, 2) = cr * cp;
  return dcm;
}
// input: lat, long in radians, height is immaterial
// output: Ce2n
Eigen::Matrix3d llh2dcm(const Eigen::Vector3d llh) {
  double sL = sin(llh[0]);
  double cL = cos(llh[0]);
  double sl = sin(llh[1]);
  double cl = cos(llh[1]);

  Eigen::Matrix3d Ce2n;
  Ce2n << -sL * cl, -sL * sl, cL, -sl, cl, 0, -cL * cl, -cL * sl, -sL;
  return Ce2n;
}

Eigen::MatrixXd nullspace(const Eigen::MatrixXd &A) {
  Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
  // ColPivHouseholderQR<MatrixXd> qr(A); //don't use column pivoting because in
  // that case Q*R-A!=0
  Eigen::MatrixXd nullQ = qr.householderQ();

  int rows = A.rows(), cols = A.cols();
  assert(rows >
         cols);  // "Rows should be greater than columns in computing nullspace"
  nullQ = nullQ.block(0, cols, rows, rows - cols).eval();
  return nullQ;
}

void leftNullspaceAndColumnSpace(const Eigen::MatrixXd &A, Eigen::MatrixXd *Q2,
                                 Eigen::MatrixXd *Q1) {
  int rows = A.rows(), cols = A.cols();
  assert(rows > cols);  // "Rows should be greater than columns in computing
                        // left nullspace"
  Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
  // don't use column pivoting because in that case Q*R-A!=0
  Eigen::MatrixXd Q = qr.householderQ();

  Q2->resize(rows, rows - cols);
  *Q2 = Q.block(0, cols, rows, rows - cols);

  Q1->resize(rows, cols);
  *Q1 = Q.block(0, 0, rows, cols);
}

Eigen::Matrix<double, Eigen::Dynamic, 1> superdiagonal(
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &M) {
  const int numElements = std::min(M.rows(), M.cols()) - 1;
  Eigen::Matrix<double, Eigen::Dynamic, 1> r(numElements, 1);
  for (int jack = 0; jack < numElements; ++jack) r[jack] = M(jack, jack + 1);
  return r;
}

Eigen::Matrix<double, Eigen::Dynamic, 1> subdiagonal(
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &M) {
  const int numElements = std::min(M.rows(), M.cols()) - 1;
  Eigen::Matrix<double, Eigen::Dynamic, 1> r(numElements, 1);
  for (int jack = 0; jack < numElements; ++jack) r[jack] = M(jack + 1, jack);
  return r;
}

void reparameterize_AIDP(const Eigen::Matrix3d &Ri, const Eigen::Matrix3d &Rj,
                         const Eigen::Vector3d &abrhoi,
                         const Eigen::Vector3d &pi, const Eigen::Vector3d &pj,
                         Eigen::Vector3d &abrhoj,
                         Eigen::Matrix<double, 3, 9> *jacobian) {
  Eigen::Matrix<double, 4, 4> Tci2cj = Eigen::Matrix<double, 4, 4>::Identity();
  Tci2cj.topLeftCorner<3, 3>() = Rj.transpose() * Ri;
  Tci2cj.block<3, 1>(0, 3).noalias() = Rj.transpose() * (pi - pj);
  Eigen::Matrix<double, 4, 1> homogi;
  homogi << abrhoi.head<2>(), 1, abrhoi[2];
  double rhoj_drhoi = 1 / (Tci2cj.row(2) * homogi);  //\rho_j divided by \rho_i
  abrhoj.head<2>() = rhoj_drhoi * Tci2cj.topLeftCorner<2, 4>() * homogi;
  abrhoj[2] = abrhoi[2] * rhoj_drhoi;
  if (jacobian) {
    Eigen::Matrix3d lhs;
    lhs.setIdentity();
    lhs.col(2) = -abrhoj;
    //{\alpha, \beta, \rho}_i
    Eigen::Matrix<double, 3, 3> subrhs;
    subrhs << Tci2cj.topLeftCorner<3, 2>(), Tci2cj.block<3, 1>(0, 3);
    jacobian->topLeftCorner<3, 3>() = rhoj_drhoi * lhs * subrhs;
    (*jacobian)(2, 2) =
        rhoj_drhoi * rhoj_drhoi * Tci2cj.block<1, 3>(2, 0) * homogi.head<3>();
    //{pi, pj}
    Eigen::Matrix<double, 3, 6> rhs;
    rhs.topLeftCorner<3, 3>() = abrhoi[2] * Rj.transpose(),
                         rhs.block<3, 3>(0, 3) = -rhs.topLeftCorner<3, 3>();
    jacobian->block<3, 6>(0, 3) = rhoj_drhoi * lhs * rhs;
  }
}

void reparameterizeNumericalJacobian(const Eigen::Matrix3d &Ri,
                                     const Eigen::Matrix3d &Rj,
                                     const Eigen::Vector3d &abrhoi,
                                     const Eigen::Vector3d &pi,
                                     const Eigen::Vector3d &pj,
                                     Eigen::Vector3d &abrhoj,
                                     Eigen::Matrix<double, 3, 9> &jacobian) {
  // numerical differentation
  Eigen::Vector3d abrhojp;
  reparameterize_AIDP(Ri, Rj, abrhoi, pi, pj, abrhoj);
  double h = 1e-8;
  for (int jack = 0; jack < 3; ++jack) {
    Eigen::Vector3d abrhoip = abrhoi;
    abrhoip[jack] = abrhoi[jack] + h;
    reparameterize_AIDP(Ri, Rj, abrhoip, pi, pj, abrhojp);
    Eigen::Vector3d subJacobian = (abrhojp - abrhoj) / h;
    jacobian.col(jack) = subJacobian;
  }

  for (int jack = 0; jack < 3; ++jack) {
    Eigen::Vector3d pip = pi;
    pip[jack] = pi[jack] + h;
    reparameterize_AIDP(Ri, Rj, abrhoi, pip, pj, abrhojp);
    Eigen::Vector3d subJacobian = (abrhojp - abrhoj) / h;
    jacobian.col(jack + 3) = subJacobian;
  }

  for (int jack = 0; jack < 3; ++jack) {
    Eigen::Vector3d pjp = pj;
    pjp[jack] = pj[jack] + h;
    reparameterize_AIDP(Ri, Rj, abrhoi, pi, pjp, abrhojp);
    Eigen::Vector3d subJacobian = (abrhojp - abrhoj) / h;
    jacobian.col(jack + 6) = subJacobian;
  }
}

Eigen::Vector3d unskew3d(const Eigen::Matrix3d &Omega) {
  return 0.5 * Eigen::Vector3d(Omega(2, 1) - Omega(1, 2),
                               Omega(0, 2) - Omega(2, 0),
                               Omega(1, 0) - Omega(0, 1));
}

// https://github.com/ethz-asl/maplab_lightweight_filtering/blob/28417454164b08d5483754f0c0d250b84f2ce951/include/lightweight_filtering/Update.hpp
double getConditionNumberOfMatrix(const Eigen::MatrixXd &matrix) {
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
  return svd.singularValues()(0) /
         svd.singularValues()(svd.singularValues().size() - 1);
}
}  // namespace vio


#include "engine/math/hcMatrix.h"
#include "engine/hcImageFITS.h"
//#include "engine/math/coordTransform.h"

#include "src/pfss.h"

//#include "extern/soil/SOIL.h"

#include <stdio.h>
#include <string.h>
#include <fstream>

#include <boost/regex.hpp>
#include "boost/lexical_cast.hpp"
#include <fstream>

using namespace std;
using namespace boost;

/*! returns the magnetic field vector at given position (spherical coordinates) for a dipole
 *  at the origin oriented along the positive z-axis. Return value given in spherical coordinates.
 *  mathematics found at http://www.physicsinsights.org/dipole_field_1.html
 */
Vec3D getDipoleField(Vec3D pos, hcFloat dipole, bool debug)
{
	hcFloat r		= pos[0] / r_sol;
	hcFloat theta	= pos[1];
	hcFloat stheta	= sin(theta);
	hcFloat ctheta	= cos(theta);
	hcFloat fac		= dipole / (r * r * r);
	hcFloat Br		= fac * 2 * ctheta;
	hcFloat Bt		= fac * stheta;

	Vec3D retval((hcFloat)Br, (hcFloat)Bt, (hcFloat)0.0f);

	if(debug)
		printf("r: %E, t: %E, fac: %E, Br: %E, Bt: %E\n", r, theta, fac, Br, Bt);

	return retval;
}

/*! mathematics found in "The Model Magnetic Configuration of the Extended Corona
in the Solar Wind Formation Region - Igor Veselovsky and Olga Panasenco"
 */
Vec3D getDipoleCurrentField(Vec3D pos, hcFloat dipole, bool debug)
{
	hcFloat r		= pos[0] / r_sol;
	hcFloat theta	= pos[1];
	hcFloat stheta	= sin(theta);
	hcFloat ctheta	= cos(theta);
	hcFloat fac		= dipole / (r * r * r);

	hcFloat mu0		= 4*PI*1E-7;
	hcFloat mu		= mu0 * dipole;
	hcFloat r0		= 2.5;
	hcFloat B0		= -1E-8;
	hcFloat Ba		= (B0*r0*r0) * (B0*r0*r0) * (B0*r0*r0) / (4 * mu * mu);
	hcFloat a		= 2 * mu / (B0*r0*r0);
	hcFloat rho		= r / a;

	hcFloat Br		= Ba / (rho * rho) * (ctheta / rho + (theta - PI/2.0 < 0 ? -1.0 : 1.0));
	hcFloat Bt		= 1 / (2 * rho * rho * rho) * Ba * stheta;

	if(debug)
	{
		printf("r: %E, t: %E, Ba: %E, a: %E, rho: %E, Br: %E, Bt: %E\n", r, theta, Ba, a, rho, Br, Bt);
		//printf("(B0*r0*r0): %E, ^3: %E, denom: %E, ba: %E\n", (B0*r0*r0), (B0*r0*r0) * (B0*r0*r0) * (B0*r0*r0), (4 * dipole * dipole), Ba);
	}

	Vec3D retval((hcFloat)Br, (hcFloat)Bt, (hcFloat)0.0f);

	return retval;
}

PFSSsolution_SHC::PFSSsolution_SHC(const char *filename, bool WSOfile, hcFloat r_ss)
{
	initNULL();
	importCoefficients(filename, WSOfile, r_ss);
}

PFSSsolution_SHC::PFSSsolution_SHC()
{
    initNULL();
}

PFSSsolution_SHC::~PFSSsolution_SHC()
{
    clear();
}

PFSSsolution_SHC::PFSSsolution_SHC(const PFSSsolution_SHC &other)
{
	printf("PFSS_magfield: cpy constructor not implemented!\n");
	initNULL();
}

PFSSsolution_SHC &PFSSsolution_SHC::operator=(const PFSSsolution_SHC &other)
{
	if(this == &other)
		return *this;

	printf("PFSSsolution_SHC: assignment operator not implemented!\n");

	return *this;
}

void PFSSsolution_SHC::initNULL()
{
	g 					= NULL;
	h 					= NULL;
	assLegPoly 			= NULL;
	coeffOrder 			= 0;
}

void PFSSsolution_SHC::clear()
{
	if(g!=NULL)
	{
		for(uint i=0;i<coeffOrder;++i)
		{
			delete [] g[i];
			delete [] h[i];
			delete [] assLegPoly[i];
		}
		delete [] g;
		delete [] h;
		delete [] assLegPoly;
	}

	initNULL();
}

/*! computes spherical harmonic coefficients from synoptic magnetogram
 *
 * 	@param 	order			maximum order of coefficients to be computed
 * 	@param	r_ss			heliocentric position of source surface (in m)
 * 	@param	photMagfield	data structure containing synoptic photospheric (LOS) magnetic field strength
 */
void PFSSsolution_SHC::determineCoefficientsFromPhotMagfield(uint order, hcFloat r_ss, SynPhotMagfield &photMagfield)
{
    init(order, r_ss);

    uint width 			= photMagfield.width;
    uint height 		= photMagfield.height;
    hcFloat	maxSinLat	= photMagfield.synInfo.maxSinLat;
    hcFloat minSinLat	= -photMagfield.synInfo.maxSinLat;
    hcFloat numerator 	= 1.0 / (width * height);

    for(uint l=0;l<=coeffOrder;++l)
        for(uint m=0;m<=l;++m)
        {
            g[l][m] = 0.0;
            h[l][m] = 0.0;
            for(uint y=0;y<height;++y)
            {
            	hcFloat dSinTheta 	= (maxSinLat - minSinLat) / (height-1);
                hcFloat theta		= PI / 2.0 - asin(maxSinLat - (height - 1 - y) * dSinTheta);
                hcFloat legPolyEval = assLegPoly[l][m].operator ()(cos(theta));
                hcFloat cosecans	= 1.0 / sin(theta);

                for(uint x=0;x<width;++x)
                {
                    hcFloat phi		= m * (x+1.0/2.0) * 2.0 * PI / width;
					hcFloat D_xy 	= photMagfield(x,y) * cosecans;
					hcFloat gm		= D_xy * cos(phi) * legPolyEval;
					hcFloat hm		= D_xy * sin(phi) * legPolyEval;
                    g[l][m] 		+= gm;
                    h[l][m] 		+= hm;
                }
            }

            /*//according to Altschuler & Newkirk for source surf at inf
            g[l][m] *= (2.0*l + 1) / (l + 1) * numerator;
            h[l][m] *= (2.0*l + 1) / (l + 1) * numerator;

            /*/

            // according to Xudong Sun:
            g[l][m] *= (2.0*l + 1) * numerator;
            h[l][m] *= (2.0*l + 1) * numerator;//*/
        }
}

bool PFSSsolution_SHC::importCoefficients(const char *filename, bool WSOfile, hcFloat r_ss)
{
    FILE* coeffFile;
    char str[1000];
    char tempstr[100];
    uint k,z,order;
    int highestOrder = 0;

	if (!doesFileExist(filename))
	{
		printf("ERROR! PFSSsolution_SHC::load input file '%s' does not exist\n", filename);
		init(0, 2.5*r_sol);
		return false;
	}

    coeffFile = fopen(filename, "r");

    uint i = 0;
    while(strcmp(fgets(str, 1000, coeffFile), "\n"))
    {
        while(str[i++] != ' ');
        strncpy(tempstr, str, --i);
        tempstr[i] = '\0';
        if (atoi(tempstr) > highestOrder)
            highestOrder = atoi(tempstr);
    }

    init((uint)highestOrder, r_ss);

    fseek(coeffFile,0, SEEK_SET);
    uint j=0;
    while (fgets(str, 1000, coeffFile) != NULL)
    {
        if(j==0)        // load g's
        {
            if(str[0] == '\n')
                ++j;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-z] = '\0';
                    g[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
        else            // load h's
        {
            if(str[0] == '\n')
                break;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    h[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }

                ++order;
            }
        }

    }
    fclose(coeffFile);

    if(WSOfile)				// WSO files employ microTesla as units
    {
    	for(uint l=0; l<=coeffOrder; ++l)
    		for(uint m=0; m<=l; ++m)
    		{
    			g[l][m] /= 100;
    			h[l][m] /= 100;
    		}
    }

    return true;
}

void PFSSsolution_SHC::exportCoefficients(const char *filename)
{
    char tempstr[10000];
    char tmp[100];

    FILE *outfile = fopen(filename, "w");
    for(uint l=0;l<=coeffOrder;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", g[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t g's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fprintf(outfile, "\n");
    for(uint l=0;l<=coeffOrder;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", h[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t h's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fclose(outfile);
}

//---------------------------------------------------------------------------------------------------------------
//---   PFSSsolution_SHC_sun
//---------------------------------------------------------------------------------------------------------------

PFSSsolution_SHC_sun::PFSSsolution_SHC_sun()
{
	initNULL();
}

PFSSsolution_SHC_sun::PFSSsolution_SHC_sun(const PFSSsolution_SHC_sun &other)
{
	initNULL();
	operator =(other);
}

PFSSsolution_SHC_sun::~PFSSsolution_SHC_sun()
{
	clear();
}

PFSSsolution_SHC_sun &PFSSsolution_SHC_sun::operator=(const PFSSsolution_SHC_sun &other)
{
	if(this == &other)
		return *this;

	init(other.coeffOrder, other.sourceSurfaceFactor*r_sol);

	for(uint l=0; l<=coeffOrder; ++l)
		for(uint m=0; m<=l; ++m)
		{
			g[l][m]	= other.g[l][m];
			h[l][m]	= other.h[l][m];
		}

	return *this;
}

PFSSsolution_SHC_sun PFSSsolution_SHC_sun::operator-(const PFSSsolution_SHC_sun &other)
{
	PFSSsolution_SHC_sun retval;

	if(this->coeffOrder != other.coeffOrder || this->sourceSurfaceFactor != other.sourceSurfaceFactor)
	{
		printf("PFSS_magfield::operator- order does not match (%u != %u) or r_ss is different.\n", this->coeffOrder, other.coeffOrder);
		retval.init(0, 2.5*r_sol);
	}
	else
	{
		retval.init(this->coeffOrder, this->sourceSurfaceFactor);
		for(uint l=0; l<=coeffOrder; ++l)
			for(uint m=0; m<=l; ++m)
			{
				hcFloat g1 		= fabs(this->g[l][m] - other.g[l][m]) / fabs(this->g[l][m]);
				hcFloat g2 		= fabs(this->g[l][m] - other.g[l][m]) / fabs(other.g[l][m]);
				hcFloat h1 		= fabs(this->h[l][m] - other.h[l][m]) / fabs(this->h[l][m]);
				hcFloat h2 		= fabs(this->h[l][m] - other.h[l][m]) / fabs(other.h[l][m]);

				retval.g[l][m] 	= this->g[l][m]==0 ? 0.0 : g1 < g2 ? g1 : g2;
				retval.h[l][m] 	= this->h[l][m]==0 ? 0.0 : h1 < h2 ? h1 : h2;
			}
	}

	return retval;

}

void PFSSsolution_SHC_sun::init(uint order, hcFloat r_ss)
{
	clear();
	coeffOrder 	= order;
	sourceSurfaceFactor = r_ss/r_sol;
	g 			= new hcFloat*[coeffOrder + 1];
	h 			= new hcFloat*[coeffOrder + 1];
	assLegPoly 	= new AssocLegendrePoly_sun*[coeffOrder + 1];

	for(uint i=0;i<=coeffOrder;++i)
	{
		g[i] 			= new hcFloat[i+1];
		h[i] 			= new hcFloat[i+1];
		assLegPoly[i] 	= new AssocLegendrePoly_sun[i+1];

		for(uint j=0;j<=i;++j)
		{
			AssocLegendrePoly_sun temp(i, j);
			assLegPoly[i][j] = temp;
		}
	}
}

void PFSSsolution_SHC_sun::eval(const Vec3D &pos, Vec3D &result)
{
	hcFloat r			= pos[0];
	hcFloat theta		= pos[1];
	hcFloat phi			= pos[2];

	hcFloat r_ss 		= sourceSurfaceFactor * r_sol;     // source surface
	hcFloat retval_r 	= 0.0;
	hcFloat retval_t 	= 0.0;
	hcFloat retval_p 	= 0.0;

	for(uint l=0;l<=coeffOrder;++l)
		for(uint m=0;m<=l;++m)
		{   // equation 16 from Notes on PFSS Extrapolation, Xudong Sun
			retval_r += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * pow(r_sol / r, (hcFloat)(l+2))
					  * (l+1+l*pow(r / r_ss, (hcFloat)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (hcFloat)(2*l+1)));

			// equation 17
			retval_t += -assLegPoly[l][m].getDeriv(theta)
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * pow(r_sol / r, (hcFloat)(l+2))
					  * (1 - pow(r / r_ss, (hcFloat)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (hcFloat)(2*l+1)));

			// equation 18
			retval_p += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * sin(m*phi) - h[l][m] * cos(m*phi) )
					  * pow(r_sol / r, (hcFloat)(l+2))
					  * (1 - pow(r / r_ss, (hcFloat)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (hcFloat)(2*l+1)));
		}

	result(0) = retval_r;
	result(1) = retval_t;
	result(2) = retval_p;
}

//---------------------------------------------------------------------------------------------------------------
//---   PFSSsolution_SHC_hoek
//---------------------------------------------------------------------------------------------------------------

PFSSsolution_SHC_hoek::PFSSsolution_SHC_hoek()
{
	initNULL();
}

PFSSsolution_SHC_hoek::PFSSsolution_SHC_hoek(const PFSSsolution_SHC_hoek &other)
{
	initNULL();
	operator =(other);
}

PFSSsolution_SHC_hoek::~PFSSsolution_SHC_hoek()
{
	clear();
}

PFSSsolution_SHC_hoek &PFSSsolution_SHC_hoek::operator=(const PFSSsolution_SHC_hoek &other)
{
	if(this == &other)
		return *this;

	printf("PFSSsolution_SHC: assignment operator not implemented!\n");

	return *this;
}

PFSSsolution_SHC_hoek PFSSsolution_SHC_hoek::operator-(const PFSSsolution_SHC_hoek &other)
{
	PFSSsolution_SHC_hoek retval;

	if(this->coeffOrder != other.coeffOrder)
	{
		printf("PFSS_magfield::operator- order does not match (%u != %u)\n", this->coeffOrder, other.coeffOrder);
		retval.init(0, 2.5*r_sol);
	}
	else
	{
		retval.init(this->coeffOrder, this->sourceSurfaceFactor);
		for(uint l=0; l<=coeffOrder; ++l)
			for(uint m=0; m<=l; ++m)
			{
				retval.g[l][m] = this->g[l][m]==0 ? 0.0 : (this->g[l][m] - other.g[l][m]) / this->g[l][m];
				retval.h[l][m] = this->h[l][m]==0 ? 0.0 : (this->h[l][m] - other.h[l][m]) / this->h[l][m];
			}
	}

	return retval;

}

void PFSSsolution_SHC_hoek::init(uint order, hcFloat r_ss)
{
	clear();
	coeffOrder 	= order;
	sourceSurfaceFactor = r_ss / r_sol;
	g 			= new hcFloat*[coeffOrder + 1];
	h 			= new hcFloat*[coeffOrder + 1];
	assLegPoly 	= new AssocLegendrePoly_sun*[coeffOrder + 1];

	for(uint i=0;i<=coeffOrder;++i)
	{
		g[i] 			= new hcFloat[i+1];
		h[i] 			= new hcFloat[i+1];
		assLegPoly[i] 	= new AssocLegendrePoly_sun[i+1];

		for(uint j=0;j<=i;++j)
		{
			AssocLegendrePoly_sun temp(i, j);
			assLegPoly[i][j] = temp;
		}
	}
}

void PFSSsolution_SHC_hoek::eval(const Vec3D &pos, Vec3D &result)
{
	hcFloat r			= pos[0];
	hcFloat theta		= pos[1];
	hcFloat phi			= pos[2];

	hcFloat r_ss 		= sourceSurfaceFactor * r_sol;     // source surface
	hcFloat retval_r 	= 0.0;
	hcFloat retval_t 	= 0.0;
	hcFloat retval_p 	= 0.0;

	for(uint l=0;l<=coeffOrder;++l)
		for(uint m=0;m<=l;++m)
		{
			hcFloat c	= -pow(r_sol / r_ss, (hcFloat)(l+2));

			retval_r += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * ((l+1) * pow(r_sol / r, (hcFloat)(l+2)) - c * l * pow(r / r_ss, (hcFloat)((int)l-1)));

			retval_t += -assLegPoly[l][m].getDeriv(theta)
					  * ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
					  * (pow(r_sol / r, (hcFloat)(l+2)) + c * pow(r / r_ss, (hcFloat)((int)l-1)));

			retval_p += assLegPoly[l][m](cos(theta))
					  * ( g[l][m] * sin(m*phi) - h[l][m] * cos(m*phi) )
					  * (pow(r_sol / r, (hcFloat)(l+2)) + c * pow(r / r_ss, (hcFloat)((int)l-1))) * m / sin(theta);
		}

	result(0) = retval_r;
	result(1) = retval_t;
	result(2) = retval_p;
}

//---------------------------------------------------------------------------------------------------------------
//---   CSSS
//---------------------------------------------------------------------------------------------------------------

CSSS_magfield::CSSS_magfield(const char *filename)
{
	initNULL();
    if(!load(filename))
    	printf("ERROR! CSSS_magfield constructor could not load file '%s'\n", filename);
}

CSSS_magfield::CSSS_magfield()
{
	initNULL();
    printf("ERROR! CSSS_magfield std constructor not implemented!\n");
}

CSSS_magfield::CSSS_magfield(const CSSS_magfield &other)
{
	initNULL();
	printf("ERROR! CSSS_magfield cpy constructor not implemented!\n");
}

CSSS_magfield::~CSSS_magfield()
{
    clear();
}

void CSSS_magfield::initNULL()
{
	g 					= NULL;
	h 					= NULL;
	assLegPoly 			= NULL;
	sourceSurfaceFactor = 2.5;
	order 			= 0;
}

void CSSS_magfield::clear()
{
	if(g != NULL)
	{
		for(uint i=0;i<=order;++i)
		{
			delete [] g[i];
			delete [] h[i];
			delete [] assLegPoly[i];
		}
		delete [] g;
		delete [] h;
		delete assLegPoly;
	}

	initNULL();
}

void CSSS_magfield::init(uint order)
{
	clear();

    this->order	= order;
    g 			= new double*[order + 1];
    h 			= new double*[order + 1];
    assLegPoly 	= new AssocLegendrePoly_sun*[order + 1];

    for(uint l=0;l<=order;++l)
    {
        g[l] 			= new double[l+1];
        h[l] 			= new double[l+1];
        assLegPoly[l] 	= new AssocLegendrePoly_sun[l+1];
        for(uint m=0;m<=l;++m)
        {
            AssocLegendrePoly_sun temp(l, m);
            assLegPoly[l][m] 	= temp;
            g[l][m]				= 0.0;
            h[l][m]				= 0.0;
        }
    }
}

void CSSS_magfield::exportCoefficients(const char *filename)
{
    char tempstr[10000];
    char tmp[100];
    FILE *outfile = fopen(filename, "w");
    for(uint l=0;l<=order;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", g[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t g's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fprintf(outfile, "\n");
    for(uint l=0;l<=order;++l)
    {
        tempstr[0] = '\0';
        sprintf(tempstr, "%u ", l);
        for(uint m=0;m<=l;++m)
        {
            tmp[0] = '\0';
            sprintf(tmp, "%f ", h[l][m]);
            strcat(tempstr, tmp);
        }
        if(l==0)
            strcat(tempstr, "\t\t h's");
        strcat(tempstr, "\n");
        fprintf(outfile, "%s", tempstr);
    }
    fclose(outfile);
}

/*
void CSSS_magfield::determineCoefficientsFromFITSsineLat(uint order, const char *FITS_filename,
		double maxSinLat, double minSinLat, bool accountForMissingFlux)
{

    uint x, y, l, m;
    initArrays(order);
    hcImageFITS fitsIMG(FITS_filename);
    uint width 	= fitsIMG.width;
    uint height = fitsIMG.height;

    double dTheta = PI / (2 * height);
    double dSinTheta = (maxSinLat - minSinLat) / (height-1);
    double dPhi = 2*PI/(width);

    // compute missing-flux-term
    double delta = 0.0;
    double tempB;
    double theta;
    double cosecans;
    double numerator = 0.0;

    for(y=0;y<height;++y)
    {
        tempB = 0.0;
        // this is for equally spaced theta
        //theta = (2.0*(height - 1 - y) + 1 ) * dTheta;
        // this is for equally spaced sin(latitude) -- theta = 180° - (lat + 90°)
        theta = PI / 2 - asin(maxSinLat - (height - 1- y) * dSinTheta);

        cosecans = 1.0 / sin(theta);
        numerator += cosecans;

        for(x=0;x<width;++x)
            if(!isnan(fitsIMG(x,y)) && !(fitsIMG(x,y) == -32768.0))
            {
                tempB += fitsIMG(x,y);
            }
        delta += tempB * cosecans;
    }

    delta /= (numerator * width);
    if(accountForMissingFlux == false)
        delta = 0;

    // now compute the coefficients
    numerator = 1.0 / (width * height);
    double legPolyEval;
    double phi;
    double D_xy;

    for(l=0;l<=coeffOrder;++l)
        for(m=0;m<=l;++m)
        {
            g[l][m] = 0.0;
            h[l][m] = 0.0;
            for(y=0;y<height;++y)
            {
                // this is for equally spaced theta
                //theta = (2*(height - 1 - y) + 1 ) * dTheta;
                //this is for equally spaced sin(latitude)
                theta 		= PI / 2 - asin(maxSinLat - (height - 1- y) * dSinTheta);

                legPolyEval = assLegPoly[l][m].operator ()(cos(theta));
                cosecans 	= 1 / sin(theta);

                for(x=0;x<width;++x)
                {
                    phi = m * x * dPhi;
                    if(!isnan(fitsIMG(x,y)) && !(fitsIMG(x,y) == -32768.0))
                        D_xy = fitsIMG(x,y) - delta;
                    else
                        D_xy = -delta;
                    g[l][m] += D_xy * cos(phi) * cosecans * legPolyEval;
                    h[l][m] += D_xy * sin(phi) * cosecans * legPolyEval;
                }
            }

            //according to Altschuler & Newkirk for source surf at inf
            //g[l][m] *= (2.0*l + 1) / (l + 1) * numerator;
            //h[l][m] *= (2.0*l + 1) / (l + 1) * numerator;

            //according to Xudong Sun:
            g[l][m] *= (2.0*l + 1) * numerator * 100;
            h[l][m] *= (2.0*l + 1) * numerator * 100;
        }
}//*/

bool CSSS_magfield::load(const char *filename)
{
	if(!doesFileExist(filename))
		return false;

	char line[1000];
	ifstream file(filename);

	uint order = 0;

	while (!file.eof())
	{
		file.getline(line, 1000);

		if((line[0] == '#') || line[0] == '\n')
			continue;

		regex re(".*([0-9]{3}).*", regex::icase);
		cmatch what;
		regex_search(line, what, re);

		if(what[0].matched)
		{
			order = boost::lexical_cast<int>(what[1]);
			break;
		}

		re.assign(".*([0-9]{2}).*", regex::icase);
		regex_search(line, what, re);

		if(what[0].matched)
		{
			order = boost::lexical_cast<int>(what[1]);
			break;
		}

		re.assign(".*([0-9]{1}).*", regex::icase);
		regex_search(line, what, re);

		if(what[0].matched)
		{
			order = boost::lexical_cast<int>(what[1]);
			break;
		}

		printf("ERROR! loadCSSScoeff: Order of solution could not be found!\n");
		return false;
	}

	init(order);

	char lstr[1000], mstr[1000], gstr[1000], hstr[1000];

	while(file >> lstr >> mstr >> gstr >> hstr)
	{
		uint l 		= atoi(lstr);
		uint m 		= atoi(mstr);
		double gval = atof(gstr);
		double hval	= atof(hstr);

		if((l>order) || (m>l))
		{
			printf("Error! loadCSSScoeff: l: %u, m: %u\n", l, m);
			return false;
		}

		g[l][m]	= gval;
		h[l][m]	= hval;
	}
	return true;
}

void CSSS_magfield::eval(const Vec3D &pos, Vec3D &result)
{
	hcFloat r			= pos[0];
	hcFloat theta		= pos[1];
	hcFloat phi			= pos[2];

    double r_ss 		= sourceSurfaceFactor * r_sol;     // source surface
    double retval_r 	= 0.0;
    double retval_t 	= 0.0;
    double retval_phi 	= 0.0;

    for(uint l=0;l<=order;++l)
        for(uint m=0;m<=l;++m)
        {   // equation 16 from Notes on PFSS Extrapolation, Xudong Sun
            retval_r += assLegPoly[l][m](cos(theta))
                    	* ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
                    	* pow(r_sol / r, (double)(l+2))
                    	* (l+1+l*pow(r / r_ss, (double)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (double)(2*l+1)));

            // equation 17 (there has to be theta instead of cos(theta) below!)
            retval_t += -assLegPoly[l][m].getDeriv(theta)
                    	* ( g[l][m] * cos(m*phi) + h[l][m] * sin(m*phi) )
                    	* pow(r_sol / r, (double)(l+2))
                    	* (1 - pow(r / r_ss, (double)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (double)(2*l+1)));

            // equation 18
            retval_phi += assLegPoly[l][m](cos(theta))
                    	  * ( g[l][m] * sin(m*phi) - h[l][m] * cos(m*phi) )
                    	  * pow(r_sol / r, (double)(l+2))
                    	  * (1 - pow(r / r_ss, (double)(2*l+1))) / (l+1+l*pow(r_sol / r_ss, (double)(2*l+1)));//*/

        }
    result(0) = retval_r;
    result(1) = retval_t;
    result(2) = retval_phi;
}

void CSSS_magfield::dump()
{
	printf("CSSS harmonic coefficients (up to order %u):\n----------------------------------------------------------\n", order);
	for(uint l=1; l<=order; ++l)
		for(uint m=0; m<=l; ++m)
			printf("l:\t%u, m:\t%u, g_lm:\t%E, h_lm:\t%E\n", m, l, g[l][m], h[l][m]);

}

//---------------------------------------------------------------------------------------------------------------
//---   PFSS_visualization
//---------------------------------------------------------------------------------------------------------------

#ifdef GUI

PFSS_visualization::PFSS_visualization(){

	drawScale			= 0.0;
	upperOrder 			= 0;
	lowerOrder			= 0;
	upperSS				= 0;
	lowerSS				= 0;
	textScreenID		= 0;
	ssAnimScreenID		= 0;
	contourProjScreenID	= 0;
	accumulatedTimediff = 0;
	animPointer			= 0;
	animationPlaying	= false;
	fps					= 0;
	numFrames			= 0;
	magLines			= NULL;
}

PFSS_visualization::PFSS_visualization(char *filename) : hcScene3D(){

    initFromCoeffFile(filename);
}

PFSS_visualization::~PFSS_visualization()
{
    clear();
}

void PFSS_visualization::clear()
{
    if(magLines != NULL)
    {
        delete [] magLines;
        magLines = NULL;
    }
}

void PFSS_visualization::init()
{
    clear();
    hcScene3D::init();
    magLines 				= NULL;
    accumulatedTimediff 	= 0.0;
    drawScale 				= 1 / (1.5 * r_sol);
    double ssSize 			= magfield.sourceSurfaceFactor * r_sol * drawScale;
    double sunSize			 = r_sol * drawScale;
    sun.appendFragment(*(new hcDSphere3D(origin3D, 1.0)));
    sources.appendFragment(*(new hcDSphere3D(origin3D, 1.0)));
    sources.TFmodel.loadIdentity();
    sources.TFmodel.scaleHom(ssSize);
    sun.TFmodel.loadIdentity();
    sun.TFmodel.scaleHom(sunSize);
    animationPlaying = false;
}

void PFSS_visualization::initFromSynopticFITSsineLat(uint order, const char *filename, double maxSinLat, double minSinLat)
{
    init();
    SynPhotMagfield synPhot;
    synPhot.load(filename);
    magfield.determineCoefficientsFromPhotMagfield(order, 2.5*r_sol, synPhot);

    sun.texContainer.rmTexture();
    sun.texContainer.addFITSTexFromFile(filename);

    sources.texContainer.rmTexture();
    sources.texContainer.addMonoTexture(char2RGBA8(255,255,255,50));

    initScene();

}

void PFSS_visualization::initFromSynopticWSO(uint order, const char *filename)
{
    std::ifstream in(filename);

    char instrings[31][1000];
    in.getline(instrings[0], 1000);
    in.getline(instrings[0], 1000);

    uint ntheta 		= 30;
    uint nphi 			= 73;
    //hcFloat *imageData 	= new hcFloat[ntheta * nphi];
    hcImageFITS img(nphi, ntheta);

    uint i;
    uint j=0;
    while(in >> instrings[0] >> instrings[1] >> instrings[2] >> instrings[3] >> instrings[4] >> instrings[5] >> instrings[6] >> instrings[7] >> instrings[8] >> instrings[9] >> instrings[10] >> instrings[11] >> instrings[12] >> instrings[13] >> instrings[14] >> instrings[15] >> instrings[16] >> instrings[17] >> instrings[18] >> instrings[19] >> instrings[20] >> instrings[21] >> instrings[22] >> instrings[23] >> instrings[24] >> instrings[25] >> instrings[26] >> instrings[27] >> instrings[28] >> instrings[29] >> instrings[30])
    {
        if(strncmp(instrings[0], "CTxxxxxxxx", 2))
        {
            printf("ERROR! PFSS_visualization::initFromSynopticWSO: Data alignment mismatch! Check the WSO-file!\n");
            printf("j: %u, instrings[0]: %s\n", j, instrings[0]);
            exit(1);
        }

        for(i=0;i<ntheta;++i)
        {
            img(nphi - j - 1, ntheta - i - 1) = atof(instrings[i+1]);
        }
        j++;
    }
    char outfilename[1000];
    outfilename[0] = '\0';
    strcat(outfilename, filename);
    strcat(outfilename, ".FITS\0");

    img.save(outfilename);
    //delete [] imageData;
    initFromSynopticFITSsineLat(order, outfilename, 14.5/15, -14.5/15);
}

void PFSS_visualization::initFromCoeffFile(const char *filename)
{
    init();
    magfield.importCoefficients(filename, true, 2.5*r_sol);

    sun.texContainer.rmTexture();
    sun.texContainer.addTexFromFile("../textures/spicules_sst_big.jpg");

    sources.texContainer.rmTexture();
    sources.texContainer.addMonoTexture(char2RGBA8(255,255,255,50));

    initScene();
}

//*/

/*! \brief compares two coefficient files
 *
 *  treats the second file as an deviation from the first and prints the
 *  relative errors for each coefficient in outfile
 */
void PFSS_visualization::compareCoeffFiles(const char *infile1, const char *infile2, const char *outfile)
{
    FILE *coeffFile1, *coeffFile2, *compFile;
    char str[1000];
    char tempstr[100];
    uint i,j,k,z,order;
    int highestOrder1 	= 0;
    int highestOrder2 	= 0;
    tempstr[0] 			= '\0';
    str[0] 				= '\0';

    coeffFile1 = fopen(infile1, "r");
    if(!coeffFile1)
    {
        printf("ERROR! PFSSsolution_SHC_sun::compareCoeffFiles: File %s does not exist!\n", infile1);
        return;
    }
    i=0;
    while(strcmp(fgets(str, 1000, coeffFile1), "\n"))
    {
        while(str[i++] != ' ');
        strncpy(tempstr, str, --i);
        tempstr[i] = '\0';
        if (atoi(tempstr) > highestOrder1)
            highestOrder1 = atoi(tempstr);
    }

    coeffFile2 = fopen(infile2, "r");
    if(!coeffFile2)
    {
        printf("ERROR! PFSSsolution_SHC_sun::compareCoeffFiles: File %s does not exist!\n", infile1);
        return;
    }
    i=0;
    while(strcmp(fgets(str, 1000, coeffFile2), "\n"))
    {
        while(str[i++] != ' ');
        strncpy(tempstr, str, --i);
        tempstr[i] = '\0';
        if (atoi(tempstr) > highestOrder2)
            highestOrder2 = atoi(tempstr);
    }

    if(highestOrder1 != highestOrder2)
    {
        printf("ERROR! PFSSsolution_SHC_sun::compareCoeffFiles: Coefficient order does not match! (%i != %i)\n", highestOrder1, highestOrder2);
        return;
    }

    fseek(coeffFile1,0, SEEK_SET);
    fseek(coeffFile2,0, SEEK_SET);

    double **g1 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    double **h1 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    double **g2 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    double **h2 = (double**)malloc(sizeof(double) * (highestOrder1+1));
    for(i=0;i<=highestOrder1;++i)
    {
        g1[i] = (double*) malloc(sizeof(double) * (i+1));
        h1[i] = (double*) malloc(sizeof(double) * (i+1));
        g2[i] = (double*) malloc(sizeof(double) * (i+1));
        h2[i] = (double*) malloc(sizeof(double) * (i+1));
    }

    j=0;
    while (fgets(str, 1000, coeffFile1) != NULL)
    {
        if(j==0)        // load g's
        {
            if(str[0] == '\n')
                ++j;
            else
            {
                i = 0;
                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    g1[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
        else            // load h's
        {
            if(str[0] == '\n')
                break;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    h1[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
    }

    j=0;
    while (fgets(str, 1000, coeffFile2) != NULL)
    {
        if(j==0)        // load g's
        {
            if(str[0] == '\n')
                ++j;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    g2[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
        else            // load h's
        {
            if(str[0] == '\n')
                break;
            else
            {
                i = 0;

                tempstr[0] = '\0';
                while(str[i++] != ' ');
                z = i;
                strncpy(tempstr, str, i-1);
                tempstr[i-1] = '\0';
                order=(uint)atoi(tempstr);
                k=0;

                while(k<=order)
                {
                    while(str[i] == ' ' || str[i] == '\t')
                        ++i;
                    z=i;
                    while(str[i] != ' ' && str[i++] != '\n');
                    strncpy(tempstr, str+z, i-1);
                    tempstr[i-1-z] = '\0';
                    h2[order][k++] = strtof(tempstr, NULL);
                    tempstr[0] = '\0';
                    z=i;
                }
                ++order;
            }
        }
    }

    fclose(coeffFile1);
    fclose(coeffFile2);
    compFile = fopen(outfile, "w");

    double error;
    double gAccum = 0.0;
    double hAccum = 0.0;
    for(i=0;i<=highestOrder1;++i)
    {
        fprintf(compFile, "%i ", i);
        for(j=0;j<=i;++j)
        {
            error = fabs((g2[i][j]-g1[i][j])/g1[i][j]);
            fprintf(compFile, "%f\t", error);
            if(!isnan((long double)error))
                gAccum += error;
        }
        fprintf(compFile, "\n");
    }

    fprintf(compFile, "\n");
    for(i=0;i<=highestOrder1;++i)
    {
        fprintf(compFile, "%i ", i);
        for(j=0;j<=i;++j)
        {
            error = fabs((h2[i][j]-h1[i][j])/h1[i][j]);
            fprintf(compFile, "%f\t", error);
            if(!isnan((long double)error))
                hAccum += error;
        }
        fprintf(compFile, "\n");
    }

    fprintf(compFile, "\nAccumulated error in g: %f\nAccumulated error in h: %f\n", gAccum, hAccum);
    fclose(compFile);

    for(i=0;i<=highestOrder1;++i)
    {
        free(g1[i]);
        free(h1[i]);
        free(g2[i]);
        free(h2[i]);
    }
    free(g1);
    free(h1);
    free(g2);
    free(h2);
}

void PFSS_visualization::initScene()
{
#define maxNumPointsPerMagLine 1000

    uint numTheta 	= 10;
    uint numPhi 	= 20;

    uint i,j,k;
    Vec3D temp;
    Vec3D pos;
    Vec3D temp_mag;
    Vec3D *posData;
    Vec3D gridPos;
    double theta, phi;

    appendObject(sun);

    magLines = new hcDObject3D[1];
    magLines[0].init();
    magLines[0].texContainer.init(3);
    magLines[0].texContainer.addMonoTexture(char2RGBA8(255, 0, 0, 255)); //downward maglines
    magLines[0].texContainer.addMonoTexture(char2RGBA8(0, 255, 0, 255)); //upward maglines
    magLines[0].texContainer.addMonoTexture(char2RGBA8(255, 255, 255, 30)); //source surface

    hcDLineStrip3D lineStrip;

    for(j=0;j<=numTheta;++j)
        for(k=0;k<numPhi;++k)
        {
            if((j==0 || j==numTheta) && k > 0) 			// compute poles only for k=0
                break;
            bool upwards;
            theta 				= j * PI / numTheta;	// TODO please review this
            phi 				= k * 2 * PI / numPhi;	// TODO please review this
            gridPos.content[0] 	= r_sol;
            gridPos.content[1] 	= theta;
            gridPos.content[2] 	= phi;

            posData 			= new Vec3D[maxNumPointsPerMagLine];

            for(i=0;i<maxNumPointsPerMagLine;++i)
            {
                if(i==0)
                {
                    magfield.eval(gridPos, temp_mag);

                    if(temp_mag.content[0] < 0)
                        upwards = false;
                    else
                        upwards = true;

                    pos = gridPos.convCoordSpher2Cart();

                    pos.scale(drawScale);
                    temp = temp_mag;
                    //temp_mag.convertSphericalToCartesian(temp);
                    temp_mag = temp_mag.convVecSpher2Cart(gridPos.convCoordSpher2Cart());
                }
                else
                {
                    if((pos.length() / drawScale < r_sol - 1E7) || (pos.length() / drawScale > magfield.sourceSurfaceFactor * r_sol))
                    {
                        lineStrip.init(i, posData);
                        lineStrip.drawOptions.computeLighting = false;

                        uint num = magLines[0].appendFragment(lineStrip);
                        if(!upwards)
                            magLines[0][num].texturePointer = 0;
                        else
                            magLines[0][num].texturePointer = 1;
                        break;
                    }

                    temp = pos;
                    temp.scale(1/drawScale);
                    temp = temp.convCoordCart2Spher();
                    magfield.eval(temp, temp_mag);
                }

                if(!upwards)
                {
                    //temp.loadZeroes();
                    //temp.minus(temp_mag);
                    temp = -temp_mag;
                    temp_mag = temp;
                }

                posData[i] = pos;
                temp_mag.scale(0.01 / temp_mag.length());
                //pos.plus(temp_mag);
                pos	+= temp_mag;
            }
            delete [] posData;
        }
    appendObject(magLines[0]);
    appendObject(sources);
}

int PFSS_visualization::loadAnimation(const char *path)
{
    uint i, j;
    numFrames 	= 1;
    lowerSS 	= 0;
    upperSS 	= 0;
    lowerOrder 	= 0;
    upperOrder 	= 0;

    char cfgFilename[1000];
    char inputFilename[1000];
    cfgFilename[0] = '\0';
    sprintf(cfgFilename, "%s%s", path, "config.cfg");
    std::ifstream config(cfgFilename);

    char option[1000], argument[1000];
    option[0] = '\0';
    argument[0] = '\0';

    while(config >> option >> argument)
    {
        if (!strcmp(option, "numFrames"))
            numFrames = atof(argument);
        if (!strcmp(option, "lowerSS"))
            lowerSS = atof(argument);
        if (!strcmp(option, "upperSS"))
            upperSS = atof(argument);
        if (!strcmp(option, "fps"))
            fps = atof(argument);
        if (!strcmp(option, "lowerOrder"))
            lowerOrder = (uint)atoi(argument);
        if (!strcmp(option, "upperOrder"))
            upperOrder = (uint)atoi(argument);
    }

    clear();
    hcScene3D::init();
    //appendElement(sun);
    magLines = new hcDObject3D[numFrames];

    for(i=0;i<numFrames;++i)
    {
        inputFilename[0] = '\0';
        sprintf(inputFilename, "%smaglines%u.obj", path, i);

        magLines[i].loadMesh(inputFilename);
        magLines[i].visible = false;
        appendObject(magLines[i]);

    }

    overlay.init();
    Vec2D ssMagScreenPos(0.38, 0.8);
    ssAnimScreenID = overlay.createAnimationScreen(ssMagScreenPos, 1.2, 0.3, numFrames);
    for(i=0;i<numFrames;++i)
    {
        inputFilename[0] = '\0';
        sprintf(inputFilename, "%sssmagfield%u.fits.contour.BMP", path, i);
        overlay[ssAnimScreenID].texContainer.addTexFromFile(inputFilename);
    }

    Fonthandler ftHandler;
    char fileNam[1000] = "../data/Roboto-Regular.ttf";
    ftHandler.init(fileNam, 20);

    uint reqWidth, reqHeight;
    float reqRelWidth, reqRelHeight;
    //uint *textlayer;

    float relHeight = 0.1;
    float relWidth = 0.5;
    uint pixelHeight = relHeight * heightPixelScale;
    uint pixelWidth = relWidth * widthPixelScale;

    Vec2D textScreenPos(-0.65, 0.9);
    textScreenID = overlay.createAnimationScreen(textScreenPos, relWidth, relHeight, numFrames);

    //textlayer = (uint*)malloc(sizeof(uint) * pixelHeight * pixelWidth);
    char text[100];

    hcImageRGBA textlayer(pixelWidth, pixelHeight);

    for(i=0;i<numFrames;++i)
    {
        text[0] = '\0';
        if(upperSS == 0)    // animation is concerned with order of spherical functions
            sprintf(text, "order: %u", lowerOrder+i);
        else
            sprintf(text, "R = %f", lowerSS + i * (float)(upperSS-lowerSS) / numFrames);

        j = 0;
        while(text[j++] != '\0');
        --j;

        ftHandler.getImgDim(text, 0, 0, reqWidth, reqHeight);
        reqRelHeight = reqHeight / heightPixelScale;
        reqRelWidth = reqWidth / heightPixelScale;
        if(reqRelHeight > relHeight)
            printf("ERROR: PFSS_visualization::loadAnimation(const char *path): Resolution of window too low for text rendering\n(height = %E, required: %E)!\n", relHeight, reqRelHeight);
        if(reqRelWidth > relWidth)
            printf("ERROR: PFSS_visualization::loadAnimation(const char *path): Resolution of window too low for text rendering\n(width = %e, required: %E)!\n", relWidth, reqRelWidth);

        ftHandler.printTextToImage(text, char2RGBA8(255, 255, 255, 255), 0, 0, pixelWidth, pixelHeight, textlayer);
        overlay[textScreenID].texContainer.addTexture2D(textlayer, false);

    }//*/
    //free(textlayer);

    Vec2D contourPosProj(0.38, 0.5);
    contourProjScreenID = overlay.createAnimationScreen(contourPosProj, 1.2, 0.3, numFrames);
    for(i=0;i<numFrames;++i)
    {
        inputFilename[0] = '\0';
        sprintf(inputFilename, "%smagfield25%u.fits.contour.BMP", path, i);
        overlay[contourProjScreenID].texContainer.addTexFromFile(inputFilename);
    }

    return 1;
}

#define outputwidth 800
#define outputheight 200

void PFSS_visualization::exportRadialMagfieldAtHeight(float r, const char *filename)
{
	hcFloat dPhi 		= 2 * PI / outputwidth;
    hcFloat dTheta 		= PI / (outputheight-1);
    //hcFloat *imageData 	= new hcFloat[outputwidth * outputheight];

    hcImageFITS img(outputwidth, outputheight);

    Vec3D pos, mag;
    pos.content[0] = r;

    for(uint x=0;x<outputwidth;++x)
        for(uint y=0;y<outputheight;++y)
        {
            pos.content[1] = PI - dTheta * y;	// TODO review this
            pos.content[2] = dPhi * x;			// TODO review this
            magfield.eval(pos, mag);
            //imageData[y * outputwidth + x] = mag[0];
            img(x, y) = mag[0];
        }

    img.save(filename);

    //delete [] imageData;
}

void PFSS_visualization::exportRadialProjectedMagfieldAtHeight(float r, const char *filename)
{
    uint x,y;
    hcFloat dPhi 		= 2 * PI / outputwidth;
    hcFloat dTheta 		= PI / (outputheight-1);
    //hcFloat *imageData 	= new hcFloat[outputwidth * outputheight];
    hcImageFITS img(outputwidth, outputheight);

    Vec3D pos, mag;
    pos.content[0] = magfield.sourceSurfaceFactor * r_sol;
    hcFloat distance = r - pos[0];

    for(x=0;x<outputwidth;++x)
        for(y=0;y<outputheight;++y)
        {
            pos.content[1] = PI - dTheta * y;	// TODO review this
            pos.content[2] = dPhi * x;			// TODO review this
            magfield.eval(pos, mag);
            img(x,y) = mag[0] * pow(pos[0]/r, 2);
        }

    img.save(filename);

    //delete [] imageData;
}

void PFSS_visualization::draw(GLuint programID)
{
    hcScene3D::draw(programID);
    overlay.draw();
}
#endif


#ifndef TODA_ACTION_VARIABLES_GK_HPP
#define TODA_ACTION_VARIABLES_GK_HPP

#include <cmath>
#include <vector>
#include <clstatphys/physics/toda_discriminant.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>

using namespace boost::math::quadrature;

void TodaActionVariablesGK(std::vector<double> & action_variables,
                         integrable::TodaDiscriminant const & discriminant,
                         int max_depth=5,double relative_error=1e-5){

  int matrix_size = discriminant.matrix_size();
  std::vector<double> roots(2*matrix_size);
  discriminant.total_roots(roots);

  auto func = [&discriminant](double x){
    if(discriminant(x) > 2) return std::acosh(0.5*discriminant(x));
    else if(discriminant(x) < -2) return std::acosh(-0.5*discriminant(x));
    else return 0.0;
  };

  for(int i = 0; i < matrix_size - 1; ++i){
    double error;
    if(roots[2*i+1] - roots[2*i+2] == 0)
      action_variables[i] = 0.0;
    else
      action_variables[i] = 2.0 / M_PI
                          * gauss_kronrod<double, 15>::integrate(
                                                               func,
                                                               roots[2*i+1],
                                                               roots[2*i+2],
                                                               max_depth,
                                                               relative_error,
                                                               &error
                                                               );
  }
}

#endif


/*
source file to apply LinearModels header file
*/

#include <iostream>
#include <armadillo>
#include <limits>
#include "LinearModels.hpp"
using namespace std;
using namespace arma;

// 1. Linear Regression
//	constructors and destructors
LinearRegression::LinearRegression() : fit_intercept(true) {};
LinearRegression::LinearRegression(const LinearRegression& source) : fit_intercept(source.fit_intercept), theta(source.theta) {};
LinearRegression::LinearRegression(bool fit_intercept1) : fit_intercept(fit_intercept1) {};
LinearRegression::~LinearRegression() {};

//	assignment operation
LinearRegression& LinearRegression::operator=(const LinearRegression& source) {
	if (this == &source) {
		cout << "self-assignment checked";
	}
	else {
		fit_intercept = source.fit_intercept;
		theta = source.theta;
	}
	return *this;
}

//	functions
// fit function to calculate the results and store to theta
const void LinearRegression::fit(mat X, const mat& y) {
	//	add an addtional column if fit intercept
	if (fit_intercept == true) X = join_rows(ones(X.n_rows), X);
	//	ipseduo inverse part of the equation
	mat ipseudo_inverse = (X.t() * X).i() * X.t();

	theta = ipseudo_inverse * y;		//	save results
	return;
}

// return predicted values with trained model
const mat LinearRegression::predict(mat _X) {
	//	_X is a test matrix with shape of (M, Z), pred_y will be the product of _X and theta
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);

	mat y_pred = _X * theta;
	return y_pred;
}

//	2. Ridge Regression
RidgeRegression::RidgeRegression() : fit_intercept(true), lambda(1.0) {};
RidgeRegression::RidgeRegression(const RidgeRegression& source) : fit_intercept(source.fit_intercept), theta(source.theta), lambda(source.lambda) {};
RidgeRegression::RidgeRegression(double lambda1, bool fit_intercept1) : fit_intercept(fit_intercept1), lambda(lambda1) {};
RidgeRegression::~RidgeRegression() {};

//	assignment operation
RidgeRegression& RidgeRegression::operator=(const RidgeRegression& source) {
	if (this == &source) {
		cout << "self-assignment checked";
	}
	else {
		fit_intercept = source.fit_intercept;
		lambda = source.lambda;
		theta = source.theta;
	}
	return *this;
}

//	functions
// fit function to calculate the results and store to theta
const void RidgeRegression::fit(mat X, const mat& y) {
	//	add an addtional column if fit intercept
	if (fit_intercept == true) X = join_rows(ones(X.n_rows), X);

	//	matrix for alpha

	mat A = lambda * eye(X.n_cols, X.n_cols);
	//	ipseduo inverse part of the equation
	mat ipseudo_inverse = (X.t() * X + A).i() * X.t();

	theta = ipseudo_inverse * y;		//	save results
	return;
}

// return predicted values with trained model
const mat RidgeRegression::predict(mat _X) {
	//	_X is a test matrix with shape of (M, Z), pred_y will be the product of _X and theta
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);
	mat y_pred = _X * theta;

	return y_pred;
}

// 3. Logistic Regression
//	constructors and destructors
LogisticRegression::LogisticRegression() : fit_intercept(true), lambda(0.0), penalty("l2") {};
LogisticRegression::LogisticRegression(const LogisticRegression& source) : fit_intercept(source.fit_intercept), theta(source.theta), lambda(source.lambda), penalty(source.penalty) {};
LogisticRegression::LogisticRegression(double lambda1, bool fit_intercept1, string penalty1) : fit_intercept(fit_intercept1), penalty(penalty1), lambda(lambda1) {};
LogisticRegression::~LogisticRegression() {};

//	assignment operation
LogisticRegression& LogisticRegression::operator=(const LogisticRegression& source) {
	if (this == &source) {
		cout << "self-assignment checked";
	}
	else {
		fit_intercept = source.fit_intercept;
		lambda = source.lambda;
		penalty = source.penalty;
		theta = source.theta;
	}
	return *this;
}

//	functions
// fit function to calculate the results and store to theta
const void LogisticRegression::fit(mat _X, const mat& y, double lr, double tol, long max_iter) {
	//	whether to fit an intercept
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);
	//	previous loss value to store loss
	double loss, l_prev = 999999.0;
	theta = vec(_X.n_cols, fill::randn);

	//	set loss and y_pred
	//double loss;
	mat y_pred;

	for (long i = 0; i < max_iter; i++) {
		y_pred = sigmoid(_X * theta);
		loss = _NLL(_X, y, y_pred);
		//	stop if change is less than tol
		if (l_prev - loss < tol) return;
		l_prev = loss;
		theta = theta - lr * _NLL_grad(_X, y, y_pred);
	}
}

// supplemental function to calculate negative log likelihood under current model
double LogisticRegression::_NLL(const mat& X, const mat& y, const mat& y_pred) {
	//	for X with M rows (number of examples), and N cols (number of features)
	auto M = X.n_rows, N = X.n_cols;
	int order; // type of penalty depending on value of penalty
	if (penalty == "l2") order = 2;
	else order = 1;

	double norm_theta = arma::norm(theta, order);

	double nll, penalty_val;
	nll = sum(-arma::log(y_pred.elem(find(y == 1)))) - sum(log(1 - y_pred.elem(find(y == 0))));

	if (order == 2) penalty_val = (lambda / 2) * pow(norm_theta, 2);
	else penalty_val = lambda * norm_theta;

	return (nll + penalty_val) / double(M);
}

// supplemental function to calculate Gradient of the penalized negative log likelihood wrt theta
vec LogisticRegression::_NLL_grad(const mat& X, const mat& y, const mat& y_pred) {
	//	for X with N rows (number of examples), and M cols (number of features)
	auto M = X.n_rows, N = X.n_cols;

	// calculate delta penalty
	vec d_penalty;
	if (penalty == "l2") d_penalty = lambda * theta;
	else d_penalty = lambda * sign(theta);

	return (((y - y_pred).t() * X).t() + d_penalty) / -double(M);
}

// return predicted values with trained model
const mat LogisticRegression::predict(mat _X) {
	//	_X is a test matrix with shape of (M, Z), pred_y will be the product of _X and theta
	if (fit_intercept == true) _X = join_rows(ones(_X.n_rows), _X);
	mat y_pred = _X * theta;

	return round(sigmoid(y_pred));
}

//  supplemental functions

//The logistic sigmoid function
mat sigmoid(const mat& X) {
	return 1 / (1 + exp(-X));
};

Machine Learning Algos/Machine Learning Algos/LinearModels/LinearModels.cpp


#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>
#include <Eigen/OrderingMethods>
#include <Eigen/SparseQR>
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <vector>

// declares a column-major sparse matrix type of double
typedef Eigen::SparseMatrix<float> SparseMatrix;
typedef Eigen::Triplet<float> Triplet;

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMajorMatrixXf;

const float GRID_DX = 0.1;
const float GRID_DY = 1;
const float MIN_X = 0;
const float MAX_X = 359.9999;
const float MIN_Y = 30;
const float MAX_Y = 150;

template <typename T>
T clamp(const T& v, const T& min_v, const T& max_v)
{
    return v < min_v ? min_v : (v > max_v ? max_v : v);
}

void saveAsBitmap(const Eigen::VectorXf& x, size_t rows, size_t cols, const char* fname)
{
    std::ofstream ofs(fname);
    ofs << "P2\n" << cols << " " << rows << "\n255\n";
    for(size_t rr = 0; rr < rows; rr++) {
        for(size_t cc = 0; cc < cols; cc++) {
            ofs << clamp<int>((int)x[rr * cols + cc], 0, 255) << " ";
        }
        ofs << "\n";
    }
}

inline
int wrap(int v, int min_v, int max_v)
{
    int width = max_v - min_v + 1;
    while(v < min_v) v += width;
    while(v > max_v) v -= width;
    return v;
}

inline
float sqr(float x)
{
    return x*x;
}

inline
float cube(float x)
{
    return x*x*x;
}

inline
float bspline(float x)
{
    if(x < -2) {
        return 0;
    } else if(x < -1) {
        return (cube(x+2)/6.0);
    } else if(x < 0) {
        return (-3*cube(x+2-1)+3*sqr(x+2-1)+3*(x+2-1)+1)/6;
    } else if(x < 1) {
        return ((3*cube(x+2-2) - 6*sqr(x+2-2) + 4)/6);
    } else if(x < 2) {
        return cube(1 - (x + 2 - 3))/6.0;
    } else {
        return 0;
    }
}

float kernel(float x, float width=4)
{
    // bspline width is 4
    width /= 4;
    return bspline(x / width ) / width;
}

inline
size_t grid_index(size_t width, size_t height, size_t grid_x, size_t grid_y)
{
    return grid_y * width + grid_x;
}

struct BSpline
{
    float grid_dx = GRID_DX;
    float grid_dy = GRID_DY;
    float grid_min_x = MIN_Y;
    float grid_max_x = MAX_X;
    float grid_min_y = MIN_Y;
    float grid_max_y = MAX_Y;

    size_t x_knots = (grid_max_x - grid_min_x) / grid_dx;
    size_t y_knots = (grid_max_y - grid_min_y) / grid_dy;
    size_t n_knots = x_knots * y_knots;

    SparseMatrix getWeights(const Eigen::MatrixXf& points) const
    {
        assert(points.cols() >= 2);
        std::vector<Triplet> coeffs;
        coeffs.reserve(points.size()*5);

        for(size_t pp = 0; pp < points.rows(); pp++) {
            const auto& pt = points.row(pp);
            float c_xbin = x_knots * (pt.x() - grid_min_x)/(grid_max_x - grid_min_x);
            float c_ybin = y_knots * (pt.y() - grid_min_y)/(grid_max_y - grid_min_y);

            // add weights to adjacent knots
            for(int ii = -2; ii <= 2; ii++) {
                for(int jj = -2; jj <= 2; jj++) {
                    int raw_xbin = (int)c_xbin + ii;
                    int wrapped_xbin = wrap(raw_xbin, 0, x_knots - 1);

                    int ybin = (int)c_ybin + jj;
                    if(ybin < 0 || ybin >= y_knots)
                        continue;

                    float w = kernel(c_xbin - raw_xbin)*kernel(c_ybin - ybin);
                    size_t grid_ind = grid_index(x_knots, y_knots, wrapped_xbin, ybin);
                    coeffs.push_back(Triplet(pp, grid_ind, w));
                }
            }
        }

        size_t num_points = points.rows();
        SparseMatrix Bmat(num_points, n_knots);
        Bmat.setFromTriplets(coeffs.begin(), coeffs.end());
        return Bmat;
    }

};

Eigen::VectorXf solve(const BSpline& spl, const Eigen::MatrixXf& points)
{
    std::cerr << "Filling Weights" << std::endl;
    auto Bmat = spl.getWeights(points);
    Bmat.makeCompressed();
    std::cerr << "Done" << std::endl;

    //Eigen::SparseQR<SparseMatrix, Eigen::NaturalOrdering<int>> qr;
    std::cerr << "QR" << std::endl;
    Eigen::SparseQR<SparseMatrix, Eigen::COLAMDOrdering<int>> qr(Bmat);
    std::cerr << "Done" << std::endl;
    //Eigen::SPQR<SparseMatrix> qr;
    //qr.compute(Bmat);
    //Eigen::SparseLU<SparseMatrix> qr(Bmat);
    std::cerr << "Solving: "<< std::endl;
    Eigen::VectorXf sol = qr.solve(points.col(2));
    std::cerr << "Done" << std::endl;
    return sol;
};

Eigen::VectorXf interpolate(const BSpline& spl,
        const Eigen::VectorXf& knot_values, const Eigen::MatrixXf& points)
{
    auto bmat = spl.getWeights(points);
    return bmat * knot_values;
}

void drawPoints(const BSpline& spl, const Eigen::MatrixXf& points, const char* fname)
{
    size_t x_bins = spl.x_knots;
    size_t y_bins = spl.y_knots;
    Eigen::VectorXf raster(x_bins * y_bins);
    raster.fill(0);
    for(size_t rr = 0; rr < points.rows(); rr++) {
        float x = points(rr, 0);
        float y = points(rr, 1);
        float c_xbin = x_bins * (x - spl.grid_min_x) / (spl.grid_max_x - spl.grid_min_x);
        float c_ybin = y_bins * (y - spl.grid_min_y) / (spl.grid_max_y - spl.grid_min_y);
        raster[(int)c_xbin + (int)c_ybin * x_bins] = 255;
    }
    saveAsBitmap(raster, y_bins, x_bins, fname);
}

int main(int argc, char** argv)
{
    size_t num_points = 30;
    Eigen::MatrixXf points = Eigen::MatrixXf::Random(num_points, 3); // x, y, value
    for(size_t rr = 0; rr < points.rows(); rr++) {
        points(rr, 0) = MIN_X + std::abs(points(rr, 0)) * (MAX_X - MIN_X);
        points(rr, 1) = MIN_Y + std::abs(points(rr, 1)) * (MAX_Y - MIN_Y);
        points(rr, 2) = 1;
    }

    BSpline spl;
    drawPoints(spl, points, "orig.pgm");

    // Matrix maps from knots to known points
    // v = Bx
    //
    // x is the value of the knots, B is the weights of values at each knot s.t.
    // the values reflect the positions of the input points and v is the value
    // at the input points
    Eigen::VectorXf knots = solve(spl, points);

    // Use the known knots to solve for the actual values at each knot location
    Eigen::MatrixXf grid_points(spl.x_knots*spl.y_knots, 2);
    for(size_t yy = 0; yy < spl.y_knots; yy++) {
        for(size_t xx = 0; xx < spl.x_knots; xx++) {
            grid_points(yy * spl.x_knots + xx, 0) = spl.grid_min_x + xx * spl.grid_dx;
            grid_points(yy * spl.x_knots + xx, 1) = spl.grid_min_y + yy * spl.grid_dy;
        }
    }

    Eigen::VectorXf interpolated = interpolate(spl, knots, grid_points);
    //Eigen::VectorXf interpolated = interpolate(spl, knots, points);

    saveAsBitmap(255*interpolated, spl.y_knots, spl.x_knots, "smoothed.pgm");
    return 0;
}


#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <math.h>

int main(int argc, char **argv)
{

    {
        Eigen::Matrix3d m = Eigen::Matrix3d::Identity(); //生成单位矩阵
        std::cout << "m^T:" << std::endl
                  << m.transpose() << std::endl; // transpose() 转置后输出

        Eigen::Vector3d v1(3, 0, 0);
        Eigen::Vector3d v2(0, 4, 0);
        Eigen::Vector3d v3(0, 0, 5);
        std::cout << "v1^T:" << std::endl
                  << v1 << std::endl
                  << "v2^T:" << std::endl
                  << v2.transpose() << std::endl
                  << "v3^T:" << std::endl
                  << v3.transpose() << std::endl;

        // m.block<1, 3>(0, 0)
        // 分块每块大小<1行，3列>
        // 取出（第0行，第0列）个块
        m.block<1, 3>(0, 0) = v1.transpose();
        m.block<1, 3>(1, 0) = v2.transpose();
        m.block<1, 3>(2, 0) = v3.transpose();

        std::cout << "m:" << std::endl
                  << m << std::endl;

        Eigen::Vector3d VecAll1;
        VecAll1.fill(1); //向量各个值全部填充为1
        std::cout << "VecAll1^T:" << std::endl
                  << VecAll1.transpose() << std::endl;

        Eigen::Vector3d normal = m.inverse() * VecAll1;
        double NormalLen = normal.norm(); //向量的模
        normal = normal / NormalLen;      //v1 v2 v3 构成平面的单位法向量
        double d_plane_o = 1 / NormalLen; //平面到原点的距离

        std::cout << "normal^T:" << std::endl
                  << normal.transpose() << std::endl;
        std::cout << d_plane_o << std::endl;
    }
    return 0;
}