#include "forward_kinematics.h"
#include <iostream>
#include <stdio.h>
#include <Eigen/Dense>
#include <unordered_map>
#include <bitset>
#include <string>
#include <utility>
//perform the forward kinematics of the robot to figure out where you are
wsState* fk (configState* c)
{
  double theta0 = tick_to_radians(c->theta[0]);
  double theta1 = tick_to_radians(c->theta[1]);
  double theta2 = tick_to_radians(c->theta[2]);
  double theta3 = tick_to_radians(c->theta[3]);
  double theta4 = tick_to_radians(c->theta[4]);
  /*  Eigen::Matrix4d T_01 = DH(M_PI/2 , 0, 26.5, theta0);
  Eigen::Matrix4d T_12 = DH(0, 150, 0 , M_PI/2+theta1);
  Eigen::Matrix4d T_23 = DH(0, 150, 0, theta2);
  Eigen::Matrix4d T_34 = DH(-M_PI/2, 0, 0, M_PI/2+theta3);
  Eigen::Matrix4d T_45 = DH(0,0,116.525, theta4);

  Eigen::Matrix4d T_05 = T_01*T_12*T_23*T_34*T_45;

  wsState* outstate = new wsState;
  outstate->x = T_05(0,3);
  outstate->y = T_05(1,3);
  outstate->z = T_05(2,3);
  outstate->alpha = tick_to_radians(c->theta[0]+c->theta[1]+
      c->theta[2]+c->theta[3]+c->theta[4]);
  //TODO calculate angles based on rotation matrix
  */
  double c0 = cos(theta0);
  double s0 = sin(theta0);
  double c1 = cos(theta1);
  double s1 = sin(theta1);
  double c12 = cos(theta1+theta2);
  double s12 = sin(theta1+theta2);
  double c123 = cos(theta1+theta2+theta3);
  double s123 = sin(theta1+theta2+theta3);

  wsState* outstate = new wsState;
  outstate->x = c0*(150*(c1+c12) + 116.525*s123);
  outstate->y = s0*(150*(c1+c12) + 116.525*s123);
  outstate->z = 150*(s12+s1) - 116.525*c123 + 26.5;
  outstate->alpha = theta1+ theta2+ theta3;

  return outstate;

}//end forward_kinematics

Eigen::Matrix4d DH(double alpha,double a,double d,double theta)
{
  Eigen::Matrix4d T;
  T << cos(theta),sin(theta)*cos(alpha),-sin(theta)*sin(alpha),a*cos(theta),
    sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha),a*sin(theta),
    0,sin(alpha),cos(alpha),d,
    0,0,0,1;
  return T;
}

//calculate Euclidean ndistance between two states
double distance(wsState* s1,wsState* s2)
{
  return (pow(s1->x - s2->x,2)+pow(s1->y-s2->y,2)+pow(s1->z-s2->z,2)+pow(s1->alpha-s2->alpha,2));
}

configState* clone_configstate(configState* c){

  configState* clone = new configState;
  clone->theta[0] = c->theta[0];
  clone->theta[1] = c->theta[1];
  clone->theta[2] = c->theta[2];
  clone->theta[3] = c->theta[3];
  clone->theta[4] = c->theta[4];
  return clone;
}

double tick_to_radians(int i){
  return (M_PI/100)*i;
}

wsState* create_wsstate(){
   return (wsState*)malloc(sizeof(struct wsState));
}

configState* create_configstate(){
  return (configState*)malloc(sizeof(struct configState));
}

visData* create_visdata(){
  return (visData*)malloc(sizeof(struct visData));
}

void deallocate_wsstate(wsState* w){
  if(w != 0)
    free(w);
}

void deallocate_configstate(configState* c){
  if(c != 0)
    free(c);
}

void deallocate_visdata(visData* v){
  if(v != 0)
    free(v);
}

void wsstate_tostring(wsState* w){
    printf("Work Space State: x:%f, y:%f, z:%f\n",
      w->x, w->y, w->z);
}

void configstate_tostring(configState* c){
  printf("Theta1: %i, Theta2: %i, Theta3: %i, Theta4: %i, Theta5: %i\n"
      , c->theta[0], c->theta[1],c->theta[2], c->theta[3], c->theta[4]);
}


//Neural network in C++
// Author: Sethu Iyer
//Sigmoid is used because the sigmoid function assumes
//minimal structure and reflects our general state of ignorance about the underlying model.

//This code implements simple 3 layer feed forward neural network with no regularization.
// we use fast sigmoid approximation to speed up the calculation.

#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

class Neural_Network
{
   private:
    //define the architecture of the network
    int inputLayerSize = 2;
    int  outputLayerSize = 1;
    int hiddenLayerSize = 3;

    // activation function and the weights
    arma::mat W1,W2;
    arma::mat z2,a2;
    arma::mat z3,a3;
    arma::mat delta3,delta2;
    arma::mat dJdW1, djdW2;

  public:
       Neural_Network()
     {
          //fills in random weights
        W1.randu(2,3);
        W2.randu(3,1);
    }

      //helper functions
        static double sigmoid(double x)
    {
      return x / (1 + abs(x));
    }
    static double sigmoidPrime(double x)
    {
      double f = sigmoid(x);
      return f * (1-f);
    }

    //forwarding function.
    arma::mat forward(arma::mat X)
    {
        z2 = X * W1;
        a2 = z2;
        a2.transform(*sigmoid);
        z3 = a2 * W2;
        a3 = z3;
        a3.transform(*sigmoid);
        return a3;
    }

    //cost function
    double costFunction(mat X,mat y)
    {
      mat yHat = forward(X);

      double cost = 0;
      for(int i = 0; i < outputLayerSize; i++)
      {
        cost =  cost + (yHat[i] - y[i])*(yHat[i] - y[i]);
      }
      cost = cost / 2;
      return cost;

    }

    //derivative of cost function
    //backpropogation of errors can be understood in this code
    void computeGradients(mat X, mat y)
    {

      mat yHat = forward(X);
      mat z = z3;
      z.transform(*sigmoidPrime);
      delta3 = (yHat - y.t()) % z;
      djdW2 = a2.t() * delta3;

      z = z2;
      z.transform(*sigmoidPrime);
      delta2 = delta3 * W2.t()*z;
      dJdW1 = X.t() * delta2;

    }

    //alpha is our learning rate
    void gradientDescent(mat X,mat y,double alpha,int numIter)
    {
      int i;
      mat transdj1,transdj2;
      for(i=0;i<numIter;i++)
      {
        computeGradients(X,y);
        transdj1 = dJdW1;
        transdj1.transform([=] (double x) { return x * alpha;});
        W1 = W1 - transdj1;
    transdj2 = djdW2;
        transdj2.transform([=] (double x ) { return x *alpha;});
        W2 = W2 - transdj2;
      }
    }
};

int main()
{

    mat X = {{0.3,1},{0.5,0.2},{1,0.4}};
    mat y = {0.75,0.82,0.93};
    Neural_Network nn;
    cout<<"Initial Cost: "<<nn.costFunction(X,y)<<endl;
    nn.gradientDescent(X,y,0.01,400);
    cout<<"After 400 Iterations: "<<nn.costFunction(X,y)<<endl;
    nn.gradientDescent(X,y,0.01,400);
    cout<<"After 800 Iterations: "<<nn.costFunction(X,y)<<endl;
    return 0;
}

3-Layer Neural Network in C++/neuralnet.cpp


//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            // start your code here
            double error = 0 ; // 第i个数据点的计算误差
            // 填写计算error的表达式
            error = yi- exp(ae * xi * xi + be * xi + ce);
            Vector3d J; // 雅可比矩阵
            J[0] = -exp(ae * xi * xi + be * xi + ce)*xi*xi;  // de/da
            J[1] = -exp(ae * xi * xi + be * xi + ce)*xi;  // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            // end your code here

            cost += error * error;
        }

        // 求解线性方程 Hx=b，建议用ldlt
 	// start your code here
        Vector3d dx;
        dx = H.ldlt().solve(b);
	// end your code here

        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}


#include <armadillo>
#include <cstring>
#include <iostream>

using namespace std;
using namespace arma;

/*  ----------------- useful help functions --------------  */

struct MyVec
{
    double *data;
    size_t n;
};

struct MyMat
{
    double *data;
    size_t num_rows, num_cols;
};

extern "C" void freeMyVec(MyVec &vec)
{
    delete[] vec.data;
    memset((void *)&vec, 0, sizeof(vec));
}

extern "C" void freeMyMat(MyMat &mat)
{
    delete[] mat.data;
    memset((void *)&mat, 0, sizeof(mat));
}

struct HuberCovResult
{
    MyVec means;
    MyMat cov;
};

extern "C" void freeHuberCovResult(const HuberCovResult &result)
{
    delete[] result.means.data;
    delete[] result.cov.data;
    memset((void *)&result, 0, sizeof(result));
}

              /* --------------------------------- */
/*------------ Tuning-Free Huber Regression Funtions ---------------*/
              /* --------------------------------- */

int sgn(const double x)
{
    return (x > 0) - (x < 0);
}

double f1(const double x, const arma::vec &resSq, const int n, const double rhs)
{
    return arma::mean(arma::min(resSq / x, arma::ones(n))) - rhs;
}

double rootf1(const arma::vec &resSq, const int n, const double rhs, double low, double up, const double tol = 0.001, const int maxIte = 500)
{
    int ite = 1;
    while (ite <= maxIte && up - low > tol)
    {
        double mid = 0.5 * (up + low);
        double val = f1(mid, resSq, n, rhs);
        if (val < 0)
        {
            up = mid;
        }
        else
        {
            low = mid;
        }
        ite++;
    }
    return 0.5 * (low + up);
}

double f2(const double x, const arma::vec &resSq, const int N, const double rhs)
{
    return arma::mean(arma::min(resSq / x, arma::ones(N))) - rhs;
}

double rootf2(const arma::vec &resSq, const int n, const int d, const int N, const double rhs, double low, double up, const double tol = 0.001,
              const int maxIte = 500)
{
    int ite = 0;
    while (ite <= maxIte && up - low > tol)
    {
        double mid = 0.5 * (up + low);
        double val = f2(mid, resSq, N, rhs);
        if (val < 0)
        {
            up = mid;
        }
        else
        {
            low = mid;
        }
        ite++;
    }
    return 0.5 * (low + up);
}

double g1(const double x, const arma::vec &resSq, const int n, const double rhs)
{
    return arma::mean(arma::min(resSq / x, arma::ones(n))) - rhs;
}

double rootg1(const arma::vec &resSq, const int n, const double rhs, double low, double up, const double tol = 0.001, const int maxIte = 500)
{
    int ite = 0;
    while (ite <= maxIte && up - low > tol)
    {
        double mid = 0.5 * (up + low);
        double val = g1(mid, resSq, n, rhs);
        if (val < 0)
        {
            up = mid;
        }
        else
        {
            low = mid;
        }
        ite++;
    }
    return 0.5 * (low + up);
}

double huberDer(const arma::vec &res, const double tau, const int n)
{
    double rst = 0.0;
    for (int i = 0; i < n; i++)
    {
        double cur = res(i);
        rst -= std::abs(cur) <= tau ? cur : tau * sgn(cur);
    }
    return rst / n;
}

