// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.

/*
 *   File "ba_b_tapenade_generated.c" is generated by Tapenade 3.14 (r7259) from this file.
 *   To reproduce such a generation you can use Tapenade CLI
 *   (can be downloaded from http://www-sop.inria.fr/tropics/tapenade/downloading.html)
 *
 *   After installing use the next command to generate a file:
 *
 *      tapenade -b -o ba_tapenade -head "compute_reproj_error(err)/(cam X) compute_zach_weight_error(err)/(w)" ba.c
 *
 *   This will produce a file "ba_tapenade_b.c" which content will be the same as the content of "ba_b_tapenade_generated.c",
 *   except one-line header. Moreover a log-file "ba_tapenade_b.msg" will be produced.
 *
 *   NOTE: the code in "ba_b_tapenade_generated.c" is wrong and won't work.
 *         REPAIRED SOURCE IS STORED IN THE FILE "ba_b.c".
 *         You can either use diff tool or read "ba_b.c" header to figure out what changes was performed to fix the code.
 *
 *   NOTE: you can also use Tapenade web server (http://tapenade.inria.fr:8080/tapenade/index.jsp)
 *         for generating but the result can be slightly different.
 */

#include "../adbench/ba.h"

extern "C" {

#include "ba.h"

/* ===================================================================== */
/*                                UTILS                                  */
/* ===================================================================== */

double sqsum(int n, double const* x)
{
    int i;
    double res = 0;
    for (i = 0; i < n; i++)
    {
        res = res + x[i] * x[i];
    }

    return res;
}

void cross(double const* a, double const* b, double* out)
{
    out[0] = a[1] * b[2] - a[2] * b[1];
    out[1] = a[2] * b[0] - a[0] * b[2];
    out[2] = a[0] * b[1] - a[1] * b[0];
}

/* ===================================================================== */
/*                               MAIN LOGIC                              */
/* ===================================================================== */

// rot: 3 rotation parameters
// pt: 3 point to be rotated
// rotatedPt: 3 rotated point
// this is an efficient evaluation (part of
// the Ceres implementation)
// easy to understand calculation in matlab:
//  theta = sqrt(sum(w. ^ 2));
//  n = w / theta;
//  n_x = au_cross_matrix(n);
//  R = eye(3) + n_x*sin(theta) + n_x*n_x*(1 - cos(theta));
void rodrigues_rotate_point(double const* __restrict rot, double const* __restrict pt, double *__restrict rotatedPt)
{
    int i;
    double sqtheta = sqsum(3, rot);
    if (sqtheta != 0)
    {
        double theta, costheta, sintheta, theta_inverse;
        double w[3], w_cross_pt[3], tmp;

        theta = sqrt(sqtheta);
        costheta = cos(theta);
        sintheta = sin(theta);
        theta_inverse = 1.0 / theta;

        for (i = 0; i < 3; i++)
        {
            w[i] = rot[i] * theta_inverse;
        }

        cross(w, pt, w_cross_pt);

        tmp = (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) *
            (1. - costheta);

        for (i = 0; i < 3; i++)
        {
            rotatedPt[i] = pt[i] * costheta + w_cross_pt[i] * sintheta + w[i] * tmp;
        }
    }
    else
    {
        double rot_cross_pt[3];
        cross(rot, pt, rot_cross_pt);

        for (i = 0; i < 3; i++)
        {
            rotatedPt[i] = pt[i] + rot_cross_pt[i];
        }
    }
}

void radial_distort(double const* rad_params, double *proj)
{
    double rsq, L;
    rsq = sqsum(2, proj);
    L = 1. + rad_params[0] * rsq + rad_params[1] * rsq * rsq;
    proj[0] = proj[0] * L;
    proj[1] = proj[1] * L;
}

void project(double const* __restrict cam, double const* __restrict X, double* __restrict proj)
{
    double const* C = &cam[3];
    double Xo[3], Xcam[3];

    Xo[0] = X[0] - C[0];
    Xo[1] = X[1] - C[1];
    Xo[2] = X[2] - C[2];

    rodrigues_rotate_point(&cam[0], Xo, Xcam);

    proj[0] = Xcam[0] / Xcam[2];
    proj[1] = Xcam[1] / Xcam[2];

    radial_distort(&cam[9], proj);

    proj[0] = proj[0] * cam[6] + cam[7];
    proj[1] = proj[1] * cam[6] + cam[8];
}

// cam: 11 camera in format [r1 r2 r3 C1 C2 C3 f u0 v0 k1 k2]
//            r1, r2, r3 are angle - axis rotation parameters(Rodrigues)
//            [C1 C2 C3]' is the camera center
//            f is the focal length in pixels
//            [u0 v0]' is the principal point
//            k1, k2 are radial distortion parameters
// X: 3 point
// feats: 2 feature (x,y coordinates)
// reproj_err: 2
// projection function:
// Xcam = R * (X - C)
// distorted = radial_distort(projective2euclidean(Xcam), radial_parameters)
// proj = distorted * f + principal_point
// err = sqsum(proj - measurement)
void compute_reproj_error(
    double const* __restrict cam,
    double const* __restrict X,
    double const* __restrict w,
    double const* __restrict feat,
    double * __restrict err
)
{
    double proj[2];
    project(cam, X, proj);

    err[0] = (*w)*(proj[0] - feat[0]);
    err[1] = (*w)*(proj[1] - feat[1]);
}

void compute_zach_weight_error(double const* w, double* err)
{
    *err = 1 - (*w)*(*w);
}

// n number of cameras
// m number of points
// p number of observations
// cams: 11*n cameras in format [r1 r2 r3 C1 C2 C3 f u0 v0 k1 k2]
//            r1, r2, r3 are angle - axis rotation parameters(Rodrigues)
//            [C1 C2 C3]' is the camera center
//            f is the focal length in pixels
//            [u0 v0]' is the principal point
//            k1, k2 are radial distortion parameters
// X: 3*m points
// obs: 2*p observations (pairs cameraIdx, pointIdx)
// feats: 2*p features (x,y coordinates corresponding to observations)
// reproj_err: 2*p errors of observations
// w_err: p weight "error" terms
void ba_objective(
    int n,
    int m,
    int p,
    double const* cams,
    double const* X,
    double const* w,
    int const* obs,
    double const* feats,
    double* reproj_err,
    double* w_err
)
{
    int i;
    for (i = 0; i < p; i++)
    {
        int camIdx = obs[i * 2 + 0];
        int ptIdx = obs[i * 2 + 1];
        compute_reproj_error(
            &cams[camIdx * BA_NCAMPARAMS],
            &X[ptIdx * 3],
            &w[i],
            &feats[i * 2],
            &reproj_err[2 * i]
        );
    }

    for (i = 0; i < p; i++)
    {
        compute_zach_weight_error(&w[i], &w_err[i]);
    }
}

extern int enzyme_const;
extern int enzyme_dup;
extern int enzyme_dupnoneed;
void __enzyme_autodiff(...) noexcept;

void dcompute_reproj_error(
    double const* cam,
    double * dcam,
    double const* X,
    double * dX,
    double const* w,
    double * wb,
    double const* feat,
    double *err,
    double *derr
)
{
    __enzyme_autodiff(compute_reproj_error,
            enzyme_dup, cam, dcam,
            enzyme_dup, X, dX,
            enzyme_dup, w, wb,
            enzyme_const, feat,
            enzyme_dupnoneed, err, derr);
}

void dcompute_zach_weight_error(double const* w, double* dw, double* err, double* derr) {
    __enzyme_autodiff(compute_zach_weight_error,
            enzyme_dup, w, dw,
            enzyme_dupnoneed, err, derr);
}

}

//! Tapenade
extern "C" {

#include <adBuffer.h>

/*
  Differentiation of sqsum in reverse (adjoint) mode:
   gradient     of useful results: *x sqsum
   with respect to varying inputs: *x
   Plus diff mem management of: x:in

 =====================================================================
                                UTILS
 ===================================================================== */
void sqsum_b(int n, const double *x, double *xb, double sqsumb) {
    int i;
    double res = 0;
    double resb = 0.0;
    double sqsum;
    resb = sqsumb;
    for (i = n-1; i > -1; --i)
        xb[i] = xb[i] + 2*x[i]*resb;
}

/* =====================================================================
                                UTILS
 ===================================================================== */
double sqsum_nodiff(int n, const double *x) {
    int i;
    double res = 0;
    for (i = 0; i < n; ++i)
        res = res + x[i]*x[i];
    return res;
}

/*
  Differentiation of cross in reverse (adjoint) mode:
   gradient     of useful results: *out *a *b
   with respect to varying inputs: *a *b
   Plus diff mem management of: out:in a:in b:in
*/
void cross_b(const double *a, double *ab, const double *b, double *bb, double
        *out, double *outb) {
    ab[0] = ab[0] + b[1]*outb[2];
    bb[1] = bb[1] + a[0]*outb[2];
    ab[1] = ab[1] - b[0]*outb[2];
    bb[0] = bb[0] - a[1]*outb[2];
    outb[2] = 0.0;
    ab[2] = ab[2] + b[0]*outb[1];
    bb[0] = bb[0] + a[2]*outb[1];
    ab[0] = ab[0] - b[2]*outb[1];
    bb[2] = bb[2] - a[0]*outb[1];
    outb[1] = 0.0;
    ab[1] = ab[1] + b[2]*outb[0];
    bb[2] = bb[2] + a[1]*outb[0];
    ab[2] = ab[2] - b[1]*outb[0];
    bb[1] = bb[1] - a[2]*outb[0];
}

void cross_nodiff(const double *a, const double *b, double *out) {
    out[0] = a[1]*b[2] - a[2]*b[1];
    out[1] = a[2]*b[0] - a[0]*b[2];
    out[2] = a[0]*b[1] - a[1]*b[0];
}

/*
  Differentiation of rodrigues_rotate_point in reverse (adjoint) mode:
   gradient     of useful results: *rot *rotatedPt
   with respect to varying inputs: *rot *pt
   Plus diff mem management of: rot:in rotatedPt:in pt:in

 =====================================================================
                               MAIN LOGIC
 ===================================================================== */
// rot: 3 rotation parameters
// pt: 3 point to be rotated
// rotatedPt: 3 rotated point
// this is an efficient evaluation (part of
// the Ceres implementation)
// easy to understand calculation in matlab:
//  theta = sqrt(sum(w. ^ 2));
//  n = w / theta;
//  n_x = au_cross_matrix(n);
//  R = eye(3) + n_x*sin(theta) + n_x*n_x*(1 - cos(theta));
void rodrigues_rotate_point_b(const double *rot, double *rotb, const double *
        pt, double *ptb, double *rotatedPt, double *rotatedPtb) {
    int i;
    double sqtheta;
    double sqthetab;
    int ii1;
    sqtheta = sqsum_nodiff(3, rot);
    if (sqtheta != 0) {
        double theta, costheta, sintheta, theta_inverse;
        double w[3], w_cross_pt[3], tmp;
        double tempb;
        theta = sqrt(sqtheta);
        costheta = cos(theta);
        sintheta = sin(theta);
        theta_inverse = 1.0/theta;
        double thetab, costhetab, sinthetab, theta_inverseb;
        double wb[3], w_cross_ptb[3], tmpb;
        for (i = 0; i < 3; ++i)
            w[i] = rot[i]*theta_inverse;
        cross_nodiff(w, pt, w_cross_pt);
        tmp = (w[0]*pt[0]+w[1]*pt[1]+w[2]*pt[2])*(1.-costheta);
        for (i = 0; i < 3; i++) /* TFIX */
            ptb[i] = 0.0;
        for (ii1 = 0; ii1 < 3; ++ii1)
            w_cross_ptb[ii1] = 0.0;
        for (ii1 = 0; ii1 < 3; ++ii1)
            wb[ii1] = 0.0;
        costhetab = 0.0;
        tmpb = 0.0;
        sinthetab = 0.0;
        for (i = 2; i > -1; --i) {
            ptb[i] = ptb[i] + costheta*rotatedPtb[i];
            costhetab = costhetab + pt[i]*rotatedPtb[i];
            w_cross_ptb[i] = w_cross_ptb[i] + sintheta*rotatedPtb[i];
            sinthetab = sinthetab + w_cross_pt[i]*rotatedPtb[i];
            wb[i] = wb[i] + tmp*rotatedPtb[i];
            tmpb = tmpb + w[i]*rotatedPtb[i];
            rotatedPtb[i] = 0.0;
        }
        tempb = (1.-costheta)*tmpb;
        wb[0] = wb[0] + pt[0]*tempb;
        ptb[0] = ptb[0] + w[0]*tempb;
        wb[1] = wb[1] + pt[1]*tempb;
        ptb[1] = ptb[1] + w[1]*tempb;
        wb[2] = wb[2] + pt[2]*tempb;
        ptb[2] = ptb[2] + w[2]*tempb;
        costhetab = costhetab - (w[0]*pt[0]+w[1]*pt[1]+w[2]*pt[2])*tmpb;
        cross_b(w, wb, pt, ptb, w_cross_pt, w_cross_ptb);
        theta_inverseb = 0.0;
        for (i = 2; i > -1; --i) {
            rotb[i] = rotb[i] + theta_inverse*wb[i];
            theta_inverseb = theta_inverseb + rot[i]*wb[i];
            wb[i] = 0.0;
        }
        thetab = cos(theta)*sinthetab - sin(theta)*costhetab - theta_inverseb/
            (theta*theta);
        if (sqtheta == 0.0)
            sqthetab = 0.0;
        else
            sqthetab = thetab/(2.0*sqrt(sqtheta));
    } else {
        {
          double rot_cross_pt[3];
          double rot_cross_ptb[3];
          for (i = 0; i < 3; i++) /* TFIX */
              ptb[i] = 0.0;
          for (ii1 = 0; ii1 < 3; ++ii1)
              rot_cross_ptb[ii1] = 0.0;
          for (i = 2; i > -1; --i) {
              ptb[i] = ptb[i] + rotatedPtb[i];
              rot_cross_ptb[i] = rot_cross_ptb[i] + rotatedPtb[i];
              rotatedPtb[i] = 0.0;
          }
          cross_b(rot, rotb, pt, ptb, rot_cross_pt, rot_cross_ptb);
        }
        sqthetab = 0.0;
    }
    sqsum_b(3, rot, rotb, sqthetab);
}

/* =====================================================================
                               MAIN LOGIC
 ===================================================================== */
// rot: 3 rotation parameters
// pt: 3 point to be rotated
// rotatedPt: 3 rotated point
// this is an efficient evaluation (part of
// the Ceres implementation)
// easy to understand calculation in matlab:
//  theta = sqrt(sum(w. ^ 2));
//  n = w / theta;
//  n_x = au_cross_matrix(n);
//  R = eye(3) + n_x*sin(theta) + n_x*n_x*(1 - cos(theta));
void rodrigues_rotate_point_nodiff(const double *rot, const double *pt, double
        *rotatedPt) {
    int i;
    double sqtheta;
    sqtheta = sqsum_nodiff(3, rot);
    if (sqtheta != 0) {
        double theta, costheta, sintheta, theta_inverse;
        double w[3], w_cross_pt[3], tmp;
        theta = sqrt(sqtheta);
        costheta = cos(theta);
        sintheta = sin(theta);
        theta_inverse = 1.0/theta;
        for (i = 0; i < 3; ++i)
            w[i] = rot[i]*theta_inverse;
        cross_nodiff(w, pt, w_cross_pt);
        tmp = (w[0]*pt[0]+w[1]*pt[1]+w[2]*pt[2])*(1.-costheta);
        for (i = 0; i < 3; ++i)
            rotatedPt[i] = pt[i]*costheta + w_cross_pt[i]*sintheta + w[i]*tmp;
    } else {
        double rot_cross_pt[3];
        cross_nodiff(rot, pt, rot_cross_pt);
        for (i = 0; i < 3; ++i)
            rotatedPt[i] = pt[i] + rot_cross_pt[i];
    }
}

/*
  Differentiation of radial_distort in reverse (adjoint) mode:
   gradient     of useful results: *rad_params *proj
   with respect to varying inputs: *rad_params *proj
   Plus diff mem management of: rad_params:in proj:in
*/
void radial_distort_b(const double *rad_params, double *rad_paramsb, double *
        proj, double *projb) {
    double rsq, L;
    double rsqb, Lb;
    rsq = sqsum_nodiff(2, proj);
    L = 1. + rad_params[0]*rsq + rad_params[1]*rsq*rsq;
    pushReal8(proj[0]);
    proj[0] = proj[0]*L;
    Lb = proj[1]*projb[1];
    projb[1] = L*projb[1];
    popReal8(&(proj[0]));
    Lb = Lb + proj[0]*projb[0];
    projb[0] = L*projb[0];
    rad_paramsb[0] = rad_paramsb[0] + rsq*Lb;
    rsqb = (rad_params[1]*2*rsq+rad_params[0])*Lb;
    rad_paramsb[1] = rad_paramsb[1] + rsq*rsq*Lb;
    sqsum_b(2, proj, projb, rsqb);
}

void radial_distort_nodiff(const double *rad_params, double *proj) {
    double rsq, L;
    rsq = sqsum_nodiff(2, proj);
    L = 1. + rad_params[0]*rsq + rad_params[1]*rsq*rsq;
    proj[0] = proj[0]*L;
    proj[1] = proj[1]*L;
}

/*
  Differentiation of project in reverse (adjoint) mode:
   gradient     of useful results: *proj
   with respect to varying inputs: *cam *X
   Plus diff mem management of: cam:in X:in proj:in
*/
void project_b(const double *cam, double *camb, const double *X, double *Xb,
        double *proj, double *projb) {
    double *C = const_cast<double*>(&(cam[3]));
    double *Cb = const_cast<double*>(&(camb[3]));
    double Xo[3], Xcam[3];
    double Xob[3], Xcamb[3];
    int ii1;
    double tempb;
    double tempb0;
    for (ii1 = 0; ii1 < BA_NCAMPARAMS; ii1++) /* TFIX */
        camb[ii1] = 0.0;
    for (ii1 = 0; ii1 < 3; ii1++)
        Xb[ii1] = 0.0;
    Xo[0] = X[0] - C[0];
    Xo[1] = X[1] - C[1];
    Xo[2] = X[2] - C[2];
    rodrigues_rotate_point_nodiff(&(cam[0]), Xo, Xcam);
    proj[0] = Xcam[0]/Xcam[2];
    proj[1] = Xcam[1]/Xcam[2];
    pushReal8Array(proj, 2); /* TFIX */
    radial_distort_nodiff(&(cam[9]), proj);
    pushReal8(proj[0]);
    proj[0] = proj[0]*cam[6] + cam[7];
    camb[6] = camb[6] + proj[1]*projb[1];
    camb[8] = camb[8] + projb[1];
    projb[1] = cam[6]*projb[1];
    popReal8(&(proj[0]));
    camb[6] = camb[6] + proj[0]*projb[0];
    camb[7] = camb[7] + projb[0];
    projb[0] = cam[6]*projb[0];
    popReal8Array(proj, 2); /* TFIX */
    radial_distort_b(&(cam[9]), &(camb[9]), proj, projb);
    for (ii1 = 0; ii1 < 3; ++ii1)
        Xcamb[ii1] = 0.0;
    tempb = projb[1]/Xcam[2];
    Xcamb[1] = Xcamb[1] + tempb;
    Xcamb[2] = Xcamb[2] - Xcam[1]*tempb/Xcam[2];
    projb[1] = 0.0;
    tempb0 = projb[0]/Xcam[2];
    Xcamb[0] = Xcamb[0] + tempb0;
    Xcamb[2] = Xcamb[2] - Xcam[0]*tempb0/Xcam[2];
    rodrigues_rotate_point_b(&(cam[0]), &(camb[0]), Xo, Xob, Xcam, Xcamb);
    Xb[2] = Xb[2] + Xob[2];
    Cb[2] = Cb[2] - Xob[2];
    Xob[2] = 0.0;
    Xb[1] = Xb[1] + Xob[1];
    Cb[1] = Cb[1] - Xob[1];
    Xob[1] = 0.0;
    Xb[0] = Xb[0] + Xob[0];
    Cb[0] = Cb[0] - Xob[0];
}

void project_nodiff(const double *cam, const double *X, double *proj) {
    const double *C = &(cam[3]);
    double Xo[3], Xcam[3];
    Xo[0] = X[0] - C[0];
    Xo[1] = X[1] - C[1];
    Xo[2] = X[2] - C[2];
    rodrigues_rotate_point_nodiff(&(cam[0]), Xo, Xcam);
    proj[0] = Xcam[0]/Xcam[2];
    proj[1] = Xcam[1]/Xcam[2];
    radial_distort_nodiff(&(cam[9]), proj);
    proj[0] = proj[0]*cam[6] + cam[7];
    proj[1] = proj[1]*cam[6] + cam[8];
}

/*
  Differentiation of compute_reproj_error in reverse (adjoint) mode:
   gradient     of useful results: *err
   with respect to varying inputs: *err *w *cam *X
   RW status of diff variables: *err:in-out *w:out *cam:out *X:out
   Plus diff mem management of: err:in w:in cam:in X:in
*/
// cam: 11 camera in format [r1 r2 r3 C1 C2 C3 f u0 v0 k1 k2]
//            r1, r2, r3 are angle - axis rotation parameters(Rodrigues)
//            [C1 C2 C3]' is the camera center
//            f is the focal length in pixels
//            [u0 v0]' is the principal point
//            k1, k2 are radial distortion parameters
// X: 3 point
// feats: 2 feature (x,y coordinates)
// reproj_err: 2
// projection function:
// Xcam = R * (X - C)
// distorted = radial_distort(projective2euclidean(Xcam), radial_parameters)
// proj = distorted * f + principal_point
// err = sqsum(proj - measurement)
void compute_reproj_error_b(const double *cam, double *camb, const double *X,
        double *Xb, const double *w, double *wb, const double *feat, double *
        err, double *errb) {
    double proj[2];
    double projb[2];
    int ii1;
    pushReal8Array(proj, 2);
    project_nodiff(cam, X, proj);
    for (ii1 = 0; ii1 < 2; ++ii1)
        projb[ii1] = 0.0;
    *wb = (proj[1]-feat[1])*errb[1];
    projb[1] = projb[1] + (*w)*errb[1];
    errb[1] = 0.0;
    *wb = *wb + (proj[0]-feat[0])*errb[0];
    projb[0] = projb[0] + (*w)*errb[0];
    errb[0] = 0.0;
    popReal8Array(proj, 2);
    project_b(cam, camb, X, Xb, proj, projb);
}

/*
  Differentiation of compute_zach_weight_error in reverse (adjoint) mode:
   gradient     of useful results: *err
   with respect to varying inputs: *err *w
   RW status of diff variables: *err:in-out *w:out
   Plus diff mem management of: err:in w:in
*/
void compute_zach_weight_error_b(const double *w, double *wb, double *err,
        double *errb) {
    *wb = -(2*(*w)*(*errb));
    *errb = 0.0;
}

}

#include <adept_source.h>
#include <adept.h>
#include <adept_arrays.h>
using adept::adouble;
using adept::aVector;