extern "C" double huberMean(MyVec _X, const double tol = 0.001, const int iteMax = 500)
{
    arma::vec X(_X.data, _X.n);
    int n = X.n_rows;
    double rhs = std::log(n) / n;
    double mx = arma::mean(X);
    X -= mx;
    double tau = arma::stddev(X) * std::sqrt((long double)n / std::log(n));
    double derOld = huberDer(X, tau, n);
    double mu = -derOld, muDiff = -derOld;
    arma::vec res = X - mu;
    arma::vec resSq = arma::square(res);
    tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
    double derNew = huberDer(res, tau, n);
    double derDiff = derNew - derOld;
    int ite = 1;
    while (std::abs(derNew) > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = muDiff * derDiff;
        if (cross > 0)
        {
            double a1 = cross / derDiff * derDiff;
            double a2 = muDiff * muDiff / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        derOld = derNew;
        muDiff = -alpha * derNew;
        mu += muDiff;
        res = X - mu;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
        derNew = huberDer(res, tau, n);
        derDiff = derNew - derOld;
        ite++;
    }
    return mu + mx;
}

double _huberMean(arma::vec X, const double tol = 0.001, const int iteMax = 500)
{
    int n = X.n_rows;
    double rhs = std::log(n) / n;
    double mx = arma::mean(X);
    X -= mx;
    double tau = arma::stddev(X) * std::sqrt((long double)n / std::log(n));
    double derOld = huberDer(X, tau, n);
    double mu = -derOld, muDiff = -derOld;
    arma::vec res = X - mu;
    arma::vec resSq = arma::square(res);
    tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
    double derNew = huberDer(res, tau, n);
    double derDiff = derNew - derOld;
    int ite = 1;
    while (std::abs(derNew) > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = muDiff * derDiff;
        if (cross > 0)
        {
            double a1 = cross / derDiff * derDiff;
            double a2 = muDiff * muDiff / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        derOld = derNew;
        muDiff = -alpha * derNew;
        mu += muDiff;
        res = X - mu;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootf1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
        derNew = huberDer(res, tau, n);
        derDiff = derNew - derOld;
        ite++;
    }
    return mu + mx;
}

double hMeanCov(const arma::vec &Z, const int n, const int d, const int N, double rhs, const double epsilon = 0.0001, const int iteMax = 500)
{
    double muOld = 0;
    double muNew = arma::mean(Z);
    double tau = arma::stddev(Z) * std::sqrt((long double)n / (2 * std::log(d) + std::log(n)));
    int iteNum = 0;
    arma::vec res(n), resSq(n), w(n);
    while ((std::abs(muNew - muOld) > epsilon) && iteNum < iteMax)
    {
        muOld = muNew;
        res = Z - muOld;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootf2(resSq, n, d, N, rhs, arma::min(resSq), arma::accu(resSq)));
        w = arma::min(tau / arma::abs(res), arma::ones(N));
        muNew = arma::as_scalar(Z.t() * w) / arma::accu(w);
        iteNum++;
    }
    return muNew;
}

extern "C" HuberCovResult huberCov(const MyMat &_X)
{
    const arma::mat X(_X.data, _X.num_rows, _X.num_cols);
    const int n = X.n_rows;
    const int p = X.n_cols;

    double rhs2 = (2 * std::log(p) + std::log(n)) / n;
    arma::vec mu(p);
    arma::mat sigmaHat(p, p);
    for (int j = 0; j < p; j++)
    {
        mu(j) = _huberMean(X.col(j), n);
        double theta = _huberMean(arma::square(X.col(j)), n);
        double temp = mu(j) * mu(j);
        if (theta > temp)
        {
            theta -= temp;
        }
        sigmaHat(j, j) = theta;
    }
    int N = n * (n - 1) >> 1;
    arma::mat Y(N, p);
    for (int i = 0, k = 0; i < n - 1; i++)
    {
        for (int j = i + 1; j < n; j++)
        {
            Y.row(k++) = X.row(i) - X.row(j);
        }
    }
    for (int i = 0; i < p - 1; i++)
    {
        for (int j = i + 1; j < p; j++)
        {
            sigmaHat(i, j) = sigmaHat(j, i) = hMeanCov(0.5 * Y.col(i) % Y.col(j), n, p, N, rhs2);
        }
    }

    HuberCovResult result;

    result.means.data = new double[mu.size()];
    result.means.n = mu.size();
    memcpy(result.means.data, mu.memptr(), mu.size() * sizeof(double));

    result.cov.data = new double[sigmaHat.n_rows * sigmaHat.n_cols];
    result.cov.num_rows = sigmaHat.n_rows;
    result.cov.num_cols = sigmaHat.n_cols;
    memcpy(result.cov.data, sigmaHat.memptr(), sigmaHat.n_rows * sigmaHat.n_cols * sizeof(double));

    return result;
}

double mad(const arma::vec &x)
{
    return 1.482602 * arma::median(arma::abs(x - arma::median(x)));
}

arma::mat standardize(arma::mat X, const arma::rowvec &mx, const arma::vec &sx, const int p)
{
    for (int i = 0; i < p; i++)
    {
        X.col(i) = (X.col(i) - mx(i)) / sx(i);
    }
    return X;
}

void updateHuber(const arma::mat &Z, const arma::vec &res, arma::vec &der, arma::vec &grad, const int n, const double tau, const double n1)
{
    for (int i = 0; i < n; i++)
    {
        double cur = res(i);
        if (std::abs(cur) <= tau)
        {
            der(i) = -cur;
        }
        else
        {
            der(i) = -tau * sgn(cur);
        }
    }
    grad = n1 * Z.t() * der;
}

extern "C" MyVec adaHuberReg(MyMat _X, MyVec _Y, const double tol = 0.0001, const int iteMax = 5000)
{
    arma::mat X(_X.data, _X.num_rows, _X.num_cols);
    arma::vec Y(_Y.data, _Y.n);
    int n = X.n_rows;
    int p = X.n_cols;
    const double n1 = 1.0 / n;
    double rhs = n1 * (p + std::log(n * p));
    arma::rowvec mx = arma::mean(X, 0);
    arma::vec sx = arma::stddev(X, 0, 0).t();
    double my = arma::mean(Y);
    arma::mat Z = arma::join_rows(arma::ones(n), standardize(X, mx, sx, p));
    Y -= my;
    double tau = 1.345 * mad(Y);
    arma::vec der(n);
    arma::vec gradOld(p + 1), gradNew(p + 1);
    updateHuber(Z, Y, der, gradOld, n, tau, n1);
    arma::vec beta = -gradOld, betaDiff = -gradOld;
    arma::vec res = Y - Z * beta;
    arma::vec resSq = arma::square(res);
    tau = std::sqrt((long double)rootg1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
    updateHuber(Z, res, der, gradNew, n, tau, n1);
    arma::vec gradDiff = gradNew - gradOld;
    int ite = 1;
    while (arma::norm(gradNew, "inf") > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = arma::as_scalar(betaDiff.t() * gradDiff);
        if (cross > 0)
        {
            double a1 = cross / arma::as_scalar(gradDiff.t() * gradDiff);
            double a2 = arma::as_scalar(betaDiff.t() * betaDiff) / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        gradOld = gradNew;
        betaDiff = -alpha * gradNew;
        beta += betaDiff;
        res -= Z * betaDiff;
        resSq = arma::square(res);
        tau = std::sqrt((long double)rootg1(resSq, n, rhs, arma::min(resSq), arma::accu(resSq)));
        updateHuber(Z, res, der, gradNew, n, tau, n1);
        gradDiff = gradNew - gradOld;
        ite++;
    }
    beta.rows(1, p) /= sx;
    beta(0) = _huberMean(Y + my - X * beta.rows(1, p));

    MyVec result;
    result.data = new double[n];
    result.n = n;
    memcpy(result.data, beta.memptr(), n * sizeof(double));
    return result;
}

extern "C" MyVec huberReg(MyMat _X, MyVec _Y, const double tol = 0.0001, const double constTau = 1.345, const int iteMax = 5000)
{
    arma::mat X(_X.data, _X.num_rows, _X.num_cols);
    arma::vec Y(_Y.data, _Y.n);
    int n = X.n_rows;
    int p = X.n_cols;
    const double n1 = 1.0 / n;
    arma::rowvec mx = arma::mean(X, 0);
    arma::vec sx = arma::stddev(X, 0, 0).t();
    double my = arma::mean(Y);
    arma::mat Z = arma::join_rows(arma::ones(n), standardize(X, mx, sx, p));
    Y -= my;
    double tau = constTau * mad(Y);
    arma::vec der(n);
    arma::vec gradOld(p + 1), gradNew(p + 1);
    updateHuber(Z, Y, der, gradOld, n, tau, n1);
    arma::vec beta = -gradOld, betaDiff = -gradOld;
    arma::vec res = Y - Z * beta;
    tau = constTau * mad(res);
    updateHuber(Z, res, der, gradNew, n, tau, n1);
    arma::vec gradDiff = gradNew - gradOld;
    int ite = 1;
    while (arma::norm(gradNew, "inf") > tol && ite <= iteMax)
    {
        double alpha = 1.0;
        double cross = arma::as_scalar(betaDiff.t() * gradDiff);
        if (cross > 0)
        {
            double a1 = cross / arma::as_scalar(gradDiff.t() * gradDiff);
            double a2 = arma::as_scalar(betaDiff.t() * betaDiff) / cross;
            alpha = std::min(std::min(a1, a2), 100.0);
        }
        gradOld = gradNew;
        betaDiff = -alpha * gradNew;
        beta += betaDiff;
        res -= Z * betaDiff;
        tau = constTau * mad(res);
        updateHuber(Z, res, der, gradNew, n, tau, n1);
        gradDiff = gradNew - gradOld;
        ite++;
    }
    beta.rows(1, p) /= sx;
    beta(0) = _huberMean(Y + my - X * beta.rows(1, p), n);

    MyVec result;
    result.data = new double[n];
    result.n = n;
    memcpy(result.data, beta.memptr(), n * sizeof(double));
    return result;
}

              /* - */
/*------------ LASSO ------------*/
              /* - */

arma::vec softThresh(const arma::vec& x, const arma::vec& lambda)
{
  return arma::sign(x) % arma::max(arma::abs(x) - lambda, arma::zeros(x.size()));
}

arma::vec cmptLambda(const arma::vec& beta, const double lambda)
{
  arma::vec rst = lambda * arma::ones(beta.size());
  rst(0) = 0;
  return rst;
}

double loss(const arma::vec& Y, const arma::vec& Ynew, const std::string lossType, const double tau)
{
  double rst = 0;
  if (lossType == "l2") {
    rst = arma::mean(arma::square(Y - Ynew)) / 2;
  } else if (lossType == "Huber") {
    arma::vec res = Y - Ynew;
    for (int i = 0; i < Y.size(); i++) {
      if (std::abs(res(i)) <= tau) {
        rst += res(i) * res(i) / 2;
      } else {
        rst += tau * std::abs(res(i)) - tau * tau / 2;
      }
    }
    rst /= Y.size();
  }
  return rst;
}

arma::vec gradLoss(const arma::mat& X, const arma::vec& Y, const arma::vec& beta,
                   const std::string lossType, const double tau)
{
  arma::vec res = Y - X * beta;
  arma::vec rst = arma::zeros(beta.size());
  if (lossType == "l2") {
    rst = -1 * (res.t() * X).t();
  } else if (lossType == "Huber") {
    for (int i = 0; i < Y.size(); i++) {
      if (std::abs(res(i)) <= tau) {
        rst -= res(i) * X.row(i).t();
      } else {
        rst -= tau * sgn(res(i)) * X.row(i).t();
      }
    }
  }
  return rst / Y.size();
}

arma::vec updateBeta(const arma::mat& X, const arma::vec& Y, arma::vec beta, const double phi,
                     const arma::vec& Lambda, const std::string lossType, const double tau)
{
  arma::vec first = beta - gradLoss(X, Y, beta, lossType, tau) / phi;
  arma::vec second = Lambda / phi;
  return softThresh(first, second);
}

double cmptPsi(const arma::mat& X, const arma::vec& Y, const arma::vec& betaNew,
               const arma::vec& beta, const double phi, const std::string lossType,
               const double tau)
{
  arma::vec diff = betaNew - beta;
  double rst = loss(Y, X * beta, lossType, tau)
    + arma::as_scalar((gradLoss(X, Y, beta, lossType, tau)).t() * diff)
    + phi * arma::as_scalar(diff.t() * diff) / 2;
    return rst;
}

Rcpp::List LAMM(const arma::mat& X, const arma::vec& Y, const arma::vec& Lambda, arma::vec beta,
                const double phi, const std::string lossType, const double tau,
                const double gamma)
{
  double phiNew = phi;
  arma::vec betaNew = arma::vec();
  double FVal, PsiVal;
  while (true) {
    betaNew = updateBeta(X, Y, beta, phiNew, Lambda, lossType, tau);
    FVal = loss(Y, X * betaNew, lossType, tau);
    PsiVal = cmptPsi(X, Y, betaNew, beta, phiNew, lossType, tau);
    if (FVal <= PsiVal) {
      break;
    }
    phiNew *= gamma;
  }
  return Rcpp::List::create(Rcpp::Named("beta") = betaNew, Rcpp::Named("phi") = phiNew);
}

arma::vec lasso(const arma::mat& X, const arma::vec& Y, const double lambda,
                const double phi0 = 0.001, const double gamma = 1.5,
                const double epsilon_c = 0.001, const int iteMax = 500)
{
  int d = X.n_cols - 1;
  arma::vec beta = arma::zeros(d + 1);
  arma::vec betaNew = arma::zeros(d + 1);
  arma::vec Lambda = cmptLambda(beta, lambda);
  double phi = phi0;
  int ite = 0;
  Rcpp::List listLAMM;
  while (ite < iteMax) {
    ite++;
    listLAMM = LAMM(X, Y, Lambda, beta, phi, "l2", 1, gamma);
    betaNew = Rcpp::as<arma::vec>(listLAMM["beta"]);
    phi = listLAMM["phi"];
    phi = std::max(phi0, phi / gamma);
    if (arma::norm(betaNew - beta, "inf") <= epsilon_c) {
      break;
    }
    beta = betaNew;
  }
  return betaNew;
}

Rcpp::List huberLasso(const arma::mat& X, const arma::vec& Y, const double lambda,
                      double tau = -1, const double constTau = 1.345, const double phi0 = 0.001,
                      const double gamma = 1.5, const double epsilon_c = 0.001,
                      const int iteMax = 500)
{
  int n = X.n_rows;
  int d = X.n_cols - 1;
  arma::vec beta = arma::zeros(d + 1);
  arma::vec betaNew = arma::zeros(d + 1);
  arma::vec Lambda = cmptLambda(beta, lambda);
  double mad;
  if (tau <= 0) {
    arma::vec betaLasso = lasso(X, Y, lambda, phi0, gamma, epsilon_c, iteMax);
    arma::vec res = Y - X * betaLasso;
    mad = arma::median(arma::abs(res - arma::median(res))) / 0.6744898;
    tau = constTau * mad;
  }
  double phi = phi0;
  int ite = 0;
  Rcpp::List listLAMM;
  arma::vec res(n);
  while (ite < iteMax) {
    ite++;
    listLAMM = LAMM(X, Y, Lambda, beta, phi, "Huber", tau, gamma);
    betaNew = Rcpp::as<arma::vec>(listLAMM["beta"]);
    phi = listLAMM["phi"];
    phi = std::max(phi0, phi / gamma);
    if (arma::norm(betaNew - beta, "inf") <= epsilon_c) {
      break;
    }
    beta = betaNew;
    res = Y - X * beta;
    mad = arma::median(arma::abs(res - arma::median(res))) / 0.6744898;
    tau = constTau * mad;
  }
  return Rcpp::List::create(Rcpp::Named("beta") = betaNew, Rcpp::Named("tau") = tau,
                            Rcpp::Named("iteration") = ite);
}

arma::uvec getIndex(const int n, const int low, const int up)
{
  arma::vec seq = arma::regspace(0, n - 1);
  return arma::find(seq >= low && seq <= up);
}

arma::uvec getIndexComp(const int n, const int low, const int up)
{
  arma::vec seq = arma::regspace(0, n - 1);
  return arma::find(seq < low || seq > up);
}

double pairPred(const arma::mat& X, const arma::vec& Y, const arma::vec& beta)
{
  int n = X.n_rows;
  int d = X.n_cols - 1;
  int m = n * (n - 1) >> 1;
  arma::mat pairX(m, d + 1);
  arma::vec pairY(m);
  int k = 0;
  for (int i = 0; i < n; i++) {
    for (int j = i + 1; j < n; j++) {
      pairX.row(k) = X.row(i) - X.row(j);
      pairY(k++) = Y(i) - Y(j);
    }
  }
  arma::vec predY = pairX * beta;
  return arma::accu(arma::square(pairY - predY));
}

Rcpp::List cvHuberLasso(const arma::mat& X, const arma::vec& Y,
                        Rcpp::Nullable<Rcpp::NumericVector> lSeq = R_NilValue, int nlambda = 30,
                        const double constTau = 2.5, const double phi0 = 0.001,
                        const double gamma = 1.5, const double epsilon_c = 0.001,
                        const int iteMax = 500, int nfolds = 3)
{
  int n = X.n_rows;
  int d = X.n_cols;
  arma::mat Z(n, d + 1);
  Z.cols(1, d) = X;
  Z.col(0) = arma::ones(n);
  arma::vec lambdaSeq = arma::vec();
  if (lSeq.isNotNull()) {
    lambdaSeq = Rcpp::as<arma::vec>(lSeq);
    nlambda = lambdaSeq.size();
  } else {
    double lambdaMax = arma::max(arma::abs(Y.t() * Z)) / n;
    double lambdaMin = 0.01 * lambdaMax;
    lambdaSeq = arma::exp(arma::linspace(std::log((long double)lambdaMin),
                                   std::log((long double)lambdaMax), nlambda));
  }
  if (nfolds > 10 || nfolds > n) {
    nfolds = n < 10 ? n : 10;
  }
  int size = n / nfolds;
  arma::vec mse = arma::zeros(nlambda);
  int low, up;
  arma::uvec idx, idxComp;
  Rcpp::List hLassoList;
  arma::vec thetaHat(d + 1);
  for (int i = 0; i < nlambda; i++) {
    for (int j = 0; j < nfolds; j++) {
      low = j * size;
      up = (j == (nfolds - 1)) ? (n - 1) : ((j + 1) * size - 1);
      idx = getIndex(n, low, up);
      idxComp = getIndexComp(n, low, up);
      hLassoList = huberLasso(Z.rows(idxComp), Y.rows(idxComp), lambdaSeq(i), -1, constTau, phi0,
                              gamma, epsilon_c, iteMax);
      thetaHat = Rcpp::as<arma::vec>(hLassoList["beta"]);
      mse(i) += pairPred(Z.rows(idx), Y.rows(idx), thetaHat);
    }
  }
  arma::uword cvIdx = mse.index_min();
  hLassoList = huberLasso(Z, Y, lambdaSeq(cvIdx), -1, constTau, phi0, gamma, epsilon_c, iteMax);
  arma::vec theta = Rcpp::as<arma::vec>(hLassoList["beta"]);
  Rcpp::List listMean = huberMean(Y - Z.cols(1, d) * theta.rows(1, d));
  theta(0) = listMean["mu"];
  return Rcpp::List::create(Rcpp::Named("theta") = theta, Rcpp::Named("lambdaSeq") = lambdaSeq,
                            Rcpp::Named("lambdaMin") = lambdaSeq(cvIdx),
                            Rcpp::Named("tauCoef") = hLassoList["tau"],
                            Rcpp::Named("tauItcp") = listMean["tau"],
                            Rcpp::Named("iteCoef") = hLassoList["iteration"],
                            Rcpp::Named("iteItcp") = listMean["iteration"]);
}

              /* ------------------------------------ */
/*------------ Alternating Gradient Descent Functions ------------*/
              /* ------------------------------------ */

double LnVal(const arma::vec &Y, double mu, double tau, const int n, double z)
{
    return (arma::accu(arma::sqrt(tau * tau + arma::square(Y - mu)) - tau) / (z * std::sqrt(n)) + z * tau / std::sqrt(n)) / n;
}

double gradientMuVal(const arma::vec &Y, double mu, double tau, const int n, double z)
{
    return -(1 / std::sqrt(n)) * arma::accu((Y - mu) / (z * arma::sqrt(tau * tau + arma::square(Y - mu)))) / n;
}

double gradientTauVal(const arma::vec &Y, double mu, double tau, const int n, double z)
{
    return (1 / std::sqrt(n)) * arma::accu(tau / (z * arma::sqrt(tau * tau + arma::square(Y - mu)))) / n - (std::sqrt(n) / z - z / std::sqrt(n)) / n;
}

extern "C" double agd(MyVec _Y, double epsilon = 1e-5, int iteMax = 5000)
{
    arma::vec Y(_Y.data, _Y.n);
    int n = Y.n_elem;
    double eta1 = 1.0;
    double eta2 = std::sqrt(n);
    double muOld = std::numeric_limits<double>::infinity();
    double muNew = arma::mean(Y);
    double tauOld = std::numeric_limits<double>::infinity();
    double delta = 0.05;
    double z = std::min(std::sqrt(std::log(n)), std::sqrt(std::log(1 / delta)));
    double tauNew = arma::stddev(Y) * std::sqrt(n) / (std::sqrt(2) * z);
    int iteNum = 0;
    double LMuOld = std::numeric_limits<double>::infinity();
    double LMuNew = LnVal(Y, muNew, tauNew, n, z);

    while (((LMuOld - LMuNew > 1e-10) && std::abs(muOld - muNew) > epsilon) && std::abs(tauOld - tauNew) > epsilon && iteNum < iteMax)
    {
        muOld = muNew;
        tauOld = tauNew;
        LMuOld = LMuNew;

        double gradientLnTauVal = gradientTauVal(Y, muOld, tauOld, n, z);
        tauNew = tauOld - eta2 * gradientLnTauVal;

        double gradientLnMuVal = gradientMuVal(Y, muOld, tauNew, n, z);
        muNew = muOld - eta1 * gradientLnMuVal;
        LMuNew = LnVal(Y, muNew, tauNew, n, z);
        iteNum += 1;
    }
    return muNew;
}

extern "C" double agdBB(MyVec _Y, double epsilon = 1e-5, int iteMax = 5000)
{
    arma::vec Y(_Y.data, _Y.n);
    int n = Y.n_elem;
    double eta1 = 1.0;
    double eta2 = std::sqrt(n);
    double muOld = std::numeric_limits<double>::infinity();
    double muNew = arma::mean(Y);
    double tauOld = std::numeric_limits<double>::infinity();
    double delta = 0.05;
    double z = std::min(std::sqrt(std::log(n)), std::sqrt(std::log(1 / delta)));
    double tauNew = arma::stddev(Y) * std::sqrt(n) / (std::sqrt(2) * z);
    int iteNum = 0;

    double stepSizeTau = eta2;
    double stepSizeMu = eta1;

    double LMuOld = std::numeric_limits<double>::infinity();
    double LMuNew = LnVal(Y, muNew, tauNew, n, z);

    double lastTauTilde;
    double lastGradTau;
    double lastMuTilde;
    double lastGradMu;

    while (((LMuOld - LMuNew) > epsilon * epsilon && std::abs(muOld - muNew) > epsilon) && std::abs(tauOld - tauNew) > epsilon && iteNum < iteMax)
    {
        LMuOld = LMuNew;
        muOld = muNew;
        tauOld = tauNew;

        double gradTau = gradientTauVal(Y, muOld, tauOld, n, z);
        if (iteNum > 0)
        {
            double sTau = tauOld - lastTauTilde;
            double yTau = gradTau - lastGradTau;
            stepSizeTau = std::max(eta2, std::min(std::min((sTau * yTau) / (yTau * yTau), (sTau * sTau) / std::max(0.000001, sTau * yTau)), 1e3));
        }
        lastGradTau = gradTau;
        lastTauTilde = tauOld;

        tauNew = std::max(1.35, tauOld - stepSizeTau * (gradientTauVal(Y, muOld, tauOld, n, z)));

        double gradMu = gradientMuVal(Y, muOld, tauNew, n, z);
        if (iteNum > 0)
        {
            double sMu = muOld - lastMuTilde;
            double yMu = gradMu - lastGradMu;
            stepSizeMu = std::max(eta1, std::min(std::min((sMu * yMu) / (yMu * yMu), (sMu * sMu) / std::max(0.000001, sMu * yMu)), 1e3));
        }
        lastGradMu = gradMu;
        lastMuTilde = muOld;

        muNew = muOld - stepSizeMu * (gradientMuVal(Y, muOld, tauNew, n, z));
        LMuNew = LnVal(Y, muNew, tauNew, n, z);
        iteNum += 1;
    }
    return muNew;
}

extern "C" double agdBacktracking(MyVec _Y, double s1 = 1.0, double gamma1 = 0.5, double beta1 = 0.8, double s2 = 1.0,
                                  double gamma2 = 0.5, double beta2 = 0.8, double epsilon = 1e-5, int iteMax = 5000)
{
    arma::vec Y(_Y.data, _Y.n);
    int n = Y.n_elem;
    double muOld = std::numeric_limits<double>::infinity();
    double muNew = arma::mean(Y);
    double tauOld = std::numeric_limits<double>::infinity();
    double delta = 0.05;
    double z = std::min(std::sqrt(std::log(n)), std::sqrt(std::log(1 / delta)));
    double tauNew = arma::stddev(Y) * std::sqrt(n) / (std::sqrt(2) * z);
    int iteNum = 0;

    while (std::abs(muOld - muNew) > epsilon && std::abs(tauOld - tauNew) > epsilon && iteNum < iteMax)
    {
        muOld = muNew;
        tauOld = tauNew;
        double LTauOld = LnVal(Y, muOld, tauOld, n, z);
        double gradientLnTauVal = gradientTauVal(Y, muOld, tauOld, n, z);

        /*backtracking for eta2*/
        double eta2 = s2;
        tauNew = tauOld - eta2*gradientLnTauVal;
        double LTauNew = LnVal(Y, muOld, tauNew, n ,z);

        while (LTauNew > LTauOld + gamma2*eta2*gradientLnTauVal)
        {
            eta2 = beta2*eta2;
            tauNew = tauOld - eta2*gradientLnTauVal;
            LTauNew = LnVal(Y, muOld, tauNew, n ,z);
            gradientLnTauVal = gradientTauVal(Y, muOld, tauNew, n, z);
        }

        double LMuOld = LTauNew;
        double gradientLnMuVal = gradientMuVal(Y, muOld, tauNew, n, z);

        /*backtracking for eta1*/
        double eta1 = s1;
        muNew = muOld - eta1*gradientLnMuVal;
        double LMuNew = LnVal(Y, muNew, tauNew, n ,z);

        while (LMuNew > LMuOld + gamma1*eta1*gradientLnMuVal)
        {
            eta1 = beta1*eta1;
            muNew = muOld - eta1*gradientLnMuVal;
            LMuNew = LnVal(Y, muNew, tauNew, n ,z);
            gradientLnMuVal = gradientMuVal(Y, muNew, tauNew, n, z);
        }
        iteNum += 1;
    }
    return muNew;
}


/**
 * @file    pose_estimation_3d3d.cpp
 * @version v1.0.0
 * @date    Nov,6 2017
 * @author  jacob.lin
 */

#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#include <chrono>

using namespace std;
using namespace cv;

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

/*  */
void pose_estimation_3d3d(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

/*  */
void bundleAdjustment(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

/* g2o edge */
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map(_point);
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 1) = -z;
        _jacobianOplusXi(0, 2) = y;
        _jacobianOplusXi(0, 3) = -1;
        _jacobianOplusXi(0, 4) = 0;
        _jacobianOplusXi(0, 5) = 0;

        _jacobianOplusXi(1, 0) = z;
        _jacobianOplusXi(1, 1) = 0;
        _jacobianOplusXi(1, 2) = -x;
        _jacobianOplusXi(1, 3) = 0;
        _jacobianOplusXi(1, 4) = -1;
        _jacobianOplusXi(1, 5) = 0;

        _jacobianOplusXi(2, 0) = -y;
        _jacobianOplusXi(2, 1) = x;
        _jacobianOplusXi(2, 2) = 0;
        _jacobianOplusXi(2, 3) = 0;
        _jacobianOplusXi(2, 4) = 0;
        _jacobianOplusXi(2, 5) = -1;
    }

    bool read(std::istream& in) {}
    bool write(std::ostream & out) const {}

protected:
    Eigen::Vector3d _point;
};

int main(int argc, char** argv)
{
    if (argc != 5) {
        cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2." << endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);
    if (img_1.empty() || img_2.empty()) {
        cerr << "img1 or img2 is empty." << endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "feature matches points total is " << matches.size() << endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat depth1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat depth2 = cv::imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);
    if (depth1.empty() || depth2.empty()) {
        cerr << "depth1 or depth2 is empty." << endl;
        return 1;
    }

    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts1, pts2;

    for (cv::DMatch m : matches) {
        ushort d1 = depth1.ptr<unsigned short>((int)keypoints_1[m.queryIdx].pt.y)[(int)keypoints_1[m.queryIdx].pt.x];
        ushort d2 = depth2.ptr<unsigned short>((int)keypoints_2[m.trainIdx].pt.y)[(int)keypoints_2[m.trainIdx].pt.x];
        if (d1 == 0 || d2 == 0) {
            continue;
        }
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        cv::Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1) / 5000.0;
        float dd2 = float(d2) / 5000.0;
        pts1.push_back(cv::Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(cv::Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    cout << "3d-3d pairs: " << pts1.size() << endl;

    cv::Mat R, t;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout << "ICP vid SVD results: " << endl;
    cout << "R = " << R << endl;
    cout << "t = " << t << endl;
    cout << "R_inv = " << R.t() << endl;
    cout << "t_inv = " << -R.t() * t << endl;

    cout << "calling bundle adjustment. \r\n";

    bundleAdjustment(pts1, pts2, R, t);

    // verify p1 = R*p2 + t
    for (int i = 0; i < 5; i ++) {
        cout << "p1 = " << pts1[i] << endl;
        cout << "p2 = " << pts2[i] << endl;
        cout << "R*p2 + t = " << R * (Mat_<double>(3, 1) << pts2[i].x, pts2[i].y, pts2[i].z) + t << endl;
        cout << endl;
    }

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 3rd: 对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 4th: 匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    /* 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 */
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) {
            min_dist = dist;
        }
        if (dist > max_dist) {
            max_dist = dist;
        }
    }

    printf("-- Max distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_2.rows; i ++) {
        if (match[i].distance <= max(2*min_dist, 30.0)) {
            matches.push_back(match[i]);
        }
    }
}

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d
        (
            (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
            (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

void pose_estimation_3d3d(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    // center of mass
    cv::Point3f p1, p2;
    int N = pts1.size();
    for (int i = 0; i < N; i ++) {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = cv::Point3f(Vec3f(p1) / N);
    p2 = cv::Point3f(Vec3f(p2) / N);

    // remove the center
    std::vector<cv::Point3f> q1(N), q2(N);
    for (int i = 0; i < N; i ++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i ++) {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    cout << "W = " << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 利用SVD求解3D-3D变换，需要U和V的行列式同号。换言之，旋转矩阵的行列式只能为1，不能为-1
    if (U.determinant() * V.determinant() < 0) {
        for (int x = 0; x < 3; ++x) {
            U(x, 2) *= -1;
        }
    }
    cout << "U = " << U << endl;
    cout << "V = " << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::mat
    R = (cv::Mat_<double>(3, 3) <<
        R_(0, 0), R_(0, 1), R_(0, 2),
        R_(1, 0), R_(1, 1), R_(1, 2),
        R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (cv::Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

void bundleAdjustment(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    // Initialize g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;   // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType* linear_solver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();   // 线性方程求解器
    Block* solver_ptr = new Block((std::unique_ptr<Block::LinearSolverType>) linear_solver);
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton((std::unique_ptr<Block>) solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();    // camera pose
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 0)));
    optimizer.addVertex(pose);

    // edges
    int index = 1;
    std::vector<EdgeProjectXYZRGBDPoseOnly*> edges;
    for (size_t i = 0; i < pts1.size(); i ++) {
        EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*>(pose));
        edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
        edge->setInformation(Eigen::Matrix3d::Identity() * 1e4);
        optimizer.addEdge(edge);
        index ++;
        edges.push_back(edge);
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << "\r\n";

    cout << endl << "after optimization: " << endl;
    cout << "T = " << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "expsum/exponential_sum.hpp"
#include "expsum/kernel_functions/sph_bessel_kernel.hpp"
// #include "expsum/fitting/fast_esprit.hpp"
#include <boost/math/special_functions/bessel.hpp>

using size_type   = arma::uword;
using expsum_type = expsum::exponential_sum<double, std::complex<double>>;

template <typename F, typename Vec>
void make_sample(F f, double xmin, double xmax, Vec& result)
{
    auto np = result.n_elem;
    auto h  = (xmax - xmin) / (np - 1);
    for (size_type n = 0; n < np; ++n)
    {
        result(n) = f(xmin + n * h);
    }

    return;
}

// expsum_type sph_bessel_kernel(int l, double xmax, size_type N, size_type L,
//                               size_type M, double eps)
// {
//     using esprit_type = expsum::fast_esprit<double>;
//     using vector_type = arma::Col<double>;

//     vector_type exact(N);
//     make_sample([=](double x) { return boost::math::sph_bessel(l, x); }, 0.0,
//                 xmax, exact);
//     auto delta = xmax / (N - 1);

//     esprit_type esprit(N, L, M);

//     esprit.fit(exact, 0.0, delta, eps);
//     expsum_type ret(esprit.exponents(), esprit.weights());

//     ret.remove_small_terms(eps / 100.0);

//     return ret;
// }

expsum_type sph_bessel_kernel(int l, double xmax, double eps)
{
    // using vector_type = arma::Col<double>;
    expsum::sph_bessel_kernel<double> body;
    body.compute(l, xmax, eps);

    expsum_type ret(body.exponents(), body.weights());

    return ret;
}

void sph_bessel_kernel_error(int l, double xmax, size_type n_samples,
                             const expsum_type& ret)
{
    using vector_type = arma::Col<double>;

    vector_type x(arma::linspace<vector_type>(0.0, xmax, n_samples));
    vector_type exact(n_samples), approx(n_samples);

    for (size_type i = 0; i < n_samples; ++i)
    {
        exact(i)    = boost::math::sph_bessel(l, x(i));
        approx(i)   = ret(x(i));
        auto abserr = std::abs(exact(i) - approx(i));
        std::cout << std::setw(24) << x(i) << std::setw(24) << exact(i)
                  << std::setw(24) << approx(i) << std::setw(24) << abserr
                  << '\n';
    }

    vector_type abserr(arma::abs(exact - approx));

    size_type imax = abserr.index_max();

    std::cout << "\n  abs. error in interval [0," << xmax << "]\n"
              << "    maximum : " << abserr(imax) << '\n'
              << "    averaged: " << arma::sum(abserr) / n_samples << '\n';
}

int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    const int lmax = 2;

    size_type N = 100001; // # of sampling points
    // size_type L = N / 2;       // window length
    // size_type M = 1000;        // max # of terms
    double xmax = 1.0e12;
    double eps  = 1.0e-12;

    expsum_type ret;

    std::cout
        << "# Approximation of spherical Bessel function by exponential sum\n";

    // for (int l = 0; l <= lmax; ++l)
    // {
    //     std::cout << "\n# --- order " << l << '\n';
    //     ret = sph_bessel_kernel(l, xmax, eps);
    //     ret.print(std::cout);

    //     sph_bessel_kernel_error(l, 10.0, N, ret);
    //     sph_bessel_kernel_error(l, 1.0e8, N, ret);
    // }

    // for (int l = 0; l <= lmax; ++l)
    // {
    //     std::cout << "\n# --- order " << l << '\n';
    //     sph_bessel_kernel(l, xmax, eps);
    // }

    std::cout << "\n# --- order " << lmax << '\n';
    ret = sph_bessel_kernel(1, xmax, eps);
    ret.print(std::cout);
    std::cout << "sum weights: " << arma::sum(ret.weight()) << std::endl;
    sph_bessel_kernel_error(1, 1.0, 100001, ret);
    // sph_bessel_kernel_error(1, xmax, N, ret);

    return 0;
}


#include <iostream>

#include <Eigen/Dense>

int main (int argc, char *argv[]){
    Eigen::MatrixXf m(2, 2);
    m << 1, 2, 3, 4;
    Eigen::VectorXf v(2);
    v << -1, 1;
    auto r = m * v;
    std::cout << r(0) << " " << r(1) << std::endl;
    return 0;
}


#include <ctime>
#include <fftw3.h>
#include <string>
#include <algorithm>
#include <iostream>
#include <random>
#include <math.h>
#include <vtkNew.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkCellArray.h>
#include <vtkIdList.h>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include "shtns.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){

    // Read the polydata
    vtkNew<vtkPolyDataReader> reader;
    reader->SetFileName("T7.vtk");
    reader->Update();
    auto pointCloud = reader->GetOutput();
    int N = pointCloud->GetNumberOfPoints();

    //**********************************************************************//
    //SHTns: Generate quadrature points
    int lmax = std::floor( sqrt(N) - 1 );
    int mmax = lmax, nlat = 0, nphi = 0, mres = 1;
    shtns_cfg shtns;
    shtns_verbose(1);
    shtns_norm norm = static_cast<shtns_norm>(SHT_REAL_NORM | SHT_NO_CS_PHASE);
    shtns = shtns_create(lmax, mmax, mres, norm );
    shtns_type flags = static_cast<shtns_type>( sht_gauss | SHT_NATIVE_LAYOUT);
    shtns_set_grid_auto(shtns, flags, 1e-14, 1, &nlat, &nphi );
    int NLM = shtns->nlm;
    int numQ = nphi*nlat;

    // Memory allocation
    double_t *f;
    f = static_cast<double_t*>(
	    fftw_malloc( NSPAT_ALLOC(shtns)*sizeof(double_t) ));
    std::complex<double_t> *Slm;
    Slm = static_cast<std::complex<double_t>*>(
	    fftw_malloc( NLM*sizeof(std::complex<double_t>) ));
    // Memory initialization
    for( auto lm = 0; lm < NLM; ++lm){
	Slm[lm] = 0.0;
    }

    // Calculate the quad point Q0
    Eigen::Matrix3Xd Q0(3,numQ);
    for( auto ip = 0; ip < nphi; ++ip){
	for( auto it = 0; it < nlat; ++it){
	    auto phi = ip*M_PI*2/nphi;
	    Q0.col(it + ip*nlat) << shtns->st[it]*std::cos(phi),
		shtns->st[it]*std::sin(phi), shtns->ct[it];
	}
    }

    //************************* Some handy functions ***********************//

    // A lambda function to write a Eigen::Matrix3Xd to VTK file
    auto writeMatToVTK =
	[](Eigen::Matrix3Xd M, std::string file, bool Proj = false){
	    vtkNew<vtkPoints> pts;
	    vtkNew<vtkCellArray> verts;
	    for( auto b = 0; b < M.cols(); ++b ){
		double x, y, z;
		x = M(0,b);
		y = M(1,b);
		z = Proj? 0.0 : M(2,b);
		pts->InsertNextPoint(x, y, z);
		verts->InsertNextCell( 1 );
		verts->InsertCellPoint( b );
	    }
	    vtkNew<vtkPolyData> poly;
	    poly->SetPoints( pts );
	    poly->SetVerts( verts );
	    vtkNew<vtkPolyDataWriter> wr;
	    wr->SetInputData( poly );
	    wr->SetFileName( file.c_str() );
	    wr->Write();
	};

    //**********************************************************************//
    clock_t t = clock();

    for( auto z = 0; z < 1000; ++z ){
	// Copy the point coordinates into a matrix
	Eigen::Map<Eigen::Matrix3Xd> points((double_t*)pointCloud->GetPoints()
		->GetData()->GetVoidPointer(0),3,N);

	// Project points to unit sphere
	points.colwise().normalize();

	// Reset the center of the sphere to origin by translating
	Vector3d center = points.rowwise().mean();
	points = points.colwise() - center;

	// Write original points to VTK
	//writeMatToVTK( points, "BasePoints.vtk" );

	// Rotate all points so that the point in 0th column is along z-axis
	Vector3d c = points.col(0);
	double_t cos_t = c(2);
	double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
	Vector3d axis;
	axis << c(1), -c(0), 0.;
	Matrix3d rotMat, axis_cross, outer;
	axis_cross << 0. , -axis(2), axis(1),
		   axis(2), 0., -axis(0),
		   -axis(1), axis(0), 0.;

	outer.noalias() = axis*axis.transpose();

	rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
	Matrix3Xd rPts(3,N);
	rPts = rotMat*points; // The points on a sphere rotated

	// Write the rotated points to VTK
	//writeMatToVTK( rPts, "RotBasePoints.vtk" );

	// Calculate the stereographic projections
	Vector3d p0;
	Map3Xd l0( &(rPts(0,1)), 3, N-1 );
	Matrix3Xd l(3,N-1), proj(3,N-1);
	p0 << 0,0,-1; // Point on the plane of projection
	c = rPts.col(0); // The point from which we are projecting
	l = (l0.colwise() - c).colwise().normalized(); // directions of projections
	for( auto j=0; j < N-1; ++j ){
	    proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
	}

	// Write the rotated points to VTK
	//writeMatToVTK( proj, "ProjBasePoints.vtk", true );

	// Insert the projected points in a CGAL vertex_with_info vector
	std::vector< std::pair< Point, unsigned> > verts;
	for( auto j=0; j < N-1; ++j ){
	    verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
	}

	Delaunay dt( verts.begin(), verts.end() );

	/*
	// Write the triangulation to file
	vtkNew<vtkCellArray> sphereTri;
	for( auto fc = dt.all_faces_begin(); fc != dt.all_faces_end(); ++fc ){
	sphereTri->InsertNextCell(3);
	for( auto zz = 2; zz >= 0; --zz ){
	auto vid = dt.is_infinite( fc->vertex( zz ) )? 0 :
	fc->vertex(zz)->info();
	sphereTri->InsertCellPoint(vid);
	}
	}
	pointCloud->SetPolys( sphereTri );
	vtkNew<vtkPolyDataWriter> wr;
	wr->SetInputData( pointCloud );
	wr->SetFileName( "Sphere.vtk" );
	wr->Write();
	*/

	// Create the input scalar field f(theta,phi)
	std::complex<double_t> Qlm[NLM];
	for(auto pp = 0; pp < NLM; ++pp)
	    Qlm[pp] = 0.0;
	Qlm[ LM(shtns, 0, 0) ] = std::complex<double_t>(0,0);
	Qlm[ LM(shtns, 1, 0) ] = std::complex<double_t>(2,0);
	Qlm[ LM(shtns, 1, 1) ] = std::complex<double_t>(0,3);
	Qlm[ LM(shtns, 7, 7) ] = std::complex<double_t>(7,0);

	Eigen::VectorXd finput(N);
	for(auto i = 0; i < N; ++i){
	    Eigen::Vector3d q;
	    q = points.col(i);
	    double_t x = q(0);
	    double_t y = q(1);
	    double_t z = q(2);
	    auto phi = std::atan2(y,x);
	    finput(i) = SH_to_point(shtns, Qlm, z, phi);
	}

	//Now we need to first identify which triangles each of the quadrature
	//points belongs to. For this, we need the coordinates of the points
	Eigen::Matrix3Xd Qr(3,numQ);// Rotated quadrature points
	Eigen::Matrix3Xd Qsp(3,numQ);// StereoProjection of quadrature points

	// Write Quadrature points to VTK file
	//writeMatToVTK( Q0, "QuadPointsOrig.vtk" );

	// Now we need to rotate and project the quadrature points
	// p0 is already defined as the z = -1 plane
	// c is the point from which we are projecting (calculated earlier)
	Qr = rotMat*Q0; // Rotate the quadrature points

	// Write the rotated points to VTK
	//writeMatToVTK( Qr, "RotQuadPoints.vtk" );

	Eigen::Matrix3Xd lQ(3,numQ);
	lQ = (Qr.colwise() - c).colwise().normalized();//projn dirn unit vectors
	for( auto j=0; j < numQ; ++j ){
	    Qsp.col(j) = ((p0(2) - Qr(2,j))/lQ(2,j))*lQ.col(j) + Qr.col(j);
	}

	// Write Stereographic projections of quadrature points to VTK file
	//writeMatToVTK( Qsp, "ProjQuadPoints.vtk", true );

	//Finally let's try to locate these points in the Triangulation and
	//interpolate it as per the weights
	Eigen::Map<Eigen::VectorXd> fquad(f,numQ);
	auto interpolate = [&points, &Q0, &finput, &fquad](const size_t i,
		const size_t j, const size_t k, const size_t q){
	    Eigen::Vector3d v0 = points.col(i);
	    Eigen::Vector3d v1 = points.col(j);
	    Eigen::Vector3d v2 = points.col(k);
	    Eigen::Vector3d qp = Q0.col(q);
	    auto A0 = ((v1-qp).cross((v2-qp))).norm();
	    auto A1 = ((v2-qp).cross((v0-qp))).norm();
	    auto A2 = ((v0-qp).cross((v1-qp))).norm();
	    auto A = A0 + A1 + A2;
	    fquad(q) = (A0/A)*finput(i) + (A1/A)*finput(j) + (A2/A)*finput(k);
	};
	for( auto j=0; j < numQ; ++j ){
	    auto query = Point( Qsp(0,j), Qsp(1,j) );
	    Delaunay::Locate_type lt;
	    int li;
	    Face_handle face = dt.locate( query, lt, li );
	    switch(lt){
		case Delaunay::FACE:
		    {
			auto id0 = face->vertex(0)->info();
			auto id1 = face->vertex(1)->info();
			auto id2 = face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		case Delaunay::EDGE:
		    {
			auto id1 = face->vertex( (li + 1)%3 )->info();
			auto id2 = face->vertex( (li + 2)%3 )->info();
			Eigen::Vector3d v1, v2, qp;
			v1 = points.col(id1);
			v2 = points.col(id2);
			qp = Q0.col(j);
			double_t ratio = (qp - v1).norm()/(qp - v2).norm();
			fquad(j) = (finput(id1) + ratio*finput(id2))/(1 + ratio);
			break;
		    }
		case Delaunay::VERTEX:
		    fquad(j) = finput( face->vertex( li )->info() );
		    break;
		case Delaunay::OUTSIDE_CONVEX_HULL:
		    {
			Eigen::Vector3d v0, v1, v2;
			auto id0 = dt.is_infinite(face->vertex(0))?
			    0 : face->vertex(0)->info();
			auto id1 = dt.is_infinite(face->vertex(1))?
			    0 : face->vertex(1)->info();
			auto id2 = dt.is_infinite(face->vertex(2))?
			    0 : face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		default:
		    std::cout<< "Point " << j << " not found!" << std::endl;
	    }
	}

	/*
	   std::cout<< "Print interpolated values and actual values..."<<std::endl;
	   Eigen::VectorXd direct(numQ);
	   for(auto i = 0; i < numQ; ++i){
	   Eigen::Vector3d q;
	   q = Q0.col(i);
	   double_t x = q(0);
	   double_t y = q(1);
	   double_t z = q(2);
	   auto phi = std::atan2(y,x);
	   direct(i) = SH_to_point(shtns, Qlm, z, phi);
	   std::cout<< fquad(i) << " " << direct(i) << std::endl;
	   }
	   */

	// Now time for spherical harmonic analysis -- find the coefficients flm
	spat_to_SH(shtns, f, Slm);

	/*
	   std::cout<< "Printing the coefficients..." << std::endl;
	// Print the spherical harmonic coefficients
	for( auto l = 0; l <= lmax; ++l ){
	for( auto m = -l; m <= l; ++m ){
	std::complex<double_t> flm;
	if( m < 0){
	auto factor = (-m%2 == 0)? 1.0 : -1.0;
	flm = factor*std::conj(Slm[ LM(shtns,l,-m) ]);
	}
	else{
	flm = Slm[ LM(shtns,l,m) ];
	}
	std::cout<< l << " " << m << " " << flm << std::endl;
	}
	}
	*/
    }
    std::cout<< "Time for 1000 steps = "
	<< ((float)(clock() - t))/CLOCKS_PER_SEC << std::endl;
    for( auto lm = 0; lm < NLM; ++lm){
	std::cout<< Slm[ lm ] << std::endl;
    }
    return 0;
}


// -*- coding: utf-8 -*-

#define _USE_MATH_DEFINES
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <chrono>
#include <assert.h>

double slp_laplace(const Eigen::Vector2d &xm, const Eigen::Vector2d &ym, const Eigen::Vector2d &y1,
		   const Eigen::Vector2d &y2, const double &hy, const Eigen::Vector2d &yn){
//  Eigen::Vector2d ym;
  Eigen::Vector2d rr1;
  Eigen::Vector2d rr2;
  double r1;
  double r2;
  Eigen::Vector2d yt;
  double uk1, uk2, uk3, uk4, uk5;
  constexpr double arctwopi = 1.0/(2.0*std::acos(-1.0));

//  ym = 0.5*(y1 + y2);

  if((xm - ym).squaredNorm() < 1.0e-8){
    return hy*(1.0 - std::log(hy*0.5))*arctwopi;
  }
  else{
    rr1 = xm - y1;
    rr2 = xm - y2;
    r1 = rr1.norm();
    r2 = rr2.norm();
    yt(0) = -yn(1);
    yt(1) = yn(0);

    uk1 = rr1.dot(yn);
    uk2 = rr1.dot(yt);
    uk3 = rr2.dot(yn);
    uk4 = rr2.dot(yt);
    uk5 = std::atan2(uk3, uk4) - std::atan2(uk1, uk2);
    return (uk4*std::log(r2) - uk2*std::log(r1) + hy - uk1*uk5)*arctwopi;
//    std::cout << "tes3 " << std::endl; //dbg
  }
}

double dlp_laplace(const Eigen::Vector2d &xm, const Eigen::Vector2d &ym, const Eigen::Vector2d &y1,
		   const Eigen::Vector2d &y2, const double &hy, const Eigen::Vector2d &yn,
		   const int &exterior){

//  Eigen::Vector2d ym;
  Eigen::Vector2d rr1;
  Eigen::Vector2d rr2;
  double r1;
  double r2;
  Eigen::Vector2d yt;
  double uk1, uk2, uk3, uk4;
  constexpr double arctwopi = 1.0/(2.0*std::acos(-1.0));

  //  ym = 0.5*(y1 + y2);

  if((xm - ym).squaredNorm() < 1.0e-8){
    if(exterior == 0){
      return 0.5;
    }else if(exterior == 1){
      return -0.5;
    }else{
      assert(false);
    }
  }
  else{
    rr1 = xm - y1;
    rr2 = xm - y2;
    r1 = rr1.norm();
    r2 = rr2.norm();
    yt(0) = -yn(1);
    yt(1) = yn(0);

    uk1 = rr1.dot(yn);
    uk2 = rr1.dot(yt);
    uk3 = rr2.dot(yn);
    uk4 = rr2.dot(yt);
    return arctwopi*(std::atan2(uk3, uk4) - std::atan2(uk1, uk2));
  }
}

int main(){
  int n;
  double rad;
  std::ifstream fr("input");
  fr >> n >> rad;
  std::cout << "n " << n << std::endl;
  std::cout << "rad " << rad << std::endl;

  Eigen::MatrixXd x = Eigen::MatrixXd::Zero(2, n);
  Eigen::MatrixXd xn = Eigen::MatrixXd::Zero(2, n);
  Eigen::MatrixXi edge = Eigen::MatrixXi::Zero(2, n);
  Eigen::VectorXd hs = Eigen::VectorXd::Zero(n);
  Eigen::MatrixXd slp = Eigen::MatrixXd::Zero(n, n);
  Eigen::MatrixXd dlp = Eigen::MatrixXd::Zero(n, n);
  Eigen::VectorXd u = Eigen::VectorXd::Zero(n);
  Eigen::VectorXd kai = Eigen::VectorXd::Zero(n);
  Eigen::VectorXd xm = Eigen::VectorXd::Zero(2);
  Eigen::VectorXd ym = Eigen::VectorXd::Zero(2);

  constexpr double pi = std::acos(-1.0);

  int i;
  int j;
  double th;
  int i0;
  int i1;
  int j0;
  int j1;
  int exterior = 0;

  for(i = 0; i < n; i++){
    th = 2.0*pi*(i + 1.5)/n;
    x(0, i) = rad*cos(th);
    x(1, i) = rad*sin(th);
    edge(0, i) = i;
    edge(1, i) = (i + 1) % n;
  }

  Eigen::Vector2d rr1; //dbg
  Eigen::Vector2d rr2; //dbg
  double r1; //dbg
  double r2; //dbg
  Eigen::Vector2d yt; //dbg
  double uk1, uk2, uk3, uk4, uk5; //dbg
  constexpr double arctwopi = 1.0/(2.0*std::acos(-1.0)); //dbg

  for(i = 0; i < n; i++){
    i0 = edge(0, i);
    i1 = edge(1, i);
    xm = 0.5*(x.col(i0) + x.col(i1));

    if((i0 >= 0 && i0 < n) && (i1 >= 0 && i1 < n)){
      xn(0, i) = x(1, i1) - x(1, i0);
      xn(1, i) = x(0, i0) - x(0, i1);

      hs(i) = std::sqrt(std::pow(xn(0, i), 2.0) + std::pow(xn(1, i), 2.0));

      xn(0, i) = xn(0, i)/hs(i);
      xn(1, i) = xn(1, i)/hs(i);
    }
    else{
      assert(false);
    }
    // x**3*y - x*y**3
    u(i) = std::pow(xm(0), 3.0)*xm(1) - xm(0)*std::pow(xm(1), 3.0);

    // (3x**2*y - y**3)nx + (x**3 - 3xy**2)ny
    kai(i) = (3.0*std::pow(xm(0), 2.0)*xm(1) - std::pow(xm(1), 3.0))*xn(0, i)
      + (std::pow(xm(0), 3.0) - 3.0*xm(0)*std::pow(xm(1), 2.0))*xn(1, i);
  }

  for(j = 0; j < n; j++){
     j0 = edge(0, j);
     j1 = edge(1, j);
     ym = 0.5*(x.col(j0) + x.col(j1));
     for(i = 0; i < n; i++){
       i0 = edge(0, i);
       i1 = edge(1, i);
       xm = 0.5*(x.col(i0) + x.col(i1));

       slp(i, j) = slp_laplace(xm, ym, x.col(j0), x.col(j1), hs(j), xn.col(j));
       dlp(i, j) = dlp_laplace(xm, ym, x.col(j0), x.col(j1), hs(j), xn.col(j), exterior);
     }
  }

  u = slp.partialPivLu().solve(dlp*u);

  std::cout << "relative error " << std::sqrt(((u - kai).dot(u - kai))/kai.dot(kai)) << std::endl;
}


// SPDX-License-Identifier: MIT
// Copyright (c) 2019-2021 Thomas Vanderbruggen <th.vanderbruggen@gmail.com>

#include <Eigen/Dense>
#include <array>
#include <scicpp/core.hpp>
#include <scicpp/linalg.hpp>

int main() {
    Eigen::Matrix<double, 4, 2> A;
    A << 0., 1., //
        1., 1.,  //
        2., 1.,  //
        3., 1.;

    const std::array b{-1., 0.2, 0.9, 2.1};
    const auto x = scicpp::linalg::lstsq(A, b);
    scicpp::print(x);
}


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
#include <type_traits>
using namespace std;

#include <boost/math/constants/constants.hpp>
using namespace boost::math;

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

void case1()
{
    cout << setprecision(64);

    auto a = float_constants::pi * 2 * 2;
    cout << "area \t\t= " << a << endl;

    using namespace double_constants;

    auto x = root_two * root_three;
    cout << "root 2 * 3 \t= " << x << endl;

    cout << "root pi \t= " << root_pi << endl;
    cout << "pi pow e \t= " << pi_pow_e << endl;
}

//////////////////////////////////////////
#include <boost/multiprecision/cpp_dec_float.hpp>

void case2()
{
    using namespace constants;

    typedef decltype(pi<float>) pi_t;
    assert(is_function<pi_t>::value);

    assert(pi<float>() == float_constants::pi);
    assert(pi<double>() == double_constants::pi);

    typedef boost::multiprecision::cpp_dec_float_100 float_100;
    cout << setprecision(100)
         << pi<float_100>() << endl;
}

int main()
{
    case1();
    case2();
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Euler Euler (function template)

  Generates an approximation of the Euler-Mascheroni constant.

  @headerref{<boost/simd/constant/euler.hpp>}

  @par Description

  1.  @code
      template<typename T> T Euler();
      @endcode

  2.  @code
      template<typename T> T Euler( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T evaluating to the Euler-Mascheroni constant defined as:
  \f$\gamma = \lim_{n \rightarrow \infty } \left( 1+ \frac{1}{2} + ... + \frac{1}{n} - \ln(n) \right)\f$.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to `T(0.57721566490153286060651209008240243104215933593992359)`

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/euler.hpp>
#include <boost/simd/constant/simd/euler.hpp>

#endif


#include "physics.hpp"

#include <Eigen/Geometry>

physics::physics()
{
	this->generator = std::mt19937_64(std::random_device{}());

}

physics::~physics()
{

}

void physics::step(double delta_t)
{
	total_time += delta_t;

	// TODO: collision detection
	#pragma omp parallel for
	for(int i = 0; i < obj_count; i++)
	{
		// Euler
		//a[current][i] = accel(x[current][i], i);
		//v[next][i] = a[current][i] * delta_t + v[current][i];
		//x[next][i] = v[next][i] * delta_t + x[current][i];

		// RK4 integration, see doc/grav_sim.tex
		// this is about 4x worse than Euler
		Eigen::Vector3d xk1 = x[current][i];
		Eigen::Vector3d vk1 = v[current][i];
		Eigen::Vector3d ak1 = accel(xk1, i);

		Eigen::Vector3d xk2 = x[current][i] + 0.5 * vk1 * delta_t;
		Eigen::Vector3d vk2 = v[current][i] + 0.5 * ak1 * delta_t;
		Eigen::Vector3d ak2 = accel(xk2, i);

		Eigen::Vector3d xk3 = x[current][i] + 0.5 * vk2 * delta_t;
		Eigen::Vector3d vk3 = v[current][i] + 0.5 * ak2 * delta_t;
		Eigen::Vector3d ak3 = accel(xk3, i);

		Eigen::Vector3d xk4 = x[current][i] + vk3 * delta_t;
		Eigen::Vector3d vk4 = v[current][i] + ak3 * delta_t;
		Eigen::Vector3d ak4 = accel(xk4, i);

		v[next][i] = v[current][i] + (delta_t / 6.0) *
				(ak1 + 2 * ak2 + 2 * ak3 + ak4);
		x[next][i] = x[current][i] + (delta_t / 6.0) *
				(vk1 + 2 * vk2 + 2 * vk3 + vk4);
	}

	current = current ? 0 : 1;
	next = next ? 0 : 1;
}

Eigen::Vector3d physics::accel(Eigen::Vector3d x_i, uint16_t skip_index)
{
	Eigen::Vector3d a(0.0, 0.0, 0.0);
	for(uint16_t j = 0; j < obj_count; j++)
	{
		if(skip_index == j)
			continue;

		Eigen::Vector3d r = x[current][j] - x_i;
		a += G * m[j] * r.normalized() / r.squaredNorm();
	}
	return a;
}

void physics::init(uint16_t obj_count)
{
	this->obj_count = obj_count;

	current = 0;
	next = 1;
	total_time = 0.0;

	x[0].resize(obj_count);
	x[1].resize(obj_count);
	v[0].resize(obj_count);
	v[1].resize(obj_count);
	a[0].resize(obj_count);
	a[1].resize(obj_count);
	r.resize(obj_count);
	m.resize(obj_count);

	// for now hard code some values
	mass_range[0] = 5e7;
	mass_range[1] = 1e8;
	radius_range[0] = 0.05;
	radius_range[1] = 0.25;
	//distance_range[0] = -radius_range[0] * 15.0;
	//distance_range[1] = radius_range[0] * 15.0;
	distance_range[0] = -4.0;
	distance_range[1] = 4.0;

	// random init stuff
	std::uniform_real_distribution<double> dist_m(mass_range[0],
		mass_range[1]);
	std::uniform_real_distribution<double> dist_r(radius_range[0],
		radius_range[1]);
	std::uniform_real_distribution<double> dist_d(distance_range[0],
		distance_range[1]);

	for(uint16_t i = 0; i < obj_count; i++)
	{
		r[i] = dist_r(generator);
		m[i] = dist_m(generator);

		// TODO: detect and avoid collisions here
		bool collision = true;
		while(collision)
		{
			// generate a new point
			x[0][i] = Eigen::Vector3d(dist_d(generator),
									dist_d(generator),
									dist_d(generator));

			// look for a collision
			collision = false;
			for(uint16_t j = 0; j < i; j++)
			{
				if(i == j)
					continue;

				// direction unit vector
				Eigen::Vector3d ji_uv = (x[0][i] - x[0][j]).normalized();

				// position plus radius in the direction of the other object
				Eigen::Vector3d ji_r = x[0][j] + ji_uv * r[j];

				double distance_j = (x[0][i] - ji_r).norm();
				// if radius > distance to j object then collision
				if(r[i] > distance_j)
				{
					collision = true;
					break;
				}
			}
		}

		v[0][i] = Eigen::Vector3d(0.0, 0.0, 0.0);
		a[0][i] = Eigen::Vector3d(0.0, 0.0, 0.0);
	}
}

void physics::deinit()
{
	// TODO: should we really use swap to force a deallocation and is this the
	// right way to do it?

	std::vector<Eigen::Vector3d> n0, n1;
	x[0].clear();
	x[1].clear();
	x[0].swap(n0);
	x[1].swap(n1);

	std::vector<Eigen::Vector3d> n2, n3;
	v[0].clear();
	v[1].clear();
	v[0].swap(n2);
	v[1].swap(n3);

	std::vector<Eigen::Vector3d> n4, n5;
	a[0].clear();
	a[1].clear();
	a[0].swap(n4);
	a[1].swap(n5);

	std::vector<double> n6;
	r.clear();
	r.swap(n6);

	std::vector<double> n7;
	m.clear();
	m.swap(n7);
}


#include <iostream>
#include <armadillo>
#include <gnuplot-iostream.h>
#include "../config.hpp"

using namespace std;
using namespace arma;

pair<vec, double> entrenarPerceptron(const mat& patrones,
                                     const vec& salidaDeseada,
                                     int nEpocas,
                                     double tasaAprendizaje,
                                     double tolerancia,
                                     string tituloGrafica);
double errorPerceptron(const vec& pesos,
                   const mat& patrones,
                   const vec& salidaDeseada);

int main()
{
    arma_rng::set_seed_random();

    mat datos;
    datos.load(config::sourceDir + "/guia1/icgtp1datos/OR_trn.csv");
    mat patronesEntOR = datos.head_cols(2);
    vec salidaDeseadaEntOR = datos.tail_cols(1);
    datos.load(config::sourceDir + "/guia1/icgtp1datos/OR_tst.csv");
    mat patronesPruebaOR = datos.head_cols(2);
    vec salidaDeseadaPruebaOR = datos.tail_cols(1);

    // OR
    // Entrenar la red graficando resultados intermedios (la recta)
    vec pesos;
    tie(pesos, ignore) = entrenarPerceptron(patronesEntOR,
                                            salidaDeseadaEntOR,
                                            100,
                                            0.1,
                                            5,
                                            "OR");
    double tasaError = errorPerceptron(pesos, patronesPruebaOR, salidaDeseadaPruebaOR);
    cout << "tasa de error del OR : " << tasaError << endl;

    // XOR
    // Entrenar la red graficando resultados intermedios (la recta)
    // Prueba
    datos.load(config::sourceDir + "/guia1/icgtp1datos/XOR_trn.csv");
    mat patronesEntXOR = datos.head_cols(2);
    vec salidaDeseadaEntXOR = datos.tail_cols(1);
    datos.load(config::sourceDir + "/guia1/icgtp1datos/XOR_tst.csv");
    mat patronesPruebaXOR = datos.head_cols(2);
    vec salidaDeseadaPruebaXOR = datos.tail_cols(1);

    tie(pesos, ignore) = entrenarPerceptron(patronesEntXOR,
                                            salidaDeseadaEntXOR,
                                            100,
                                            0.4,
                                            30,
                                            "XOR");
    tasaError = errorPerceptron(pesos, patronesPruebaXOR, salidaDeseadaPruebaXOR);
    cout << "tasa de error del XOR : " << tasaError << endl;

    return 0;
}

namespace ic {
int sign(double numero)
{
    if (numero >= 0)
        return 1;
    else
        return -1;
}
}

pair<vec, double> entrenarPerceptron(const mat& patrones,
                                     const vec& salidaDeseada,
                                     int nEpocas,
                                     double tasaAprendizaje,
                                     double tolerancia,
                                     string tituloGrafica)
{
    // Inicializar pesos y tasa de error
    vec pesos = randu<vec>(patrones.n_cols + 1) - 0.5;
    double tasaError = 0;

    // Extender las matrices de patrones con la entrada correspondiente al umbral
    const mat patronesExt = join_horiz(ones(patrones.n_rows) * (-1), patrones);

    // Separar patrones de entrenamiento en casos verdaderos y falsos
    // para graficarlos en colores distintos
    const mat verdaderos = patrones.rows(find(salidaDeseada == 1));
    const mat falsos = patrones.rows(find(salidaDeseada == -1));

    // Objeto para graficar
    Gnuplot gp;
    // Caracter usado para dejar de graficar
    const char caracterSalteo = 's';
    char caracter = ' ';

    // Ciclo de las epocas
    for (int epoca = 1; epoca <= nEpocas; ++epoca) {
        // Ciclo para una época
        for (unsigned int i = 0; i < patrones.n_rows; ++i) {
            double z = dot(patronesExt.row(i), pesos);
            int y = ic::sign(z);

            if (caracter != caracterSalteo) {
                cout << "Patrón que entró: " << patrones.row(i)
                     << "Salida para ese patrón: " << y << '\n'
                     << "Salida deseada: " << salidaDeseada(i) << endl;

                caracter = getchar();
            }

            // Actualizar pesos
            // Tener en cuenta el orden en que colocamos la salida por la direccion del gradiente.
            pesos += tasaAprendizaje * 0.5 * (salidaDeseada(i) - y) * patronesExt.row(i).t();

            // Graficar recta
            double pendiente = -pesos(1) / pesos(2);
            double ordenadaOrigen = pesos(0) / pesos(2);
            mat puntosRecta = {{-2, -2 * pendiente + ordenadaOrigen},
                               {2, 2 * pendiente + ordenadaOrigen}};

            if (caracter != caracterSalteo) {
                gp << "set title '" << tituloGrafica << "' font ',13'\n"
                   << "set xlabel 'x_1'\n"
                   << "set ylabel 'x_2'\n"
                   << "set grid\n"
                   << "unset key\n"
                   << "plot " << gp.file1d(verdaderos) << "with points, "
                   << gp.file1d(falsos) << "with points, "
                   << gp.file1d(puntosRecta) << "with lines" << endl;
            }
        }
        // Fin ciclo (época)

        // Parte de validación - Cálculo de la tasa de error para ver si dejamos de entrenar
        int errores = 0;

        for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
            double z = dot(patronesExt.row(i), pesos);
            int y = ic::sign(z);

            if (y != salidaDeseada(i))
                ++errores;
        }

        tasaError = static_cast<double>(errores) / patrones.n_rows * 100;
        if (tasaError < tolerancia)
            break;
    }
    // Fin ciclo (epocas)

    return {pesos, tasaError};
}

double errorPerceptron(const vec& pesos,
                   const mat& patrones,
                   const vec& salidaDeseada)
{
    // Se extiende la matriz de patrones con la entrada correspondiente al umbral
    const mat patronesExt = join_horiz(ones(patrones.n_rows) * (-1), patrones);

    int errores = 0;

    for (unsigned int i = 0; i < patronesExt.n_rows; ++i) {
        double z = dot(patronesExt.row(i), pesos);
        int y = ic::sign(z);

        if (y != salidaDeseada(i))
            ++errores;
    }

    double tasaError = static_cast<double>(errores) / patronesExt.n_rows * 100;

    return tasaError;
}


#include "DarkART/Special_Functions.hpp"

#include <algorithm>
#include <iostream>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_coulomb.h>
#include <gsl/gsl_sf_coupling.h>

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

#include "arb_hypgeom.h"

namespace DarkART
{
// using namespace std::complex_literals;

// 1. Gaunt coefficients
double Gaunt_Coefficient(int j1, int j2, int j3, int m1, int m2, int m3)
{
	return sqrt((2.0 * j1 + 1.0) * (2.0 * j2 + 1.0) * (2.0 * j3 + 1.0)) / sqrt(4.0 * M_PI) * gsl_sf_coupling_3j(2 * j1, 2 * j2, 2 * j3, 0, 0, 0) * gsl_sf_coupling_3j(2 * j1, 2 * j2, 2 * j3, 2 * m1, 2 * m2, 2 * m3);
}

// 2. Spherical Bessel function j_L
double Spherical_Bessel_jL_arb(int L, double x)
{
	double prefactor = sqrt(M_PI / 2.0 / x);
	double result;
	slong prec;
	arb_t J_arb, x_arb, n_arb;
	arb_init(J_arb);
	arb_init(x_arb);
	arb_init(n_arb);
	arb_set_d(x_arb, x);
	arb_set_d(n_arb, 1.0 * L + 0.5);
	for(prec = 80;; prec *= 2)
	{
		arb_hypgeom_bessel_j(J_arb, n_arb, x_arb, prec);
		if(arb_rel_accuracy_bits(J_arb) >= 53)
		{
			result = arf_get_d(arb_midref(J_arb), ARF_RND_NEAR);
			break;
		}
		else if(prec > 10000)
		{
			result = NAN;
			break;
		}
	}
	arb_clear(J_arb);
	arb_clear(n_arb);
	arb_clear(x_arb);
	return prefactor * result;
}

double Spherical_Bessel_jL(int L, double x)
{
	if(L < 10)
		return gsl_sf_bessel_jl(L, x);
	else
		return Spherical_Bessel_jL_arb(L, x);
}

// 3. Coulomb wave
double Coulomb_Wave_ARB(int L, double eta, double rho)
{
	double result;
	slong prec;
	arb_t F, l, eta_2, rho_2;
	arb_init(F);
	arb_init(l);
	arb_init(eta_2);
	arb_init(rho_2);
	arb_set_d(l, L);
	arb_set_d(eta_2, eta);
	arb_set_d(rho_2, rho);
	for(prec = 80;; prec *= 2)
	{
		arb_hypgeom_coulomb(F, NULL, l, eta_2, rho_2, prec);
		if(arb_rel_accuracy_bits(F) >= 53)
		{
			result = arf_get_d(arb_midref(F), ARF_RND_NEAR);
			break;
		}
		else if(prec > 10000)
		{
			result = NAN;
			break;
		}
	}
	arb_clear(F);
	arb_clear(eta_2);
	arb_clear(rho_2);
	arb_clear(l);
	return result;
}

double Coulomb_Wave_GSL(int L, double eta, double rho, int& status)
{
	double fc_array[1];
	double F_exponent[1];
	status = gsl_sf_coulomb_wave_F_array(L, 0, eta, rho, fc_array, F_exponent);
	return fc_array[0];
}

double Coulomb_Wave(int L, double eta, double rho)
{
	int status;
	double cw = Coulomb_Wave_GSL(L, eta, rho, status);
	if(status != 0 || std::isnan(cw))
		cw = Coulomb_Wave_ARB(L, eta, rho);
	return cw;
}

}	// namespace DarkART


/*
 * prime_utils.cpp: Function definitions for working with primes. The following
 * methods are defined here:
 *     - Miller-Rabin probabilistic primality test, with an iterative tester
 *       method
 *     - Modular addition, which is used by modular multiplication
 *     - Modular multiplication and exponentiation, which are used by the
 *       Miller-Rabin test
 *     - Sieve of Atkin
 *     - Integer square root, which is used by the Sieve
 *
 * Version:     1.0.0
 * License:     MIT License (see LICENSE.txt for more details)
 * Author:      Joshua Morrison (MrM21632)
 * Last Edited: 2/6/2018, 1:20am
 */

#include <cstdlib>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include "prime_utils.h"

uint64_t isqrt(uint64_t n) {
    // Base Case: if n < 2, then sqrt(n) = n.
    if (n < 2) return n;

    // We calculate a "small" and "large" candidate for our return value:
    //     1. s, which is equal to 2 * isqrt(n / 4), is the small candidate.
    //     2. l, which is equal to one larger than s, is the large candidate.
    uint64_t s = isqrt(n >> 2) << 1;
    uint64_t l = s + 1;

    if (l*l > n)
        return s;
    else
        return l;
}

uint64_t mod_add(uint64_t a, uint64_t b, uint64_t n) {
    return ((a % n) + (b % n)) % n;
}

uint64_t mod_mult(uint64_t a, uint64_t b, uint64_t n) {
    uint64_t r = 0;  // Remainder

    // If a is larger than the modulus, we need to reduce it in order to avoid
    // integer overflow.
    if (a >= n) a %= n;

    // Perform modular addition on r until the multiplier is 0.
    while (b > 0) {
        if (b & 1)
            r = mod_add(r, a, n);

        a = (a * 2) % n;
        b >>= 1;
    }

    // (a * b) mod n == ((a mod n) * (b mod n)) mod n. At this point, we have
    // calculated r = (a mod n) * (b mod n), so we now need to reduce r by mod
    // n, then return that result.
    return r % n;
}

uint64_t mod_pow(uint64_t a, uint64_t b, uint64_t n) {
    uint64_t r = 1;  // Remainder

    // If a is larger than the modulus, we need to reduce it in order to avoid
    // integer overflow.
    if (a >= n) a %= n;

    // Perform modular multiplication on both the remainder and the base, until
    // our exponent is 0.
    while (b > 0) {
        if (b & 1)
            r = mod_mult(r, a, n);

        a = mod_mult(a, a, n);
        b >>= 1;
    }

    return r;
}

bool miller_rabin(uint64_t n, uint64_t d) {
    boost::random::random_device rd;
    boost::random::mt19937 mt(rd());
    boost::random::uniform_int_distribution<uint64_t> dist(2, n-2);

    uint64_t a = dist(mt);
    uint64_t x = mod_pow(a, d, n);

    // Base Case: if a^d mod n is 1 or n-1, then we already know n is prime, so
    // we can return true.
    if (x == 1 || x == n-1)
        return true;

    // Perform modular multiplication on x until d is equal to n-1.
    while (d != (n - 1)) {
        x = mod_mult(x, x, n);
        d <<= 1;

        if (x == 1) return false;
        if (x == (n - 1)) return true;
    }

    // If we reach this point, we can safely say that n is not prime, so we
    // return false.
    return false;
}

bool is_prime(uint64_t n, uint64_t k) {
    // Base cases
    if (n <= 1) return false;
    if (n <= 3) return true;
    if (!(n & 1)) return false;

    // At this point, we know n is odd and greater than 1. Now, our goal is to
    // find an integer d such that n-1 = d * 2^r, where r >= 1. The loop divides
    // d by 2 until it is no longer even - at that point, we will have the value
    // we desire.
    uint64_t d = n - 1;
    while (!(d & 1))
        d >>= 1;

    // Call miller_rabin(n, d) k times. If any of the tests fail, we know that n
    // is not prime, so we return false.
    for (uint64_t i = 0; i < k; ++i) {
        if (!miller_rabin(n, d))
            return false;
    }

    // If we reach this point, we can safely assume (NOT guarantee!) n is prime,
    // so we return true.
    return true;
}

bool *sieve_of_atkin(uint64_t n) {
    bool *data = new bool[n + 1]();
    data[2] = true;
    data[3] = true;
    uint64_t lim = isqrt(n);

    // This loop runs for all x in [1, lim].
    for (uint64_t x = 1; x <= lim; ++x) {
        // This loop runs for all y in [1, lim].
        //
        // k is prime if any of the following are true:
        //     1. For k = 4*x^2 + y^2, k is prime iff k mod 12 = 1 or 5.
        //     2. For k = 3*x^2 + y^2, k is prime iff k mod 12 = 7.
        //     3. For k = 3*x^2 - y^2, k is prime iff k mod 12 = 11.
        // All of these also assume that k is not greater than n, and for the
        // third case, that k is not negative (i.e., that x > y).
        for (uint64_t y = 1; y <= lim; ++y) {
            uint64_t k = (4*x*x) + (y*y);
            if ((k <= n) && ((k % 12 == 1) || (k % 12 == 5)))
                data[k] = !data[k];

            k = (3*x*x) + (y*y);
            if ((k <= n) && (k % 12 == 7))
                data[k] = !data[k];

            k = (3*x*x) - (y*y);
            if ((x > y) && (k <= n) && (k % 12 == 11))
                data[k] = !data[k];
        }
    }

    // This loop runs for all j in [5, lim].
    //
    // We perform one last loop through the array. If j is prime, then we mark
    // all multiples of its square as composite, a la the standard Sieve of
    // Eratosthenes.
    for (uint64_t j = 5; j <= lim; ++j) {
        if (data[j]) {
            for (uint64_t k = j*j; k <= n; k += j*j)
                data[k] = false;
        }
    }

    return data;
}


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

// With contributions from Cornelius Steinhardt

#ifndef MTL_MATRIX_HOUSEHOLDER_INCLUDE
#define MTL_MATRIX_HOUSEHOLDER_INCLUDE

#include <cmath>
#include <cassert>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vector {

/// Computes Householder vector v and scalar b for vector \p y
/** such that identity_matrix(size(y))-b*v*v' projects the vector y
    to a positive multiple of the first unit vector. **/
template <typename Vector>
std::pair<typename mtl::vector::dense_vector<typename Collection<Vector>::value_type, parameters<> >, typename Collection<Vector>::value_type>
inline householder(Vector& y)
{
    vampir_trace<2004> tracer;
    assert(size(y) > 0);
    typedef typename  Collection<Vector>::value_type   value_type;
    // typedef typename  Collection<Vector>::size_type    size_type;
    const value_type  zero= math::zero(y[0]), one= math::one(y[0]);

    Vector            v(y);
    v[0]= one;
    irange            tail(1, imax);
    value_type        s( dot(v[tail], v[tail]) ), b, v0;

    //evaluation of v and b
    if (s == zero)
        b= zero;
    else {
	value_type mu= sqrt(y[0] * y[0] + s);
	v0= v[0]= y[0] <= zero ? y[0] - mu : -s / (y[0] + mu); // complex < zero????
	b= 2 * v0 * v0 / (s + v0 * v0);                        // 2* complex???????
	v/= v0;                                                // normalization of the first entry
    }

    return std::make_pair(v,b);
}

/// Computes Householder vector for vector \p y
/** More stable Householder transformation, also for non-square matrices. **/
template <typename Vector>
typename mtl::vector::dense_vector<typename Collection<Vector>::value_type, parameters<> >
inline householder_s(Vector& y)
{
    vampir_trace<2005> tracer;
    typedef typename  Collection<Vector>::value_type   value_type;
    const value_type  zero= math::zero(y[0]);

    Vector            u(y);
    value_type        nu(sqrt( dot(u, u) )), s;

    if (nu != zero){
	if(u[0] < 0){
		s= -1;
	} else {
		s= 1;
	}
	u[0]= u[0] + s * nu;
	u/= sqrt( dot(u, u) );
    }

    return u;
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_HOUSEHOLDER_INCLUDE