namespace adeptTest {

template<typename T>
void cross(
    const T* const a,
    const T* const b,
    T* out)
{
    out[0] = a[1] * b[2] - a[2] * b[1];
    out[1] = a[2] * b[0] - a[0] * b[2];
    out[2] = a[0] * b[1] - a[1] * b[0];
}

////////////////////////////////////////////////////////////
//////////////////// Declarations //////////////////////////
////////////////////////////////////////////////////////////

// cam: 11 camera in format [r1 r2 r3 C1 C2 C3 f u0 v0 k1 k2]
//            r1, r2, r3 are angle - axis rotation parameters(Rodrigues)
//            [C1 C2 C3]' is the camera center
//            f is the focal length in pixels
//            [u0 v0]' is the principal point
//            k1, k2 are radial distortion parameters
// X: 3 point
// feats: 2 feature (x,y coordinates)
// reproj_err: 2
// projection function:
// Xcam = R * (X - C)
// distorted = radial_distort(projective2euclidean(Xcam), radial_parameters)
// proj = distorted * f + principal_point
// err = sqsum(proj - measurement)
template<typename T>
void computeReprojError(
    const T* const cam,
    const T* const X,
    const T* const w,
    const double* const feat,
    T *err);

// w: 1
// w_err: 1
template<typename T>
void computeZachWeightError(const T* const w, T* err);

// n number of cameras
// m number of points
// p number of observations
// cams: 11*n cameras in format [r1 r2 r3 C1 C2 C3 f u0 v0 k1 k2]
//            r1, r2, r3 are angle - axis rotation parameters(Rodrigues)
//            [C1 C2 C3]' is the camera center
//            f is the focal length in pixels
//            [u0 v0]' is the principal point
//            k1, k2 are radial distortion parameters
// X: 3*m points
// obs: 2*p observations (pairs cameraIdx, pointIdx)
// feats: 2*p features (x,y coordinates corresponding to observations)
// reproj_err: 2*p errors of observations
// w_err: p weight "error" terms
// projection function:
// Xcam = R * (X - C)
// distorted = radial_distort(projective2euclidean(Xcam), radial_parameters)
// proj = distorted * f + principal_point
// err = sqsum(proj - measurement)
template<typename T>
void ba_objective(int n, int m, int p,
    const T* const cams,
    const T* const X,
    const T* const w,
    const int* const obs,
    const double* const feats,
    T* reproj_err,
    T* w_err);

// rot: 3 rotation parameters
// pt: 3 point to be rotated
// rotatedPt: 3 rotated point
// this is an efficient evaluation (part of
// the Ceres implementation)
// easy to understand calculation in matlab:
//  theta = sqrt(sum(w. ^ 2));
//  n = w / theta;
//  n_x = au_cross_matrix(n);
//  R = eye(3) + n_x*sin(theta) + n_x*n_x*(1 - cos(theta));
template<typename T>
void rodrigues_rotate_point(
    const T* const rot,
    const T* const pt,
    T *rotatedPt);

////////////////////////////////////////////////////////////
//////////////////// Definitions ///////////////////////////
////////////////////////////////////////////////////////////

template<typename T>
T sqsum(int n, const T* const x)
{
    T res = 0;
    for (int i = 0; i < n; i++)
        res = res + x[i] * x[i];
    return res;
}

template<typename T>
void rodrigues_rotate_point(
    const T* const rot,
    const T* const pt,
    T *rotatedPt)
{
    T sqtheta = sqsum(3, rot);
    if (sqtheta != 0)
    {
        T theta, costheta, sintheta, theta_inverse,
            w[3], w_cross_pt[3], tmp;

        theta = sqrt(sqtheta);
        costheta = cos(theta);
        sintheta = sin(theta);
        theta_inverse = 1.0 / theta;

        for (int i = 0; i < 3; i++)
            w[i] = rot[i] * theta_inverse;

        cross(w, pt, w_cross_pt);

        tmp = (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) *
            (1. - costheta);

        for (int i = 0; i < 3; i++)
            rotatedPt[i] = pt[i] * costheta + w_cross_pt[i] * sintheta + w[i] * tmp;
    }
    else
    {
        T rot_cross_pt[3];
        cross(rot, pt, rot_cross_pt);

        for (int i = 0; i < 3; i++)
            rotatedPt[i] = pt[i] + rot_cross_pt[i];
    }
}

template<typename T>
void radial_distort(
    const T* const rad_params,
    T *proj)
{
    T rsq, L;
    rsq = sqsum(2, proj);
    L = 1. + rad_params[0] * rsq + rad_params[1] * rsq * rsq;
    proj[0] = proj[0] * L;
    proj[1] = proj[1] * L;
}

template<typename T>
void project(const T* const cam,
    const T* const X,
    T* proj)
{
    const T* const C = &cam[3];
    T Xo[3], Xcam[3];

    Xo[0] = X[0] - C[0];
    Xo[1] = X[1] - C[1];
    Xo[2] = X[2] - C[2];

    rodrigues_rotate_point(&cam[0], Xo, Xcam);

    proj[0] = Xcam[0] / Xcam[2];
    proj[1] = Xcam[1] / Xcam[2];

    radial_distort(&cam[9], proj);

    proj[0] = proj[0] * cam[6] + cam[7];
    proj[1] = proj[1] * cam[6] + cam[8];
}

template<typename T>
void computeReprojError(
    const T* const cam,
    const T* const X,
    const T* const w,
    const double* const feat,
    T *err)
{
    T proj[2];
    project(cam, X, proj);

    err[0] = (*w)*(proj[0] - feat[0]);
    err[1] = (*w)*(proj[1] - feat[1]);
}

template<typename T>
void computeZachWeightError(const T* const w, T* err)
{
    *err = 1 - (*w)*(*w);
}

template<typename T>
void ba_objective(int n, int m, int p,
    const T* const cams,
    const T* const X,
    const T* const w,
    const int* const obs,
    const double* const feats,
    T* reproj_err,
    T* w_err)
{
    for (int i = 0; i < p; i++)
    {
        int camIdx = obs[i * 2 + 0];
        int ptIdx = obs[i * 2 + 1];
        computeReprojError(&cams[camIdx * BA_NCAMPARAMS], &X[ptIdx * 3],
            &w[i], &feats[i * 2], &reproj_err[2 * i]);
    }

    for (int i = 0; i < p; i++)
    {
        computeZachWeightError(&w[i], &w_err[i]);
    }
}
};

void adept_compute_reproj_error(
    double const* cam,
    double * dcam,
    double const* X,
    double * dX,
    double const* w,
    double * wb,
    double const* feat,
    double *err,
    double *derr
)
{

    adept::Stack stack;

      adouble acam[BA_NCAMPARAMS];
      adept::set_values(acam, BA_NCAMPARAMS, cam);

      adouble aX[3];
      adept::set_values(aX, 3, X);

      adouble aw;
      aw.set_value(*w);

      adouble areproj_err[2];

      stack.new_recording();

      adeptTest::computeReprojError(acam, aX, &aw, feat, areproj_err);

      for(unsigned i=0; i<2; i++) areproj_err[i].set_gradient(derr[i]);

      stack.compute_adjoint();

      *wb = aw.get_gradient();
      adept::get_gradients(aX, 3, dX);
      adept::get_gradients(acam, BA_NCAMPARAMS, dcam);
}

void adept_compute_zach_weight_error(double const* w, double* dw, double* err, double* derr) {
    adept::Stack stack;

      adouble aw;
      aw.set_value(*w);

      adouble aw_err;

      stack.new_recording();
      adeptTest::computeZachWeightError(&aw, &aw_err);
      aw_err.set_gradient(1.);
      stack.compute_adjoint();

      *dw = aw.get_gradient();
}


#include <random>
#include <cmath>
#include <armadillo>

#define PERIODIC_UP(i, size) (((i) >= (size)) ? ((i) - (size)) : ((i)))
#define PERIODIC_DOWN(i, size) (((i) < 0) ? ((i) + (size)) : ((i)))

void metropolis_sampling(int num_spins, int num_cycles, double temperature,
        double *expectation_values)
{
    std::random_device rd;
    std::mt19937_64 generator(rd());

    std::uniform_real_distribution<double> distribution(0.0, 1.0);

    double energy = 0;
    double magnetic_moment = 0;
    double diff_zero = 8;
    double diff_step = 4;

    // Create spin matrix
    arma::mat spin_matrix = arma::zeros<arma::mat>(num_spins, num_spins);
    // Fill spin matrix with ones
    spin_matrix.ones();

    // Calculate initial energy for all spin up states
    expectation_values[0] = -2*arma::sum(arma::sum(spin_matrix));
    // Calculate initial magnetic moment
    expectation_values[1] = arma::sum(arma::sum(spin_matrix));

    arma::vec energy_difference = arma::zeros<arma::mat>(5);

    for (int i = 0; i < 5; i++) {
        energy_difference(i) = exp(-(-diff_zero + i*diff_step)/temperature);
    }

    for (int cycle = 0; cycle < num_cycles; cycle++) {
        for (int i = 0; i < num_spins; i++) {
            for (int j = 0; j < num_spins; j++) {
                int ix = (int) (distribution(generator) * num_spins);
                int iy = (int) (distribution(generator) * num_spins);

                int high_ix = PERIODIC_UP(ix + 1, num_spins);
                int low_ix = PERIODIC_DOWN(ix - 1, num_spins);
                int high_iy = PERIODIC_UP(iy + 1, num_spins);
                int low_iy = PERIODIC_DOWN(iy - 1, num_spins);

                int delta_energy = 2*spin_matrix(ix, iy)
                    * (spin_matrix(ix, low_iy) + spin_matrix(ix, high_iy)
                       + spin_matrix(low_ix, iy) + spin_matrix(high_ix, iy));

                int diff_index = (delta_energy + diff_zero)/diff_step;
                if (distribution(generator) <= energy_difference(diff_index)) {
                    spin_matrix(ix, iy) *= -1.0;
                    magnetic_moment += 2*spin_matrix(ix, iy);
                    energy += delta_energy;
                }
            }
        }
        expectation_values[0] += energy;
        expectation_values[1] += magnetic_moment;
    }
}


// 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_EIGENVALUE_SYMMETRIC_INCLUDE
#define MTL_MATRIX_EIGENVALUE_SYMMETRIC_INCLUDE

#include <cmath>
#include <boost/utility.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/make_copy_or_reference.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/operation/diagonal.hpp>
#include <boost/numeric/mtl/operation/givens.hpp>
#include <boost/numeric/mtl/operation/hessenberg.hpp>
#include <boost/numeric/mtl/operation/householder.hpp>
#include <boost/numeric/mtl/operation/qr.hpp>
#include <boost/numeric/mtl/operation/rank_one_update.hpp>
#include <boost/numeric/mtl/operation/signum.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>

#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

/// Eigenvalues of symmetric matrix A with implicit QR algorithm
// Return Diagonalmatrix with eigenvalues as diag(A)
template <typename Matrix>
mtl::vector::dense_vector<typename Collection<Matrix>::value_type, vector::parameters<> >
inline qr_sym_imp(const Matrix& A)
{
    vampir_trace<5010> tracer;
    using std::abs; using mtl::signum; using mtl::real;
    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Magnitude<value_type>::type      magnitude_type; // to multiply with 2 not 2+0i
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A), N;
    value_type       zero= math::zero(A[0][0]), h00, h10, h11, beta, mu, a, b, tol;
    const magnitude_type two(2);
    Matrix           Q(nrows,ncols), H(nrows,ncols),  G(2,2);

    tol= 1.0e-8; // ????evtl ein Iterator wie bei den Gleichungssystemen, Problem: Keine Rechte Seite bzw b

    MTL_THROW_IF(ncols != nrows , matrix_not_square());

    // Hessenberg_form of Matrix A
    H= hessenberg(A);
    N= nrows;

    // QR_algo with implizit sym QR-step from Wilkinson
    while (1) {
	h00= H[N-2][N-2];
	h10= H[N-1][N-2];
	h11= H[N-1][N-1];

	//reduction, residuum and watch for breakdown
	if(abs(h10) < tol * abs(h11 + h00))
	    N--;
	if (N < 2)
	    break;

	// Wilkinson_shift
	beta= (h00 - h11) / two;
	mu = h11 + (beta != zero ? beta - signum(beta) * sqrt(beta * beta + h10 * h10) : -h10);
	a= H[0][0] - mu, b= H[1][0];

	//implizit QR-step
	for (size_type k = 0; k < N - 1; k++) {
	    givens<Matrix>(H, a, b).trafo(k);
	    if (k < N - 2)
		a= H[k+1][k], b= H[k+2][k];
	}
    }
    return diagonal(H);
}

/// Evaluation of eigenvalues with QR-Algorithm of matrix A
// Return Diagonalmatrix with eigenvalues as diag(A)
template <typename Matrix>
mtl::vector::dense_vector<typename Collection<Matrix>::value_type, vector::parameters<> >
inline qr_algo(const Matrix& A, typename Collection<Matrix>::size_type itMax)
{
    vampir_trace<5011> tracer;
    typedef typename Collection<Matrix>::size_type    size_type;
    size_type        ncols = num_cols(A), nrows = num_rows(A);
    Matrix           Q(nrows, ncols), H(nrows, ncols), R(nrows, ncols);

    MTL_THROW_IF(ncols != nrows , matrix_not_square());

    H= hessenberg(A);
    for (size_type i = 0; i < itMax; i++) {
	boost::tie(Q, R)= qr_factors(H);
	H= R * Q;
    }
    return diagonal(H);
}

# ifdef MTL_SYMMETRIC_EIGENVALUE_WITH_QR

/// Calculation of eigenvalues of symmetric matrix A
template <typename Matrix>
mtl::vector::dense_vector<typename Collection<Matrix>::value_type, vector::parameters<> >
inline eigenvalue_symmetric(const Matrix& A, typename Collection<Matrix>::size_type itMax)
{
    return qr_algo(A, itMax == 0 ? num_rows(A) : itMax);
}

#else

/// Calculation of eigenvalues of symmetric matrix A
template <typename Matrix>
mtl::vector::dense_vector<typename Collection<Matrix>::value_type, vector::parameters<> >
inline eigenvalue_symmetric(const Matrix& A, typename Collection<Matrix>::size_type)
{
    typedef dense2D<typename Collection<Matrix>::value_type, parameters<> >    arg_type;
    make_in_copy_or_reference<arg_type, Matrix>  copy_or_ref(A);
    return qr_sym_imp(copy_or_ref.value);
}

#endif

#if 0 // Too nasty to get it through all warnings :-!

/// Calculation of eigenvalues of symmetric matrix A
template <typename Matrix>
mtl::vector::dense_vector<typename Collection<Matrix>::value_type, vector::parameters<> >
inline eigenvalue_symmetric(const Matrix& A,
			    typename Collection<Matrix>::size_type itMax= 0)
{
    vampir_trace<5012> tracer;
# ifdef MTL_SYMMETRIC_EIGENVALUE_WITH_QR
    return qr_algo(A, itMax == 0 ? num_rows(A) : itMax);
# else
    itMax= 0; // for not yelling at unused variable
    // qr_sym_imp works only with dense matrices of dynamic size, for other types copy
    typedef dense2D<typename Collection<Matrix>::value_type>    arg_type;
    make_in_copy_or_reference<arg_type, Matrix>  copy_or_ref(A);
    return qr_sym_imp(copy_or_ref.value);
# endif
}

#endif

template <typename Matrix>
mtl::vector::dense_vector<typename Collection<Matrix>::value_type, vector::parameters<> >
inline eigenvalue_symmetric(const Matrix& A)
{
    return eigenvalue_symmetric(A, 0);
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_EIGENVALUE_SYMMETRIC_INCLUDE


#include <cmath>
#include <string>
#include <coodinate_system.hpp>
#include <tle.hpp>
#include <orbit.hpp>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "mathematic_utils.hpp"

using namespace Eigen;

// 地心直交座標系で、
//  射点位置(P0)
//  衛星位置(P1)
//  軌道接平面法線(N)
// から、
//  楕円体(E(O,R)) 面上の着弾点を得る
Vector3dSet find_impact_point(
    const Vector3d& P0, const Vector3d& P1, const Vector3d& N,
    const Vector3d& O, const Vector3d& R)
{
  Vector3d v1 = P1 - P0;                   // 発射ビームの向き
  Vector3d v2 = reflect(v1,N);             // 反射ビームの向き
  Vector3dSet Q = intersection(P1,v2,O,R); // 着弾点候補
  Vector3dSet X = neighoring(P1,Q);        // 着弾点候補のうちP1に近い点

  return X;
}

// 測地座標系で、
//  射点位置(P)
//  使う人工衛星のTLE
//  現在時刻 t
// から、地上の着弾点を得る
bool find_impact(const geodetic& P, geodetic* X,
                 const std::string& tle_str, const time_t* t)
{
  // TLE読み込み
  TLE tle;
  std::string tle_str1 = tle_str.substr(0,69);
  std::string tle_str2 = tle_str.substr(69,69);
  tle.set(tle_str1,tle_str2);

  // 地球自転角
  double Pg = 2. * M_PI * greenwich_sidereal_time(t) / 24.;

  // 地心直交座標系での射点座標
  rectangular Pr = P.toRectangular(Pg);
  Eigen::Vector3d P0(Pr.X, Pr.Y, Pr.Z);

  // 軌道
  orbit orb;
  orb.setTLE(&tle);
  double since_day = orb.elapsed_day(t);
  double since_min = since_day * 1440.;

  double motion = tle.motion; // revolutions per day
  double dayp6  = 1. / (6. * motion); // day for 1/6 revolution.
  double minp6  = dayp6 * 1440.;

  // 人工衛星の所在地
  double position0[3],position1[3],position2[3];
  double velocity0[3],velocity1[3],velocity2[3];
  orb.sgp(position0,velocity0,since_min - minp6);
  orb.sgp(position1,velocity1,since_min);
  orb.sgp(position2,velocity2,since_min + minp6);

  Eigen::Vector3d p0(position0);
  Eigen::Vector3d p1(position1); // 人工衛星の現在地の位置ベクトル
  Eigen::Vector3d p2(position2);
  Eigen::Vector3d v1(velocity1);

  Eigen::Vector3d n0 = normalize(p0.cross(p2)); // 軌道面の法線ベクトル
  Eigen::Vector3d n1 = v1.cross(normalize(n0)); // 反射面の法線ベクトル

  Eigen::Vector3d O(0.,0.,0.);
  Eigen::Vector3d R(6378.137,6378.137,6356.752); // 地球楕円体
  Vector3dSet Q = find_impact_point(P0,p1,n1,O,R);

  rectangular Xr;
  bool f = (Q.size() != 0);
  if (f) {
    Eigen::Vector3d q = Q[0];
    Xr.X = q[0];
    Xr.Y = q[1];
    Xr.Z = q[2];
  } else {
    Xr.X = 0.0;
    Xr.Y = 0.0;
    Xr.Z = 0.0;
  }

  *X = Xr.toGeodetic(Pg); // 測地座標系に変換
  if      ( X->latitude  >  M_PI) X->latitude  -= 2.*M_PI;
  else if ( X->latitude  < -M_PI) X->latitude  += 2.*M_PI;
  if      ( X->longitude >  M_PI) X->longitude -= 2.*M_PI;
  else if ( X->longitude < -M_PI) X->longitude += 2.*M_PI;

  return f;
}


/**
 * @file MathIO.hpp
 * @author bwu
 * @brief I/O functions of math
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_MATH_MATHIO_HPP
#define GENERIC_MATH_MATHIO_HPP
#include "LinearAlgebra.hpp"
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>
#include <fstream>
#include <complex>
namespace {
using namespace generic::math;
using namespace generic::math::la;

///@brief out stream a vector
template <typename num_type, size_t N>
inline std::ostream & operator<< (std::ostream & os, const Vector<num_type, N> & v)
{
    return os << v.Data();
}

///@brief out stream a matrix
template <typename num_type, size_t M, size_t N>
inline std::ostream & operator<< (std::ostream & os, const Matrix<num_type, M, N> & m)
{
    return os << m.Data();
}
}

namespace generic {
namespace math {
namespace la {
using namespace boost::numeric::ublas;

class MatrixIO
{
public:
    template <typename num_type, bool zero_based = true>
    static bool ReadSparseMatrixComplex(const std::string & dia, const std::string & offDia, mapped_matrix<std::complex<num_type> > & m, std::string * err = nullptr)
    {
        std::ifstream in(dia);
        if(!in.is_open()) {
            if(err) *err = "Error: fail to open: " + dia;
            return false;
        }

        size_t i, j;
        num_type real, imag;
        size_t row = m.size1();
        size_t col = m.size2();
        while(!in.eof()){
            in >> i >> real >> imag;
            if(zero_based) m(i, i) = std::complex<num_type>(real, imag);
            else m(i - 1, i - 1) = std::complex<num_type>(real, imag);
        }

        in.close();
        in.open(offDia);
        if(!in.is_open()) {
            if(err) *err = "Error: fail to open: " + dia;
            return false;
        }

        while(!in.eof()){
            in >> i >> j >> real >> imag;
            if(zero_based) m(i, j) = std::complex<num_type>(real, imag);
            else m(i - 1, j - 1) = std::complex<num_type>(real, imag);
        }
        in.close();
        return true;
    }

    template <typename num_type, bool zero_based = true>
    static bool ReadSparseMatrix(const std::string & dia, const std::string & offDia, coordinate_matrix<num_type> & m, std::string * err = nullptr)
    {
        std::ifstream in(dia);
        if(!in.is_open()) {
            if(err) *err = "Error: fail to open: " + dia;
            return false;
        }

        size_t i, j;
        num_type real, imag;
        size_t row = m.size1();
        size_t col = m.size2();
        while(!in.eof()){
            in >> i >> real >> imag;
            if(zero_based) m(i, i) = real;
            else m(i - 1, i - 1) = real;
        }

        in.close();
        in.open(offDia);
        if(!in.is_open()) {
            if(err) *err = "Error: fail to open: " + dia;
            return false;
        }

        while(!in.eof()){
            in >> i >> j >> real >> imag;
            if(zero_based) m(i, j) = real;
            else m(i - 1, j - 1) = real;
        }
        in.close();
        return true;
    }
};
}//namespace la
}//namespace math
}//namespace generic
#endif//GENERIC_MATH_MATHIO_HPP


/*
 * File: dht.cc
 * Created Date: 2019-12-29
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Sunday December 29th 2019 12:11:36 pm
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#include "dht.hpp"

#include <Eigen/Dense>
#include <boost/math/special_functions/bessel.hpp>
#include <cmath>
#include <iostream>

using namespace Eigen;
using boost::math::cyl_bessel_j;
using boost::math::cyl_bessel_j_zero;
using std::pow;

DiscreteHankelTransform::DiscreteHankelTransform(int order, int nr)
    : order_(order), nr_(nr), roots_(nr_), tmatrix_(MatrixXd::Zero(nr_, nr_)) {
  for (int i = 0; i < nr_; ++i) {
    roots_(i) = cyl_bessel_j_zero(float(order_), i + 1);
  }
  for (int k = 0; k < nr_ - 1; ++k) {
    for (int m = 0; m < nr_ - 1; ++m) {
      tmatrix_(m, k) =
          2.0 / (roots_(nr_ - 1) * pow(cyl_bessel_j(order + 1, roots_(k)), 2)) *
          cyl_bessel_j(order, roots_(m) * roots_(k) / roots_(nr_ - 1));
    }
  }
}

DiscreteHankelTransform::~DiscreteHankelTransform() = default;

VectorXd DiscreteHankelTransform::r_sampling(double rmax) {
  rmax_ = rmax;
  VectorXd r = roots_ / rmax_ * roots_(nr_ - 1);
  return r;
}

VectorXd DiscreteHankelTransform::k_sampling(double rmax) {
  rmax_ = rmax;
  VectorXd k = roots_ / rmax_;
  return k;
}

VectorXd DiscreteHankelTransform::forward(const Ref<const VectorXd> &fr) {
  VectorXd fk = pow(rmax_, 2) / roots_(nr_ - 1) * (tmatrix_ * fr);
  return fk;
}

VectorXd DiscreteHankelTransform::backward(const Ref<const VectorXd> &fk) {
  VectorXd fr = roots_(nr_ - 1) / pow(rmax_, 2) * (tmatrix_ * fk);
  return fr;
}

VectorXd DiscreteHankelTransform::shift(const Ref<const VectorXd> &raw, int m) {
  VectorXd ret(VectorXd::Zero(nr_ - 1));
  for (int q = 0; q < nr_ - 1; ++q) {
    for (int p = 0; p < nr_ - 1; ++p) {
      for (int k = 0; k < nr_ - 1; ++k) {
        ret(q) += tmatrix_(m, k) * tmatrix_(k, q) * tmatrix_(k, p) * raw(p);
      }
    }
  }
  return ret;
}

MatrixXd DiscreteHankelTransform::tmatrix() const { return tmatrix_; }


#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main()
{
    // Ax=b
    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
        << A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
}


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

int main()
{
    using namespace boost;
    // ID numbers for the routers (vertices).
    enum
    {
        A,
        B,
        C,
        D,
        E,
        F,
        G,
        H,
        n_vertices
    };
    const int n_edges = 11;
    typedef std::pair< int, int > Edge;

    // The list of connections between routers stored in an array.
    Edge edges[] = { Edge(A, B), Edge(A, C), Edge(B, D), Edge(B, E), Edge(C, E),
        Edge(C, F), Edge(D, H), Edge(D, E), Edge(E, H), Edge(F, G),
        Edge(G, H) };

    // Specify the graph type and declare a graph object
    typedef edge_list< Edge*, Edge, std::ptrdiff_t,
        std::random_access_iterator_tag >
        Graph;
    Graph g(edges, edges + n_edges);

    // The transmission delay values for each edge.
    float delay[] = { 5.0, 1.0, 1.3, 3.0, 10.0, 2.0, 6.3, 0.4, 1.3, 1.2, 0.5 };

    // Declare some storage for some "external" vertex properties.
    char name[] = "ABCDEFGH";
    int parent[n_vertices];
    for (int i = 0; i < n_vertices; ++i)
        parent[i] = i;
    float distance[n_vertices];
    std::fill(
        distance, distance + n_vertices, (std::numeric_limits< float >::max)());
    // Specify A as the source vertex
    distance[A] = 0;

    bool r = bellman_ford_shortest_paths(g, int(n_vertices),
        weight_map(
            make_iterator_property_map(&delay[0], get(edge_index, g), delay[0]))
            .distance_map(&distance[0])
            .predecessor_map(&parent[0]));

    if (r)
        for (int i = 0; i < n_vertices; ++i)
            std::cout << name[i] << ": " << distance[i] << " "
                      << name[parent[i]] << std::endl;
    else
        std::cout << "negative cycle" << std::endl;

    return EXIT_SUCCESS;
}


#include <iostream>
#include <Eigen\Dense>

using namespace Eigen;
using namespace std;

void main() {
	{
		ArrayXXf m(2, 2);
		m(0, 0) = 1.0;
		m(0, 1) = 2.0;
		m(1, 0) = 3.0;
		m(1, 1) = m(0, 1) + m(1, 0);

		cout << m << endl;
		m << 1.0, 2.0, 3.0, 4.0;
		cout << m << endl;
	}

	{
		ArrayXXf a(3, 3);
		ArrayXXf b(3, 3);
		a << 1, 2, 3,
			4, 5, 6,
			7, 8, 9;
		b << 1, 2, 3,
			1, 2, 3,
			1, 2, 3;

		// Adding two arrays
		cout << "a + b = " << endl << a + b << endl << endl;
		// Subtracting a scalar from an array
		cout << "a - 2 = " << endl << a - 2 << endl;
	}

	{
		ArrayXXf a(2, 2);
		ArrayXXf b(2, 2);

		a << 1, 2, 3, 4;
		b << 5, 6, 7, 8;

		cout << "a * b = \n" << a * b << endl;
	}

	{
		// COEFFICIENT-WISE OPERATIONS
		cout << "COEFFICIENT-WISE OPERATIONS" << endl;
		ArrayXf a = ArrayXf::Random(5);
		a *= 2;
		cout << "a = " << endl << a << endl;
		cout << "a.abs() = " << endl << a.abs() << endl;
		cout << "a.abs().sqrt() = " << endl << a.abs().sqrt() << endl;
		cout << "a.min(a.abs().sqrt()) = " << endl << a.min(a.abs().sqrt()) << endl;

	}

	{
		MatrixXf m(2, 2);
		MatrixXf n(2, 2);
		MatrixXf result(2, 2);
		m << 1, 2,
			 3, 4;
		n << 5, 6,
			 7, 8;

		result = m * n;
		cout << "-- m : -- \n" << m << endl;
		cout << "-- n : -- \n" << n << endl;
		cout << "-- Matrix m * n : -- \n" << result << endl;
		result = m.array() * n.array();
		cout << "-- Matrix m.array() * n.array() : -- \n" << result << endl;
		result = m.cwiseProduct(n);
		cout << "-- m.cwiseProduct(n) : -- \n" << result << endl;
		result = m.array() + 4;
		cout << "-- Array m + 4: -- \n" << result << endl;
		result = (m.array() + 4).matrix() * m;
		cout << "-- Combo 1: (m.array() + 4).matrix() * m -- \n" << result << endl;
		result = (m.array() * n.array()).matrix() * m;
		cout << "-- Combo 2: (m.array() * n.array()).matrix() * m --\n" << result << endl;
	}

	system("pause");
}


/*hseqr.cpp
 */

#include<iostream>
#include<complex>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
//#include <boost/numeric/bindings/detail/complex_utils.hpp>
#include <boost/numeric/bindings/vector_view.hpp>
#include <boost/numeric/bindings/lapack/computational/hseqr.hpp>
#include <boost/numeric/bindings/lapack/computational/trevc.hpp>

using std::cout;
using std::endl;
using std::vector;
using std::complex;

namespace ublas =  boost::numeric::ublas;
namespace lapack =  boost::numeric::bindings::lapack;
namespace bindings =  boost::numeric::bindings;
namespace tag =  boost::numeric::bindings::tag;

void hseqr(int);
template <typename T>
void Hessenberg(ublas::matrix<T, ublas::column_major>&);
template <typename T>
void Hessenberg(ublas::matrix<complex<T>, ublas::column_major>&);

int main()
{
  cout << "I'm testing uBlas." << endl;

  int n = 5;
  hseqr(n);

}

void hseqr(int n)
{
  cout << "\nCalculating eigenvalues using LAPACK's hseqr." << endl;
  ublas::matrix<double, ublas::column_major> H(n,n);
  Hessenberg(H);
  cout << "\nUpper Hessenberg matrix H:\n" << H << endl;

  ublas::vector<complex<double> > values(n);
  ublas::matrix<double, ublas::column_major> Z(n,n);

  cout << "\nHSEQR for only eigenvalues." << endl;
  ublas::matrix<double, ublas::column_major> Z_dummy(1,1);
  lapack::hseqr('E', 'N', 1, n, H, values, Z_dummy);
  /*
  lapack::hseqr('E', 'N', 1, n, H,
      bindings::detail::real_part_view(values),
      bindings::detail::imag_part_view(values),
      Z_dummy);
  bindings::detail::interlace(values);
  */
  cout << "\nH:\n" << H << endl;
  cout << "\nvalues: " << values << endl;

  cout << "\nHSEQR for eigenvalues and Schur vectors." << endl;
  Hessenberg(H);
  cout << "H:\n" << H << endl;
  lapack::hseqr('S', 'I', 1, n, H, values, Z);
  /*
  lapack::hseqr('S', 'I', 1, n, H,
      bindings::detail::real_part_view(values),
      bindings::detail::imag_part_view(values),
      Z);
  bindings::detail::interlace(values);
  */
  cout << "\nH: " << H << endl;
  cout << "\nvalues: " << values << endl;
  cout << "\nZ: " << Z << endl;

  cout << "\n==================================" << endl;
  cout << "Recalculating original matrix..." << endl;
  ublas::matrix<double, ublas::column_major> cH(n,n);
  cH = ublas::prod(H, ublas::herm(Z));
  cH = ublas::prod(Z, cH);
  cout << "'New' original matrix:\n" << cH << endl;
  cout << "==================================" << endl;

  cout << "\nHSEQR for only eigenvalues.  Complex version" << endl;
  ublas::matrix<complex<double>, ublas::column_major> G(n,n);
  Hessenberg(G);
  cout << "\nG:\n" << G << endl;
  ublas::matrix<complex<double>, ublas::column_major> cZ_dummy(1,1);
  lapack::hseqr('E', 'N', 1, n, G, values, cZ_dummy);
  cout << "\nG:\n" << G << endl;
  cout << "\nvalues: " << values << endl;

  cout << "\nHSEQR for eigenvalues and Schur vectors." << endl;
  Hessenberg(G);
  cout << "G:\n" << G << endl;
  ublas::matrix<complex<double>, ublas::column_major> cZ(n,n);
  lapack::hseqr('S', 'I', 1, n, G, values, cZ);
  cout << "\nG:\n " << G << endl;
  cout << "\nvalues: " << values << endl;
  cout << "\nZ:\n " << Z << endl;

  cout << "\n==================================" << endl;
  cout << "Recalculating original matrix..." << endl;
  ublas::matrix<complex<double>, ublas::column_major> origG(G);
  origG = ublas::prod(G, ublas::herm(cZ));
  origG = ublas::prod(cZ, origG);
  cout << "'New' original matrix:\n" << origG << endl;
  cout << "==================================" << endl;

  ublas::matrix<complex<double>, ublas::column_major> cVL(cZ);
  ublas::matrix<complex<double>, ublas::column_major> cVR(cZ);
  boost::numeric::bindings::detail::array<complex<double> > work_c(2*n);
  boost::numeric::bindings::detail::array<double> work_r(n);
  ublas::vector<fortran_bool_t> select_dummy(n);
  fortran_int_t m_info(n+1);
  lapack::trevc(tag::both(),'B',select_dummy,G,cVL,cVR,n,m_info,lapack::workspace(work_c,work_r));

  cout << "\n==================================" << endl;
  cout << "Testing left & right eigenvectors..." << endl;
  Hessenberg(G);
  cout << "Many 'zeros':" << endl;
  for(int i=0; i<n; ++i)
  {
    cout << (ublas::prod(G, column(cVR, i)) - values(i) * column(cVR, i)) << endl;
    cout << (ublas::prod(ublas::herm(G), column(cVL, i)) - conj(values(i)) * column(cVL, i)) << endl;
  }
  cout << "==================================" << endl;

  cout << "\n==================================" << endl;
  cout << "Verifying diagonal matrix..." << endl;
  ublas::matrix<complex<double>, ublas::column_major> cG(n,n,0);
  Hessenberg(G);
  G = ublas::prod(G, cVR);
  G = ublas::prod(ublas::herm(cVL), G);
  cout << "'diagonal' matrix:\n" << G << endl;
  cout << "==================================" << endl;

}

template <typename T>
void Hessenberg(ublas::matrix<T, ublas::column_major>& H)
{
  T k = 1;
  for(unsigned int i = 0; i < H.size1(); ++i)
  {
    for(unsigned int j = i; j <= H.size2(); ++j)
    {
      if(j > 0)
      {
        H(i,j-1) = k;
        k += 1;
      }
    }
  }
}

template <typename T>
void Hessenberg(ublas::matrix<complex<T>, ublas::column_major>& H)
{
  T k = 1.0;
  for(unsigned int i = 0; i < H.size1(); ++i)
  {
    for(unsigned int j = i; j <= H.size2(); ++j)
    {
      if(j > 0)
      {
        T real = k++;
        T imag = k++;
        H(i,j-1) = complex<T>(real, imag);
      }
    }
  }
}


#include "gfunc.h"
#include <algorithm>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/variant.hpp>

namespace bg = boost::geometry;

typedef bg::model::d2::point_xy<double> DPoint;
typedef bg::model::segment<DPoint> DSegment;

std::vector<double>
intersection_seg_arc(double xc, double yc, double rr, double x0, double y0, double x1, double y1) {
    double t1, t2;
    std::vector<double> res;
    double h = xc;
    double k = yc;
    double r = rr;
    double a = (x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0);
    double b = 2 * (x1 - x0) * ((x0 - h)) + 2 * (y1 - y0) * (y0 - k);
    double c = (x0 - h) * (x0 - h) + (y0 - k) * (y0 - k) - r * r;
    if (b * b - 4 * a * c < 0) {
        return res;
    } else {
        t1 = (-b + sqrt(b * b - 4 * a * c)) / (2 * a);
        t2 = (-b - sqrt(b * b - 4 * a * c)) / (2 * a);
    }
    if (t1 >= 0 && t1 <= 1) {

        res.push_back((x1 - x0) * t1 + x0);
        res.push_back((y1 - y0) * t1 + y0);
        // res[0] = (x1 - x0) * t1 + x0;
        // res[1] = (y1 - y0) * t1 + y0;
    } else if (t2 >= 0 && t2 <= 1) {
        res.push_back((x1 - x0) * t2 + x0);
        res.push_back((y1 - y0) * t2 + y0);
        // res[0] = ((x1 - x0) * t2 + x0);
        // res[1] = ((y1 - y0) * t2 + y0);
    } else {
        // 无交点
        return res;
    }
    return res;
}

//std::vector<double> intersection_seg_seg(double *&st0, double *&ed0, double *&st1, double *&ed1) {
//    std::vector<double> res;
//    DPoint pt00(st0[0],st0[1]);
//    DPoint pt01(ed0[0],ed0[1]);
//    DPoint pt10(st1[0],st1[1]);
//    DPoint pt11(ed1[0],ed1[1]);
//    DSegment sg0(pt00,pt01);
//    DSegment sg1(pt10,pt11);
//
//    std::list<DPoint> lstPoints;
//
//    if (bg::intersects(sg0, sg1)){
//        bg::intersection(sg0, sg1, lstPoints);
//        res.push_back(lstPoints.begin()->x());
//        res.push_back(lstPoints.begin()->y());
//    }
//    return res;
//}

std::vector<double> intersection_seg_seg(py::list st0, py::list ed0, py::list st1, py::list ed1) {
    std::vector<double> res;
    DPoint pt00(st0[0].cast<double>(), st0[1].cast<double>());
    DPoint pt01(ed0[0].cast<double>(), ed0[1].cast<double>());
    DPoint pt10(st1[0].cast<double>(), st1[1].cast<double>());
    DPoint pt11(ed1[0].cast<double>(), ed1[1].cast<double>());
    DSegment sg0(pt00, pt01);
    DSegment sg1(pt10, pt11);

    std::list<DPoint> lstPoints;

    if (bg::intersects(sg0, sg1)) {
        bg::intersection(sg0, sg1, lstPoints);
        res.push_back(lstPoints.begin()->x());
        res.push_back(lstPoints.begin()->y());
    }
    return res;
}


#ifndef MLT_MODELS_TRANSFORMERS_PRINCIPAL_COMPONENTS_ANALYSIS_IMPL_HPP
#define MLT_MODELS_TRANSFORMERS_PRINCIPAL_COMPONENTS_ANALYSIS_IMPL_HPP

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

#include "transformer.hpp"

namespace mlt {
namespace models {
namespace transformers {
	template<class ConcreteType>
	class PrincipalComponentsAnalysisImpl : public Transformer<ConcreteType> {
	public:
		inline auto components_size() const { assert(_fitted); return _components_size; }

		inline const auto components() const { assert(_fitted); return _components; }

		inline const auto explained_variance_ratio() const { assert(_fitted); return _explained_variance_ratio; }

		inline const auto noise_variance() const { assert(_fitted); return _noise_variance; }

		Self& fit(Features input, bool = true) {
			assert(_components_size == -1 || _components_size <= input.cols());

			_mean = input.rowwise().mean();
			MatrixXd final = input.colwise() - _mean;

			auto svd = ((final * final.transpose()) / input.cols()).jacobiSvd(ComputeThinU);

			_explained_variance = svd.singularValues();
			_explained_variance_ratio = _explained_variance / _explained_variance.sum();

			if (_components_size < 1 && _variance_to_retain < 0) {
				_components_size = input.rows();
			} else if (_components_size < 1 && _variance_to_retain > 0) {
				double acum = 0;
				size_t i = 0;
				while (i < _explained_variance_ratio.rows() && acum < _variance_to_retain) {
					acum += _explained_variance_ratio(i);
					i++;
				}
				_components_size = i + 1;
			}

			if (_components_size > svd.matrixU().cols()) {
				_components_size = _components.cols();
			}

			if (_components_size < std::min(input.rows(), input.cols())) {
				_noise_variance = _explained_variance.tail(_explained_variance.size() - _components_size).mean();
			} else {
				_noise_variance = 0;
			}

			_components = svd.matrixU().leftCols(_components_size);
			_explained_variance = _explained_variance.head(_components_size);
			_explained_variance_ratio = _explained_variance_ratio.head(_components_size);

			_fitted = true;

			return _self();
		}

		Result transform(Features input) const {
			assert(_fitted);

			if (_whiten) {
				auto transformed = (_components.transpose() * (input.colwise() - _mean));
				return transformed.array().colwise() * _explained_variance.cwiseSqrt().cwiseInverse().array();
			}

			return _components.transpose() * (input.colwise() - _mean);
		}

		Features inverse_transform(Result input) const {
			assert(_fitted);

			if (_whiten) {
				return (_components * (input.array().colwise() *_explained_variance.cwiseSqrt().array()).matrix()).colwise() + _mean;
			}

			return (_components * input).colwise() + _mean;
		}

	protected:
		explicit PrincipalComponentsAnalysisImpl(int components_size, bool whiten = false) : _components_size(components_size), _whiten(whiten) {}

		explicit PrincipalComponentsAnalysisImpl(double variance_to_retain, bool whiten = false) : _variance_to_retain(variance_to_retain), _whiten(whiten) {
			assert(variance_to_retain > 0 && variance_to_retain <= 1);
		}

		explicit PrincipalComponentsAnalysisImpl(bool whiten = false) : _whiten(whiten) {}

		int _components_size = -1;
		double _variance_to_retain = -1;
		VectorXd _mean;
		MatrixXd _components;
		VectorXd _explained_variance;
		VectorXd _explained_variance_ratio;
		double _noise_variance = 0;
		bool _whiten;
	};
}
}
}
#endif


/*
Copyright 2020 Standard Cyborg

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

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

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

#include "standard_cyborg/algorithms/PrincipalAxes.hpp"
#include "standard_cyborg/algorithms/Centroid.hpp"

#include "standard_cyborg/sc3d/Geometry.hpp"
#include "standard_cyborg/math/Mat3x4.hpp"

#include "standard_cyborg/util/DataUtils.hpp"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#include <Eigen/Eigenvalues>
#pragma clang diagnostic pop

using standard_cyborg::sc3d::Geometry;
using standard_cyborg::math::Vec3;
using standard_cyborg::math::Mat3x4;

using standard_cyborg::sc3d::Face3;

namespace standard_cyborg {

namespace algorithms {

Mat3x4 computeNormalwisePrincipalAxes(const Geometry& geometry)
{
    using namespace math;

    Eigen::Matrix3f Moment;
    Moment.setZero();

    Vec3 areaVector{0.0f, 0.0f, 0.0f};
    Vec3 centroid{0.0f, 0.0f, 0.0f};
    float areaSum = 0.0f;

    int numFaces = geometry.faceCount();
    const std::vector<Face3>& faces = geometry.getFaces();
    const std::vector<Vec3>& positions = geometry.getPositions();

    for (int i = 0; i < numFaces; i++) {
        const Face3& face = faces[i];
        const Vec3& pA = positions[face[0]];
        const Vec3& pB = positions[face[1]];
        const Vec3& pC = positions[face[2]];

        Vec3 faceCentroid = (pA + pB + pC) * (1.0f / 3.0f);

        // Vectors along two of the edges
        Vec3 vBA = pB - pA;
        Vec3 vCA = pC - pA;

        // Face area vector (strictly speaking, twice the area, but since we use it as
        // a weight and also divide by twice the area, it's strictly equivalent.
        Vec3 dArea = Vec3::cross(vBA, vCA);

        // Integrate the differential surface element area vectors into a summed area
        // vectors. For a perfectly closed volume, this will sum to zero and won't work
        // very well, but if part of the model is open, it will provide stable orientation
        // where eigenvalues+eigenvectors don't.
        areaVector += dArea;

        // Add this face's contributions to accumulators
        float dAreaMagnitude = dArea.norm();
        centroid += faceCentroid * dAreaMagnitude;
        areaSum += dAreaMagnitude;

        Moment(0, 0) += dArea.x * dArea.x;
        Moment(1, 0) += dArea.x * dArea.y;
        Moment(2, 0) += dArea.x * dArea.z;

        //Moment(0, 1) += dArea.y * dArea.x;
        Moment(1, 1) += dArea.y * dArea.y;
        Moment(2, 1) += dArea.y * dArea.z;

        //Moment(0, 2) += dArea.z * dArea.x;
        //Moment(1, 2) += dArea.z * dArea.y;
        Moment(2, 2) += dArea.z * dArea.z;
    }

    centroid /= areaSum;

    // The eigen docs state these entries are not used at all. We could copy them, but
    // it has no effect.
    // Moment(0, 1) = Moment(1, 0);
    // Moment(0, 2) = Moment(2, 0);
    // Moment(1, 2) = Moment(2, 1);

    // Recall that if a self-adjoint matrix is real, then it's also symmetric, and the
    // eigenvalues of a real, symmetric matrix are real and also orthogonal.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Moment);

    auto eigenvectors = eigensolver.eigenvectors();
    auto eigenvalues = eigensolver.eigenvalues();

    Vec3 axis0 = toVec3(eigenvectors.col(0));
    Vec3 axis1 = toVec3(eigenvectors.col(1));

    // Eigenvectors are unique up to a sign, so we use the integrated area vector to
    // select an orientation stable with respect to small peturtbations. The first two
    // eigenvectors correspond to the strongest eigenvalues, so we flip their parity.
    // The third eigenvalue encodes the least amount of information, so we overwrite it
    // to enforce the correct parity.
    if (Vec3::dot(areaVector, axis0) < 0.0f) axis0 = -axis0;
    if (Vec3::dot(areaVector, axis1) < 0.0f) axis1 = -axis1;
    Vec3 axis2{Vec3::cross(axis0, axis1)};

    // Return the transform which moves a model at the origin *into* alignment with this
    // model. To align this model upright, you must invert this transform.
    return Mat3x4{
        axis0.x, axis1.x, axis2.x, centroid.x,
        axis0.y, axis1.y, axis2.y, centroid.y,
        axis0.z, axis1.z, axis2.z, centroid.z
    };
}

Mat3x4 computePointwisePrincipalAxes(const Geometry& geometry)
{
    using namespace math;

    Eigen::Matrix3f Moment;
    Moment.setZero();

    Vec3 areaVector{0.0f, 0.0f, 0.0f};
    Vec3 centroid(computeCentroid(geometry));

    int numFaces = geometry.faceCount();
    const std::vector<Face3>& faces = geometry.getFaces();
    const std::vector<Vec3>& positions = geometry.getPositions();

    for (int i = 0; i < numFaces; i++) {
        const Face3 face = faces[i];
        const Vec3 pA = positions[face[0]];
        const Vec3 pB = positions[face[1]];
        const Vec3 pC = positions[face[2]];

        Vec3 offset = (pA + pB + pC) * (1.0f / 3.0f) - centroid;

        // Face area vector (strictly speaking, twice the area, but since we use it as
        // a weight and also divide by twice the area, it's strictly equivalent.
        Vec3 dArea = 0.5 * Vec3::cross(pB - pA, pC - pA);

        // Integrate the differential surface element area vectors into a summed area
        // vectors. For a perfectly closed volume, this will sum to zero and won't work
        // very well, but if part of the model is open, it will provide stable orientation
        // where eigenvalues+eigenvectors don't.
        areaVector += dArea;

        // Weight each face by its area
        float weight = dArea.norm();

        Moment(0, 0) += offset.x * offset.x * weight;
        Moment(1, 0) += offset.x * offset.y * weight;
        Moment(2, 0) += offset.x * offset.z * weight;

        //Moment(0, 1) += offset.y * offset.x * weight;
        Moment(1, 1) += offset.y * offset.y * weight;
        Moment(2, 1) += offset.y * offset.z * weight;

        //Moment(0, 2) += offset.z * offset.x * weight;
        //Moment(1, 2) += offset.z * offset.y * weight;
        Moment(2, 2) += offset.z * offset.z * weight;
    }

    // Recall that if a self-adjoint matrix is real, then it's also symmetric, and the
    // eigenvalues of a real, symmetric matrix are real and also orthogonal.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Moment);

    auto eigenvectors = eigensolver.eigenvectors();
    auto eigenvalues = eigensolver.eigenvalues();

    // Eigenvalues are sorted in *increasing* order, so the strongest axis is the last
    Vec3 axis0 = toVec3(eigenvectors.col(2));
    Vec3 axis1 = toVec3(eigenvectors.col(1));

    // Eigenvectors are unique up to a sign, so we use the integrated area vector to
    // select an orientation stable with respect to small peturtbations. The first two
    // eigenvectors correspond to the strongest eigenvalues, so we flip their parity.
    // The third eigenvalue encodes the least amount of information, so we overwrite it
    // to enforce the correct parity.
    if (Vec3::dot(areaVector, axis0) < 0.0f) axis0 = -axis0;
    if (Vec3::dot(areaVector, axis1) < 0.0f) axis1 = -axis1;
    Vec3 axis2{Vec3::cross(axis0, axis1)};

    // Return the transform which moves a model at the origin *into* alignment with this
    // model. To align this model upright, you must invert this transform.
    return Mat3x4{
        axis0.x, axis1.x, axis2.x, centroid.x,
        axis0.y, axis1.y, axis2.y, centroid.y,
        axis0.z, axis1.z, axis2.z, centroid.z
    };
}

Mat3x4 computePointwisePrincipalAxes(const std::vector<math::Vec3>& positions)
{
    using namespace math;

    Eigen::Matrix3f Moment;
    Moment.setZero();

    Vec3 areaVector{0.0f, 0.0f, 0.0f};
    Vec3 centroid(computeCentroid(positions));

    for (int i = 0; i < positions.size(); i++) {
        const Vec3 pA = positions[i];

        Vec3 offset = pA - centroid;
        areaVector += offset;

        Moment(0, 0) += offset.x * offset.x;
        Moment(1, 0) += offset.x * offset.y;
        Moment(2, 0) += offset.x * offset.z;

        //Moment(0, 1) += offset.y * offset.x * weight;
        Moment(1, 1) += offset.y * offset.y;
        Moment(2, 1) += offset.y * offset.z;

        //Moment(0, 2) += offset.z * offset.x * weight;
        //Moment(1, 2) += offset.z * offset.y * weight;
        Moment(2, 2) += offset.z * offset.z;
    }

    // Recall that if a self-adjoint matrix is real, then it's also symmetric, and the
    // eigenvalues of a real, symmetric matrix are real and also orthogonal.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Moment);

    auto eigenvectors = eigensolver.eigenvectors();
    auto eigenvalues = eigensolver.eigenvalues();

    // Eigenvalues are sorted in *increasing* order, so the strongest axis is the last
    Vec3 axis0 = toVec3(eigenvectors.col(2));
    Vec3 axis1 = toVec3(eigenvectors.col(1));

    // Eigenvectors are unique up to a sign, so we use the integrated area vector to
    // select an orientation stable with respect to small peturtbations. The first two
    // eigenvectors correspond to the strongest eigenvalues, so we flip their parity.
    // The third eigenvalue encodes the least amount of information, so we overwrite it
    // to enforce the correct parity.
    if (Vec3::dot(areaVector, axis0) < 0.0f) axis0 = -axis0;
    if (Vec3::dot(areaVector, axis1) < 0.0f) axis1 = -axis1;
    Vec3 axis2{Vec3::cross(axis0, axis1)};

    // Return the transform which moves a model at the origin *into* alignment with this
    // model. To align this model upright, you must invert this transform.
    return Mat3x4{
        axis0.x, axis1.x, axis2.x, centroid.x,
        axis0.y, axis1.y, axis2.y, centroid.y,
        axis0.z, axis1.z, axis2.z, centroid.z
    };
}

}

} // namespace StandardCyborg {


#include "lorenz_aliases.h"
#include "lorenz.h"
#include "lorenz_equations.h"
#include "push_back_local_maxima.h"

#include <fstream>
#include <vector>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

void outputLorenzAttractorData(const std::string& output_directory)
{
	constexpr double sigma = 10.0;
	constexpr double r = 28.0;
	constexpr double b = 8.0 / 3.0;

	std::ofstream file_stream;

	file_stream.open(output_directory);

	state_type x = { 10.0 , 1.0 , 1.0 }; // initial conditions

	auto lambda_writer = [&file_stream](const state_type& x, const double t)
	{
		file_stream << t << ',' << x[0] << ',' << x[1] << ',' << x[2] << std::endl;
	};

	boost::numeric::odeint::integrate(lorenz(sigma, r, b), x, 0.0, 100.0, 0.01, lambda_writer);

	file_stream.close();
}

void outputLorenzBifurcationDiagramData(const std::string& output_file)
{

	// range for parameter change
	constexpr double r_min = 0.0;
	constexpr double r_max = 400.0;

	constexpr int r_num_divisions = 400;
	constexpr double r_division = (r_max - r_min) / (double)r_num_divisions;

	std::ofstream file_stream;

	file_stream.open(output_file);

	double r{ r_min };

	for (int i{ 0 }; i < r_num_divisions; i++)
	{
		r += r_division;
		constexpr double sigma{ 10.0 };
		constexpr double b{ 8.0 / 3.0 };

		state_type state = { 15.0, 20.0, 30.0 };

		constexpr double transient_time = 100.0;

		boost::numeric::odeint::integrate(lorenz(sigma, r, b), state, 0.0, transient_time, 0.1);

		std::vector<state_type> states;
		std::vector<double> times;

		boost::numeric::odeint::integrate(lorenz(sigma, r, b), state, 0.0, 100.0, 0.1, push_back_local_maxima(states, times));

		for (size_t i{ 0 }; i < states.size(); i++)
			file_stream << times[i] << ',' << r << ',' << states[i][0] << ',' << states[i][1] << ',' << states[i][2] << std::endl;
	}

	file_stream.close();

	return;
}


#ifndef _EIGEN_RIDGE_HPP_

#define _EIGEN_RIDGE_HPP_

/*
 * L2-regularized (ridge) linear regression without intercept in c++11 as Eigen3 template function
 * Uses singular value decomposition, works with both tall and wide design matrices
 * Written by Carlos Guerreiro carlos@perceptiveconstructs.com
 * This is free and unencumbered software released into the public domain.
 */

#include <Eigen/Dense>

template<typename M, typename V, typename P>
M ridge(const M& A, const V& y,  P alpha) {
  const auto& svd = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
  const auto& s = svd.singularValues();
  const auto r = s.rows();
  const auto& D = s.cwiseQuotient((s.array().square() + alpha).matrix()).asDiagonal();
  return svd.matrixV().leftCols(r) * D * svd.matrixU().transpose().topRows(r) * y;
}

#endif


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main()
{
  Matrix3d m = Matrix3d::Random();
  m = (m + Matrix3d::Constant(1.2)) * 50;
  cout << "m =" << endl << m << endl;
  Vector3d v(1,2,3);

  cout << "m * v =" << endl << m * v << endl;
}


// The template and inlines for the -*- C++ -*- 3d vector classes.
// Initially implemented by Wai-Shing Luk <luk036@gmail.com>
//

/** @file include/vector3d.hpp
 *  This is a C++ Library header.
 */

#ifndef FUN_VECTOR3D_HPP
#define FUN_VECTOR3D_HPP 1

#include <boost/operators.hpp>   // for boost::addable etc

namespace fun
{

/**
 *  3-dimensional vector.
 *
 *  @param  Tp  Type of vector elements
 */
template <typename _K>
class vector3d :
  boost::equality_comparable < vector3d<_K>,
  boost::addable < vector3d<_K>,
  boost::subtractable < vector3d<_K>,
  boost::multipliable2 < vector3d<_K>, _K,
  boost::dividable2 < vector3d<_K>, _K
  > > > > >
{
  /// Value typedef.
  typedef _K value_type;

public:
  /// static memeber zero
  static const vector3d<_K> zero;

  /// Default constructor.
  constexpr vector3d<_K>(
    const _K& e1,
    const _K& e2,
    const _K& e3) noexcept
    : _e1{e1}, _e2{e2}, _e3{e3} { }

  /// Return first element of vector.
  constexpr _K e1() const noexcept { return _e1; }
  /// Return second element of vector.
  constexpr _K e2() const noexcept { return _e2; }
  /// Return third element of vector.
  constexpr _K e3() const noexcept { return _e3; }

  // Lets the compiler synthesize the assignment operator
  // vector3d<_K>& operator= (const vector3d<_K>&);
  /// Assign this vector to vector @a w.
  /// Add @a w to this vector.
  vector3d<_K>& operator+=(const vector3d<_K>& w)
  { _e1 += w.e1(); _e2 += w.e2(); _e3 += w.e3(); return *this; }

  /// Subtract @a w from this vector.
  vector3d<_K>& operator-=(const vector3d<_K>& w)
  { _e1 -= w.e1(); _e2 -= w.e2(); _e3 -= w.e3(); return *this; }

  /// Multiply this vector by @a a.
  vector3d<_K>& operator*=(const _K& a)
  { _e1 *= a; _e2 *= a; _e3 *= a; return *this; }

  /// Divide this vector by @a a.
  vector3d<_K>& operator/=(const _K& a)
  { _e1 /= a; _e2 /= a; _e3 /= a; return *this; }

  constexpr bool operator== (const vector3d<_K>& w) const
  { return e1() == w.e1() && e2() == w.e2() && e3() == w.e3(); }

private:
  _K _e1;
  _K _e2;
  _K _e3;
};

template <typename _K>
const vector3d<_K> vector3d<_K>::zero (_K(0), _K(0), _K(0));

/// Return @a v.
template<typename _K>
inline constexpr vector3d<_K>
operator+(const vector3d<_K>& v) noexcept
{ return v; }

/// Return negation of @a v/
template<typename _K>
inline constexpr vector3d<_K>
operator-(const vector3d<_K>& v) noexcept
{ return vector3d<_K>(-v.e1(), -v.e2(), -v.e3()); }

///  Return dot product of  @a v and @a w
template<typename _K>
inline constexpr _K
dot(const vector3d<_K>& v, const vector3d<_K>& w) noexcept
{ return v.e1()*w.e1() + v.e2()*w.e2() + v.e3()* w.e3(); }

///  Return new vector @a v x @a w (cross product).
template<typename _K>
inline constexpr vector3d<_K>
cross(const vector3d<_K>& v, const vector3d<_K>& w) noexcept
{
  return vector3d<_K>(
     v.e2()*w.e3() - v.e3()*w.e2(),
    -v.e1()*w.e3() + v.e3()*w.e1(),
     v.e1()*w.e2() - v.e2()*w.e1()  );
}

///  Return determinant of @a p,  @a q and @a r.
template<typename _K>
inline constexpr _K
det(const vector3d<_K>& p, const vector3d<_K>& q,
    const vector3d<_K>& r) noexcept
{
  return dot(p, cross(q, r)); // Pl\{"}ucker's formula
}

///  Insertion operator for vector values.
template<typename _K, class _Stream>
_Stream& operator<<(_Stream& os, const vector3d<_K>& v)
{
  os << '[' << v.e1() << ',' << v.e2() << ',' << v.e3() << ']';
  return os;
}

///  Return the quadrance of  @a v
template<typename _K>
inline constexpr _K
quadrance(const vector3d<_K>& v) noexcept
{ return dot(v,v); }

} // namespace fun

#endif


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef UTIL_MEDIAN
#define UTIL_MEDIAN

#include <boost/range/algorithm.hpp>
#include <vector>       // std::vector

namespace sferes {
  namespace util {
  	class Median
		{
		public:
  		/**
  		 * Calculate the median of the doubles in the given list.
  		 */
  		static double calculate_median(std::vector<double>& list)
  		{
  			size_t size = list.size();	// Size of the list

  			assert(size > 0);

  			std::sort(list.begin(), list.end());

  			// If there are an odd number of doubles
  			if (size % 2 == 1)
  			{
  				// Take the middle number
  				size_t index_middle = (size - 1)/2;
  				return list[index_middle];
  			}
  			// If there are an even number of doubles
  			else
  			{
  				size_t index_above = size / 2;
  				size_t index_below = index_above - 1;

  				// Average of two middle numbers
  				return (list[index_above] + list[index_below]) / 2;
  			}
  		}
		};
  }
}
#endif


/**
 * @file semimprk_main.cc
 * @brief NPDE homework SemImpRK code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>

#include "semimprk.h"

int main() {
  auto f = [](Eigen::Vector3d y) -> Eigen::Vector3d {
    return Eigen::Vector3d(y(0) * y(1), y(1) * y(2), y(2) - y(0));
  };
  auto df = [](Eigen::Vector3d y) {
    Eigen::Matrix3d J;
    J << y(1), y(0), 0.0, 0.0, y(2), y(1), -1.0, 0.0, 1.0;
    return J;
  };
  Eigen::Vector3d y0(1.0, 2.0, 3.0);
  unsigned int M = 10;
  double T = 2.0;

  std::cout << "Test of SolveRosenbrock():"
            << SemImpRK::SolveRosenbrock(f, df, y0, M, T).back().transpose()
            << std::endl;

  double cvgRate = SemImpRK::CvgRosenbrock();
  std::cout << "Convergence rate: " << cvgRate << std::endl;

  return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <math.h>
#include "scf.h"
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
using std::sqrt;
int INDEX(int i,int j) {
    if (i>j) return i*(i+1)/2 + j;
    return j*(j+1)/2 + i;
}
#define MAX_ITER 75
#define EDIFF_CONV 0.000000000002
#define RMS_CONV 0.000000000014
double rmsDensity(MatrixXd A, MatrixXd B, int mat_size){
    double result = 0;
    for (int i =0 ; i < mat_size; i++){
        for(int j =0; j < mat_size; j++){
            result += pow(A(i,j) - B(i,j),2);
        }
    }
    return pow(result,0.5);
}

void run_scf() {
//creating variables/objects
// Reading input
    string SCF_options[1] = {"num_elec"};
    ifstream input ("INPUT");
    string newline,temp_str;
    int occ = 0;
    while(getline(input,newline)){
        istringstream ss(newline);
        ss >> temp_str;
        if (temp_str == "num_elec") {
            ss >> occ;
            occ /= 2;
        }
    }

    ifstream numffile ("../mol/num_nao.dat");
    int num_basisf ;
    numffile >> num_basisf;
    int mat_length = num_basisf * (num_basisf + 1) / 2 ;
    int total_mat_length = num_basisf * num_basisf;
    double enuc, rms, Ediff;
    double e0, eX, eC, etotal =0;
    MatrixXd t_array(num_basisf,num_basisf);
    MatrixXd v_array(num_basisf,num_basisf);
    MatrixXd s_array(num_basisf,num_basisf);
    MatrixXd hcore(num_basisf,num_basisf);
    MatrixXd F0(num_basisf,num_basisf);
    MatrixXd FC(num_basisf,num_basisf);
    MatrixXd FX(num_basisf,num_basisf);
    MatrixXd AO_eigenvectors(num_basisf,num_basisf);
    MatrixXd D0(num_basisf,num_basisf);
    MatrixXd DC(num_basisf,num_basisf);
    MatrixXd DX(num_basisf,num_basisf);
    MatrixXd FX_org(num_basisf,num_basisf);
    MatrixXd FC_org(num_basisf,num_basisf);
    VectorXd eri_array(num_basisf*num_basisf*num_basisf*num_basisf);

 // Reading variables
 // Nuclear repulsion energy: enuc.dat --> enuc
 // Kinetic energy inegrals: t.dat --> t_array
 // Nuclear attraction inegrals: v.dat --> v_array
 // AO Overlap: s.dat --> s_array
 // Core Hamiltonian: hcore

 // Reading enuc
    ifstream enucfile ("../mol/enuc.dat");
    enucfile >> enuc;
//  Reading One-Electron Inegrals

    ifstream tfile ("../mol/t.dat");
    ifstream vfile ("../mol/v.dat");
    ifstream sfile ("../mol/s.dat");
    while(getline(tfile, newline)){
        int i,j;
        double temp_t;
        istringstream ss(newline);
        ss >> i >> j >> temp_t;
        t_array(i-1,j-1) = temp_t;
        if(i != j ) t_array(j-1,i-1) = temp_t;
    }

    while(getline(vfile, newline)){
        int i,j;
        double temp_v;
        istringstream ss(newline);
        ss >> i >> j >> temp_v;
        v_array(i-1,j-1) = temp_v;
        if(i != j ) v_array(j-1,i-1) = temp_v;
    }
    while(getline(sfile, newline)){
        int i,j;
        double temp_s;
        istringstream ss(newline);
        ss >> i >> j >> temp_s;
        s_array(i-1,j-1) = temp_s;
        if(i != j ) s_array(j-1,i-1) = temp_s;
    }
//  Core Hamiltonian hcore;
//  Hcore = T + V
        hcore = t_array + v_array;

//  Read Two Electron repulsion data
//  eri.dat > eri_array
    ifstream erifile ("../mol/eri.dat");
    while(getline(erifile, newline)){
        int i, j, ij, k, l, kl, ijkl;
        double temp_eri;
        istringstream ss(newline);
        ss >> i >> j >> k >> l >> temp_eri;
        ij = INDEX(i,j);
        kl = INDEX(k,l);
        ijkl = INDEX(ij,kl);
        eri_array(ijkl) = temp_eri;
    }

//  Orthogonlization matrix
// V * D^-1/2 * V^-1
    SelfAdjointEigenSolver<MatrixXd> s_eigen(s_array);

//  Initial Guess of Wavefunction
    F0 = s_eigen.operatorSqrt().inverse().transpose() * hcore * s_eigen.operatorSqrt().inverse();
    SelfAdjointEigenSolver<MatrixXd> F_eigen(F0);
    AO_eigenvectors = s_eigen.operatorSqrt().inverse() * F_eigen.eigenvectors();
    for ( int i = 0 ; i < num_basisf ; i++){
        for ( int j = 0 ; j < num_basisf; j++){
            for (int k = 0 ; k < occ ; k++){
               D0(i,j) +=  AO_eigenvectors.col(k).row(i) * AO_eigenvectors.col(k).row(j);
            }
        }
    }
//  Compute Inital Hartree Fock Energy
    for (int i =0 ; i < num_basisf; i++){
        for (int j =0 ; j < num_basisf; j++){
            e0 += D0(i,j) * (hcore(i,j) + hcore(i,j));
        }
    }
    etotal = e0 + enuc;
    DC = DX = D0;
    eC = eX = e0;
    cout << setw(10) << "Iter";
    cout << setw(25)<< setprecision(15) << "E(elec)";
    cout << setw(25)<< setprecision(15) << "E(Total)";
    cout <<setw(25) << setprecision(15) << "D(E)" ;
    cout << setw(25) << setprecision(15) << "RMS(D)" << endl;
    cout << setw(10) << "0";
    cout << setw(25)<< setprecision(15) << eC;
    cout << setw(25)<< setprecision(15) << eC + enuc;
    cout <<setw(25) << setprecision(15) << "0.00";
    cout << setw(25) << setprecision(15) << "0.00" << endl;
// enter into SCF loop
for (int iter = 1 ; iter <= MAX_ITER; iter++){
// Build new Fock Matrix
    for ( int i =1; i <= num_basisf; i++){
        for (int j =1 ; j <= num_basisf; j++){
            FC(i-1,j-1) = hcore(i-1,j-1);
            for(int k =1; k <= num_basisf; k++){
                for( int l=1; l <= num_basisf; l++){
                    int ij = INDEX(i,j);
                    int kl = INDEX(k,l);
                    int ijkl = INDEX(ij,kl);
                    int ik = INDEX(i,k);
                    int jl = INDEX(j,l);
                    int ikjl = INDEX(ik,jl);
                    FC(i-1,j-1) += DC(k-1,l-1) * ( 2.0 * eri_array(ijkl) - eri_array(ikjl));
                }
            }
        }
    }

//  Build Density Matrix
    FC_org = s_eigen.operatorSqrt().inverse().transpose() * FC * s_eigen.operatorSqrt().inverse();
    SelfAdjointEigenSolver<MatrixXd> FC_eigen(FC_org);
    MatrixXd AOC_eigenvectors(num_basisf,num_basisf);
    AOC_eigenvectors = s_eigen.operatorSqrt().inverse() * FC_eigen.eigenvectors();
    for ( int i = 0 ; i < num_basisf ; i++){
        for ( int j = 0 ; j < num_basisf; j++){
            DC(i,j)= 0;
            for (int k = 0 ; k < occ ; k++){
               DC(i,j) +=  AOC_eigenvectors.col(k).row(i) * AOC_eigenvectors.col(k).row(j);
            }
        }
    }

//  Compute new Hartree Fock Energy
    eC = 0;
    for (int i =0 ; i < num_basisf; i++){
        for (int j =0 ; j < num_basisf; j++){
            eC += DC(i,j) * ( hcore(i,j) + FC(i,j));
        }
    }
    Ediff = eC - eX;
    rms = rmsDensity(DC,DX,num_basisf);
    cout << setw(10) << iter;
    cout << setw(25)<< setprecision(15) << eC;
    cout << setw(25)<< setprecision(15) << eC + enuc;
    cout <<setw(25) << setprecision(15) << Ediff;
    cout << setw(25) << setprecision(15) << rms << endl;

    if (Ediff < EDIFF_CONV && rms < RMS_CONV){
        cout << "CONVERGENCE has been reached" << endl;
        break;
    }else if(iter == MAX_ITER){
        cout << "NO CONVERGENCE has been reached" << endl;
    }
    //Save current information for next loop
    eX = eC;
    DX = DC;

} //end of SCF Loop
}


#include <iostream>
#include <vector>
#include <string>
#include <math.h>
#include <ctime>
#include <armadillo>

void plotData(std::vector<double> data);

class Layer {
    arma::mat sigmoid(arma::mat x) {
        return 1.0 / (1.0 + exp(-1.0*x));
    }

    public:
    arma::mat weights;
    arma::mat negativeVisibleProbabilities;
    arma::umat positiveHiddenStates;
    arma::mat positiveAssociations, negativeAssociations;

    Layer() {
        weights.randu(1, 1);
    }

    Layer(int neuronCount, int inputsPerNeuron) {
        arma::arma_rng::set_seed(1);
        weights.randn(inputsPerNeuron, neuronCount);
        weights *= 0.1;
        weights.insert_rows(0, 1);
        weights.insert_cols(0, 1);
    }

    void calculatePositiveAssociations(arma::mat x) {
        arma::mat positiveHiddenProbabilities = sigmoid(x*weights);
        arma::arma_rng::set_seed(1);
        arma::mat randomNormalProbabilties = arma::randu(size(positiveHiddenProbabilities));
        positiveHiddenStates = positiveHiddenProbabilities > randomNormalProbabilties;
        positiveAssociations = x.t()*positiveHiddenProbabilities;
    }

    void calculateNegativeAssociations() {
        arma::mat negativeVisibleActivations = positiveHiddenStates*weights.t();
        negativeVisibleProbabilities = sigmoid(negativeVisibleActivations);
        negativeVisibleProbabilities.col(0) = arma::ones<arma::vec>(negativeVisibleProbabilities.n_rows);
        arma::mat negativeHiddenActivations = negativeVisibleActivations*weights;
        arma::mat negativeHiddenProbabilities = sigmoid(negativeHiddenActivations);
        negativeAssociations = negativeVisibleProbabilities.t()*negativeHiddenProbabilities;
    }

    void run(arma::mat x) {
        calculatePositiveAssociations(x);
        calculateNegativeAssociations();
    }
};

class RBM {
    arma::mat input;
    Layer L1;
    int L1NodeCount = 2;
    double learningRate = 0.0005;
    double exampleCount;

    arma::mat sigmoid_derivative(arma::mat x) {
        return x % (1-x);
    }

    void randomInitWeights(int visibleNodeCount) {
        L1 = Layer(L1NodeCount, visibleNodeCount);
    }

    public:

    RBM(arma::mat i) {
        input = join_rows(arma::ones<arma::mat>(i.n_rows, 1), i);
        randomInitWeights(i.n_cols);
        exampleCount = i.n_rows;
    }

    void train(int numIt) {
        double error;

        for(int i=0; i<numIt; i++) {
            L1.run(input);
            L1.weights += learningRate * ((L1.positiveAssociations-L1.negativeAssociations) / exampleCount);

            if (i%1000 == 0) {
                error = accu(square(input - L1.negativeVisibleProbabilities));
                std::cout << "Step "<< i<<": "<< error << std::endl;
            }
        }
        error = accu(square(input - L1.negativeVisibleProbabilities));
        std::cout << "Step "<< numIt <<": "<< error << std::endl;
    }

    arma::umat run(arma::mat i) {
        arma::mat input = join_rows(arma::ones<arma::mat>(i.n_rows, 1), i);
        L1.calculatePositiveAssociations(input);
        return L1.positiveHiddenStates.cols(1, L1.positiveHiddenStates.n_cols-1);
    }
};

void plotData(std::vector<double> data) {
    FILE *pipe = popen("gnuplot -persist" , "w");

    if (pipe != NULL) {

        fprintf(pipe, "set style line 5 lt rgb 'cyan' lw 3 pt 6 \n");
        fprintf(pipe, "plot '-' with linespoints ls 5 \n");

        for (int i=0; i<data.size(); i++) {
            fprintf(pipe, "%lf %lf\n", double(i), data[i]);
        }
        fprintf(pipe, "e");

        fflush(pipe);
        pclose(pipe);
    }
    else {
        std::cout << "Could not open gnuplot pipe" << std::endl;
    }
}

int main() {
    arma::mat input = {{1,1,1,0,0,0},{1,0,1,0,0,0},{1,1,1,0,0,0},{0,0,1,1,1,0},{0,0,1,1,0,0},{0,0,1,1,1,0}};
    int numIterations = 6000;

    std::cout << "RBM trained on multiple user's movie preferences" << std::endl;
    RBM model(input);

    std::clock_t startTime;
    startTime = std::clock();
    model.train(numIterations);
    std::cout << "Training Time: " << (std::clock() - startTime) / (double)(CLOCKS_PER_SEC / 1000) << " ms" << std::endl << std::endl;
    arma::mat test = {{0,0,0,1,1,0}};
    std::cout << "Test User:" << std::endl;
    std::cout << "Likes 1st Harry Potter:\t" << test(0,0) << std::endl;
    std::cout << "Likes Avatar:\t\t" << test(0,1) << std::endl;
    std::cout << "Likes 3rd LOTR:\t\t" << test(0,2) << std::endl;
    std::cout << "Likes Gladiator:\t" << test(0,3) << std::endl;
    std::cout << "Likes Titanic:\t\t" << test(0,4) << std::endl;
    std::cout << "Likes Troll 2:\t\t" << test(0,5) << std::endl << std::endl;
    startTime = std::clock();
    arma::umat hiddenStates = model.run(test);
    std::cout << "Prediction Time: " << (std::clock() - startTime) / (double)(CLOCKS_PER_SEC / 1000) << " ms" << std::endl;
    std::cout << "Hidden Neuron Activations:" << std::endl;
    std::cout << "Likes Oscar Winners:\t" << hiddenStates(0,0) << std::endl;
    std::cout << "Likes SciFi / Fantasy:\t" << hiddenStates(0,1) << std::endl;
    return 0;
}