// 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 <tiny.h>
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(convex_signed_distance_to_vertex_edge_vp);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef tiny::MathTypes<double>     math_types;
  typedef math_types::vector3_type   vector3_type;
  typedef math_types::real_type      real_type;

  vector3_type a = vector3_type::make(1.0, 0.0, 0.0);
  vector3_type b = vector3_type::make(0.0, 0.0, 0.0);

  // First we use a test point that does not lie on the line

  // Front side of A voronoi plane
  {
    vector3_type p = vector3_type::make( 2.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of A voronoi plane
  {
    vector3_type p = vector3_type::make( 0.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In A voronoi plane
  {
    vector3_type p = vector3_type::make( 1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Front side of B voronoi plane
  {
    vector3_type p = vector3_type::make( -1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of B voronoi plane
  {
    vector3_type p = vector3_type::make( 1.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In B voronoi plane
  {
    vector3_type p = vector3_type::make( 0.0, 1.0,  1.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Second we use a test point that lies on the line

  // Front side of A voronoi plane
  {
    vector3_type p = vector3_type::make( 2.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of A voronoi plane
  {
    vector3_type p = vector3_type::make( 0.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In A voronoi plane
  {
    vector3_type p = vector3_type::make( 1.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, a, b);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

  // Front side of B voronoi plane
  {
    vector3_type p = vector3_type::make( -1.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 1.0, 0.01 );
  }
  // Back side of B voronoi plane
  {
    vector3_type p = vector3_type::make( 1.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, -1.0, 0.01 );
  }
  // In B voronoi plane
  {
    vector3_type p = vector3_type::make( 0.0, 0.0,  0.0);
    real_type sign_p = 0.0;
    sign_p = convex::signed_distance_to_vertex_edge_voronoi_plane(p, b, a);
    BOOST_CHECK_CLOSE( sign_p, 0.0, 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#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;
    }
}


#include "PalindromeQuality.hpp"
#include "span.hpp"
#include <cmath>

using std::runtime_error;
using std::cout;
using std::cerr;
using std::pow;

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace boost::accumulators;
using namespace shasta;

using stats_accumulator = accumulator_set<float, features<tag::count, tag::mean, tag::variance>>;

double qualityCharToErrorProbability(char q) {
    return pow(10, double(q - 33) / -10.0);
}

bool shasta::isPalindromic(
        span<char> qualities,
        double relativeMeanDifference,
        double minimumMean,
        double minimumVariance
){

    bool isPalindromic = false;

    stats_accumulator leftStats;
    stats_accumulator rightStats;

    auto length = qualities.size();

    // Don't bother classifying any reads that would have less than 3 scores per side
    if (length < 6){
        return false;
    }

    size_t midpoint = length/2;

    for (size_t i=0; i<midpoint; i++){
        const auto q = qualities[i];
        auto p = qualityCharToErrorProbability(q);

        leftStats(p);
    }

    for (size_t i=midpoint; i<length; i++){
        const auto q = qualities[i];
        auto p = qualityCharToErrorProbability(q);

        rightStats(p);
    }

    float leftMean = mean(leftStats);
    float leftVariance = variance(leftStats);

    float rightMean = mean(rightStats);
    float rightVariance = variance(rightStats);

    // Compare the mean and variance using thresholds derived empirically from some palindromic reads
    if (rightMean - leftMean > relativeMeanDifference and rightMean >= minimumMean){
        if (rightVariance > leftVariance and rightVariance > minimumVariance){
            isPalindromic = true;
        }
    }

    return isPalindromic;
}


///////////////////////////////////////////////////////////////////////////////
// vector_space::functional::equal.hpp                                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_EQUAL_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_EQUAL_HPP_ER_2009
#include <boost/call_traits.hpp>
#include <ostream>
#include <boost/range.hpp>
#include <boost/mpl/nested_type.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/vector_space/functional/l2_distance_squared.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace vector_space{

// This is a predicate for (x == y) iff (|| x - y || == 0).
//
// Requirements:
// F<R> distance(x);        Construction
// distance(y)              Returns an object of type range_value<R>::type
//
// Usage:
// functor<R,F> fun(x,eps);
// fun(y) returns true if distance(y) < eps
template<typename R, template<typename> class F = l2_distance_squared>
class equal{
    typedef F<R> arg_;
    typedef typename mpl::nested_type<arg_>::type distance_type;
    typedef typename remove_reference<R>::type const_range_type;
    typedef typename remove_const<const_range_type>::type range_type;
    typedef typename range_value<range_type>::type value_type;
public:

    //Construction
    equal(typename call_traits<R>::param_type x,value_type eps);
    equal(typename call_traits<R>::param_type x);
    equal(const equal& that);
    equal& operator=(const equal& that);

    //Evaluate
    typedef bool result_type;

    template<typename R1> result_type operator()(const R1& y)const;
    result_type epsilon()const;

//private:
    equal();
    distance_type distance_;
    value_type eps_;
    static value_type default_eps_;
};

//
template<typename R, template<typename> class F>
std::ostream& operator<<(std::ostream& out,const equal<R,F>& e){
    out << '(' <<  e.epsilon() << ')';
    return out;
}

// Static members
template<typename R, template<typename> class F>
typename equal<R,F>::value_type
equal<R,F>::default_eps_
    = math::tools::epsilon<typename equal<R,F>::value_type>();

template<typename R, template<typename> class F>
typename equal<R,F>::result_type
equal<R,F>::epsilon()const{
    return eps_;
}

//Construction

template<typename R, template<typename> class F>
equal<R,F>::equal(typename call_traits<R>::param_type x,value_type eps)
:distance_(x),eps_(eps){}

template<typename R, template<typename> class F>
equal<R,F>::equal(typename call_traits<R>::param_type x)
:distance_(x),eps_(default_eps_){}

template<typename R, template<typename> class F>
equal<R,F>::equal(const equal& that)
:distance_(that.distance_),eps_(that.eps_){}

template<typename R, template<typename> class F>
equal<R,F>&
equal<R,F>::operator=(const equal& that){
    if(&that!=this){
        distance_ = that.distance_;
        eps_ = that.eps_;
    }
    return *this;
}

// Evaluate
template<typename R, template<typename> class F>
template<typename R1>
typename equal<R,F>::result_type
equal<R,F>::operator()(const R1& y)const{
    return (distance_(y)<eps_);
}

}// vector_space
}// detail
}// statistics
}// boost

#endif


// 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;
}


/* test_non_central_chi_squared.cpp
 *
 * Copyright Steven Watanabe 2011
 * Copyright Thijs van den Berg 2014
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/non_central_chi_squared_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::non_central_chi_squared_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME non_central_chi_squared
#define BOOST_MATH_DISTRIBUTION boost::math::non_central_chi_squared
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME k
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(k) boost::uniform_real<>(0.00001, k)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME lambda
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(lambda) boost::uniform_real<>(0.00001, lambda)

#include "test_real_distribution.ipp"


#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl; using namespace mtl::mat;

    const unsigned                xd= 2, yd= 5, n= xd * yd;
    dense2D<double>               A(n, n);
    compressed2D<double>          B(n, n);
    hessian_setup(A, 3.0); laplacian_setup(B, xd, yd);

    typedef std::complex<double>  cdouble;
    dense_vector<cdouble>         v(n), w(n);
    for (unsigned i= 0; i < size(v); i++)
	v[i]= cdouble(i+1, n-i), w[i]= cdouble(i+n);

    v+= A * w;
    w= B * v;

    std::cout << "v is " << v << "\n";
    std::cout << "w is " << w << "\n";

    return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
MatrixXf mat(2,2);
mat << 1, 2,  4, 7;
cout << "Here is the matrix mat:\n" << mat << endl << endl;

mat = 2 * mat;
cout << "After 'mat = 2 * mat', mat = \n" << mat << endl << endl;

mat = mat - MatrixXf::Identity(2,2);
cout << "After the subtraction, it becomes\n" << mat << endl << endl;

ArrayXXf arr = mat;
arr = arr.square();
cout << "After squaring, it becomes\n" << arr << endl << endl;

// Combining all operations in one statement:
mat << 1, 2,  4, 7;
mat = (2 * mat - MatrixXf::Identity(2,2)).array().square();
cout << "Doing everything at once yields\n" << mat << endl << endl;

}
  return 0;
}


//测试功能：尝试自己手动“模拟”关键帧和普通帧被创建的过程，并且在视野中进行显示
//不过目前准备首先实现的功能的就是，能够通过控制面板中的参数变化来控制窗口中帧的位姿变化
//可能首先需要解决从Eigen欧拉角=>旋转矩阵，这样的一个变换

//NOTICE 但是这个有问题，恐怕还是得使用矩阵的那种变换形式比较好。
#include <pangolin/pangolin.h>
#include <vector>
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

#define PI (3.1415926535897932346f)

using namespace std;

//COL MAJOR!!!
/*
vector<GLfloat> Twc=
{
    1,0,0,0,
    0,1,0,0,
    0,0,1,0,
    5,0,0,1         //Trans
};
*/

typedef struct __Pose
{

    __Pose(double _pitch,double _roll,double _yaw,double _x,double _y,double _z,bool key=false):
        pitch(_pitch),roll(_roll),yaw(_yaw),x(_x),y(_y),z(_z),keyFrame(key)
        {}

    double pitch;
    double roll;
    double yaw;

    double x;
    double y;
    double z;

    bool keyFrame;
}Pose;

void drawFrame(const float w=2.0f);
double deg2rad(const double deg);
Eigen::Matrix3d degEuler2matrix(double pitch,double roll,double yaw);
vector<GLfloat> eigen2glfloat(Eigen::Isometry3d T);
void drawAllFrames(vector<Pose> frames,bool orderRT=true);

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

    vector<Pose> frames;

    //========================= 窗口 ========================
    pangolin::CreateWindowAndBind(
        "Frame - simulation",     //窗口标题
        640,        //窗口尺寸
        480);       //窗口尺寸
    glEnable(GL_DEPTH_TEST);

    //========================== 3D 交互器 =====================
    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(
            640,480,            //相机图像的长和宽
            420,420,320,240,    //相机的内参,fu fv u0 v0
            0.2,500),           //相机所能够看到的最浅和最深的像素
        pangolin::ModelViewLookAt(
            -20,-20,-20,            //相机光心位置,NOTICE z轴不要设置为0
            0,0,0,              //相机要看的位置
            pangolin::AxisY)    //和观察的方向有关
    );

    //======================== 调节面板 ==============================
     const int UI_WIDTH=180;
     pangolin::View& d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -640.0f/480.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

    pangolin::CreatePanel("ui")
      .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

    //接下来要开始准备添加控制选项了
    pangolin::Var<double> axisSize("ui.axis_size",5,1,20);

    pangolin::Var<bool> checkOrderBtn("ui.Order_RT",true,true);
    pangolin::Var<double> frameRoll("ui.frame_roll",0,-90,90);
    pangolin::Var<double> framePitch("ui.frame_pitch",0,-90,90);
    pangolin::Var<double> frameYaw("ui.frame_yaw",0,-180,180);
    pangolin::Var<double> frameX("ui.frame_X",0,-100,100);
    pangolin::Var<double> frameY("ui.frame_Y",0,-100,100);
    pangolin::Var<double> frameZ("ui.frame_Z",0,-100,100);

    pangolin::Var<bool> addFrameBtn("ui.add_frame",false,false);
    pangolin::Var<bool> addKeyFrameBtn("ui.add_keyFrame",false,false);

    pangolin::Var<bool> resetFrameBtn("ui.reset_frame",false,false);
    pangolin::Var<bool> resetViewBtn("ui.reset_view",false,false);

    while( !pangolin::ShouldQuit() )
    {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);

        // 因为后面的^画图需要,暂时需要将这里的背景修改成为白色
        glClearColor(1,1,1,0.0);

        //按钮的响应 - 复位视图
        if(pangolin::Pushed(resetViewBtn))
        {
            s_cam.SetModelViewMatrix(
                pangolin::ModelViewLookAt(
            -50,-50,-50,  0,0,0,  pangolin::AxisNegY));

            cout<<"Reset view."<<endl;
        }

        //按钮的响应 - 复位帧的位姿
        if(pangolin::Pushed(resetViewBtn))
        {
            //BUG 目前的问题是，就算是数值复位了，但是控制面板上的gui并不会复位
            framePitch=0.0f;
            framePitch.GuiChanged();
            frameRoll=0.0f;
            frameYaw=0.0f;
            frameX=0.0f;
            frameY=0.0f;
            frameZ=0.0f;

            cout<<"Reset frame pose."<<endl;
        }

        //按钮的相应- 添加帧
        if(pangolin::Pushed(addFrameBtn))
        {
            frames.push_back(Pose(
                (double)framePitch,
                (double)frameRoll,
                (double)frameYaw,
                (double)frameX,
                (double)frameY,
                (double)frameZ
            ));
        }

        if(pangolin::Pushed(addKeyFrameBtn))
        {
            frames.push_back(Pose(
                (double)framePitch,
                (double)frameRoll,
                (double)frameYaw,
                (double)frameX,
                (double)frameY,
                (double)frameZ,
                true
            ));
        }

        //尝试按照谢晓佳的视频中给出的代码绘制
        pangolin::glDrawAxis((double)axisSize);

        if(checkOrderBtn)
        {
            glRotatef((double)framePitch,1.0,0.0,0.0);
            glRotatef((double)frameRoll,0.0,0.0,1.0);
            glRotatef((double)frameYaw,0.0,1.0,0.0);
            glTranslatef((double)frameX,(double)frameY,(double)frameZ);
        }
        else
        {
            glTranslatef((double)frameX,(double)frameY,(double)frameZ);
            glRotatef((double)framePitch,1.0,0.0,0.0);
            glRotatef((double)frameRoll,0.0,0.0,1.0);
            glRotatef((double)frameYaw,0.0,1.0,0.0);
        }

        //绘制帧
        glPushMatrix();
        //glMultMatrixf(Twc.data());
        glColor3f(0.0f,0.0f,1.0f);
        drawFrame();
        drawAllFrames(frames,(bool)checkOrderBtn);
        glPopMatrix();

        //不要忘记了这个东西!!!
        glFlush();

        // Swap frames and Process Events
        pangolin::FinishFrame();
    }

    return 0;
}

void drawFrame(const float w)
{
    const float h=w*0.75;
    const float z=w*0.6;

    glLineWidth(2);

    glBegin(GL_LINES);

    glVertex3f(0,0,0);
    glVertex3f(w,h,z);
    glVertex3f(0,0,0);
    glVertex3f(w,-h,z);
    glVertex3f(0,0,0);
    glVertex3f(-w,-h,z);
    glVertex3f(0,0,0);
    glVertex3f(-w,h,z);
    glVertex3f(w,h,z);
    glVertex3f(w,-h,z);
    glVertex3f(-w,h,z);
    glVertex3f(-w,-h,z);
    glVertex3f(-w,h,z);
    glVertex3f(w,h,z);
    glVertex3f(-w,-h,z);
    glVertex3f(w,-h,z);

    glEnd();
}

double deg2rad(const double deg)
{
    return deg/180.0f*PI;
}

Eigen::Matrix3d degEuler2matrix(double pitch,double roll,double yaw)
{
    Eigen::Vector3d rotation_vector(
            deg2rad((double)yaw),
            deg2rad((double)pitch),
            deg2rad((double)roll));
    Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();
    rotation_matrix=Eigen::AngleAxisd(rotation_vector[0],Eigen::Vector3d::UnitZ())
        *Eigen::AngleAxisd(rotation_vector[1],Eigen::Vector3d::UnitY())
        *Eigen::AngleAxisd(rotation_vector[2],Eigen::Vector3d::UnitX());

    return rotation_matrix;

}

vector<GLfloat> eigen2glfloat(Eigen::Isometry3d T)
{
    //注意是列优先
    vector<GLfloat> res;
    for(int j=0;j<4;j++)
    {
        for(int i=0;i<4;i++)
        {
            res.push_back(T(i,j));
        }
    }
    return res;
}

void drawAllFrames(vector<Pose> frames,bool orderRT)
{
    size_t n=frames.size();

    for(int i=0;i<n;i++)
    {
        if(frames[i].keyFrame)
        {
            glColor3f(1.0f,0.0f,0.0f);
        }
        else
        {
            glColor3f(0.0f,1.0f,0.0f);
        }

        if(orderRT)
        {
            glRotatef(frames[i].pitch,1.0,0.0,0.0);
            glRotatef(frames[i].roll,0.0,0.0,1.0);
            glRotatef(frames[i].yaw,0.0,1.0,0.0);
            glTranslatef(frames[i].x,frames[i].y,frames[i].z);

            drawFrame();
        }
        else
        {
            glTranslatef(frames[i].x,frames[i].y,frames[i].z);
            glRotatef(frames[i].pitch,1.0,0.0,0.0);
            glRotatef(frames[i].roll,0.0,0.0,1.0);
            glRotatef(frames[i].yaw,0.0,1.0,0.0);
            drawFrame();
        }
    }
}


//
// $Id$
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// 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 <boost/cstdint.hpp>

#include "pwiz/utility/findmf/base/base/base.hpp"
#include "pwiz/utility/misc/unit.hpp"

namespace {
  using namespace pwiz::util;
	typedef boost::int32_t int32_t;
  // Tests that the Foo::Bar() method does Abc.
  void testseq() {
    std::vector<double> res;
    ralab::base::base::seq(1.,10.,0.5,res);
    ralab::base::base::seq(10.,2.,res);
    ralab::base::base::seq(2.,10.,res);
    ralab::base::base::seq(10., 2. , -0.5 , res);

    std::vector<int32_t> res2;
    ralab::base::base::seq(10,2,res2);
    ralab::base::base::seq(10,res);
    ralab::base::base::seq(res2,res);

    std::vector<unsigned int> resunsigned;
    ralab::base::base::seq(1u,10u,1u,resunsigned);
    ralab::base::base::seq(1u,10u,resunsigned);

    std::vector<double> resdouble;
    ralab::base::base::seq_length(100. , 1300.,18467,resdouble);
    unit_assert(resdouble.size() == 18467);
    ralab::base::base::seq_length(100. , 1300.,19467,resdouble);
    unit_assert(resdouble.size() == 19467);

    ralab::base::base::seq_length(0.,1000.,1000,resdouble);
    unit_assert(resdouble.size() == 1000);
  }

  // Tests that Foo does Xyz.
  void testmean() {
    std::vector<double> x;
    x.push_back(1.0);
    x.push_back(1.0);
    x.push_back(1.0);
    x.push_back(1.);
    x.push_back(2.);
    x.push_back(3.);
    x.push_back(5.);
    x.push_back(5.);
    x.push_back(6.);
    x.push_back(7.);
    x.push_back(8.);
    double res = ralab::base::base::mean(x);
    unit_assert_equal ( 3.636364, res, 1e-4);
    res = ralab::base::base::mean(x, 0.3);
    unit_assert_equal ( 3.2, res, 1e-4);
    res = ralab::base::base::mean(x, 0.4);
    unit_assert_equal ( 3.33333, res, 1e-4);
    res = ralab::base::base::mean(x, 0.5);
    std::cout << res << std::endl;
    unit_assert_equal ( 3., res, 1e-4);
    res = ralab::base::base::mean(x.begin(),x.end());
    unit_assert_equal ( 3.636364, res, 1e-4);

  }

  void testgeometricmean(){
    std::vector<double> x;
    x.push_back(1.0);
    x.push_back(2.0);
    x.push_back(3.0);

    double res = ralab::base::base::geometricMean(x.begin(), x.end());
    unit_assert_equal( 1.817121, res, 1e-4 );

  }
}  // namespace

int main(int argc, char **argv) {
 testseq();
testmean();
testgeometricmean();
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing rsqrt capabilities

    Returns the inverse of the square root of the input.

    @par semantic:

    For any given value @c x of floating type @c T:

    @code
    T r = rsqrt(x);
    @endcode

    For signed type is similar to:

    @code
    T r = T(1)/sqrt(x)
    @endcode

    @par Note

    If full accuracy is not needed a sometimes faster less accurate version of the function
    can be sppeded by the fast_ decorator : fast_(rsqrt)(x).

    @par Decorators

    fast_ for floating entries

  **/
  Value rsqrt(Value const & v0);
} }
#endif

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

#endif


//          Copyright Gavin Band 2008 - 2012.
// 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/bind.hpp>
#include "test_case.hpp"
#include "integration/NewtonRaphson.hpp"
#include <Eigen/Dense>

namespace impl {
	double function_1( double v ) {
		return -v*v + v + 2.0 ;
	}

	double derivative_1( double v ) {
		return -2.0 * v + 1 ;
	}

	double quadratic( double v, double a, double b, double c ) {
		return a*v*v + b*v + c ;
	}

	double quadratic_derivative( double v, double a, double b ) {
		return 2*a*v + b ;
	}

	Eigen::VectorXd affine_map( Eigen::VectorXd const& point, Eigen::MatrixXd const& matrix, Eigen::VectorXd const& offset ) {
		return ( matrix * ( point - offset ) ) ;
	}

	Eigen::MatrixXd affine_derivative( Eigen::VectorXd const& point, Eigen::MatrixXd const& matrix ) {
		return matrix ;
	}

	Eigen::VectorXd another_map( Eigen::VectorXd const& point, Eigen::MatrixXd const& matrix, Eigen::VectorXd const& offset ) {
		// A linear map plus quadratic deviation.
		Eigen::VectorXd v( 2 ) ;
		v << 	-( ( point - offset )( 0 ) * ( point - offset )( 0 ) ),
				-( ( point - offset )( 1 ) * ( point - offset )( 1 ) ) ;
		return ( matrix * ( point - offset ) ) + v ;
	}

	Eigen::MatrixXd another_derivative( Eigen::VectorXd const& point, Eigen::MatrixXd const& matrix, Eigen::VectorXd const& offset ) {
		Eigen::MatrixXd M( 2, 2 ) ;
		M <<	-2 * ( point( 0 ) - offset( 0 ) ),	0,
				0,									-2 * ( point( 1 ) - offset( 1 ) ) ;
		return matrix + M ;
	}
}

AUTO_TEST_CASE( test_newton_raphson_1d ) {
	std::cerr << "test_newton_raphson_1d(): finding roots of  5 x - 2...\n" ;
	for( double epsilon = 0.1; epsilon > 0.0000000000001; epsilon /= 10 ) {
		std::cerr << "epsilon = " << epsilon << ".\n" ;
		double root = integration::find_root_by_newton_raphson(
			boost::bind( impl::quadratic, _1, 0, 5, -2 ),
			boost::bind( impl::quadratic_derivative, _1, 0, 5 ),
			-10.0,
			epsilon
		) ;

		std::cerr << "root: " << root << ".\n" ;
		TEST_ASSERT( std::abs( root - ( 2.0 / 5.0 )) < epsilon ) ;
	}

	std::cerr << "test_newton_raphson_1d(): finding roots of 2 + x - x^2...\n" ;
	for( double epsilon = 0.1; epsilon > 0.0000000000001; epsilon /= 10 ) {
		std::cerr << "epsilon = " << epsilon << ".\n" ;
		double left_root = integration::find_root_by_newton_raphson(
			boost::bind( impl::quadratic, _1, -1, 1, 2 ),
			boost::bind( impl::quadratic_derivative, _1, -1, 1 ),
			-10.0,
			epsilon
		) ;
		std::cerr << "Left root: " << left_root << ".\n" ;
		TEST_ASSERT( std::abs( left_root + 1 ) < epsilon ) ;

		double right_root = integration::find_root_by_newton_raphson(
			&impl::function_1,
			&impl::derivative_1,
			10.0,
			epsilon
		) ;

		std::cerr << "Right root: " << right_root << ".\n" ;
		TEST_ASSERT( std::abs( right_root - 2 ) < epsilon ) ;
	}
}

AUTO_TEST_CASE( test_newton_raphson_nd_1 ) {
	std::cerr << "test_newton_raphson_nd_1(): finding roots of \\Sigma x = 0...\n" ;

	Eigen::MatrixXd sigma ;
	sigma.resize( 2, 2 ) ;
	sigma( 0, 0 ) = 5 ;
	sigma( 1, 1 ) = 5 ;
	sigma( 0, 1 ) = 0.5 ;
	sigma( 1, 0 ) = 0.5 ;

	Eigen::VectorXd actual_root = Eigen::VectorXd( 2 ) ;
	actual_root << 50, 50 ;

	Eigen::VectorXd initial_point( 2 ) ;
	initial_point << -100.0, -100.0 ;
	for( double epsilon = 0.1; epsilon > 0.0000000000001; epsilon /= 10 ) {
		std::cerr << "==================== epsilon = " << epsilon << ".\n" ;
		Eigen::VectorXd root = integration::find_root_by_newton_raphson(
			boost::bind( impl::affine_map, _1, sigma, actual_root ),
			boost::bind( impl::affine_derivative, _1, sigma ),
			initial_point,
			epsilon
		) ;

		std::cerr << "root is " << root << ".\n" ;
		TEST_ASSERT( ( root - actual_root ).norm() < epsilon ) ;
	}
}

AUTO_TEST_CASE( test_newton_raphson_nd_2 ) {
	std::cerr << "test_newton_raphson_nd_2(): finding roots of \\Sigma x = 0...\n" ;

	Eigen::MatrixXd sigma ; // variance-covariance matrix
	sigma.resize( 2, 2 ) ;
	sigma( 0, 0 ) = 5 ;
	sigma( 1, 1 ) = 5 ;
	sigma( 0, 1 ) = 0 ;
	sigma( 1, 0 ) = 0 ;

	Eigen::VectorXd actual_root = Eigen::VectorXd( 2 ) ;
	actual_root << 50, 50 ;

	Eigen::VectorXd initial_point( 2 ) ;
	initial_point << -100.0, -100.0 ;
	for( double epsilon = 0.1; epsilon > 0.000000000000001; epsilon /= 10 ) {
		std::cerr << "============== epsilon = " << epsilon << ".\n" ;
		Eigen::VectorXd root = integration::find_root_by_newton_raphson(
			boost::bind( impl::another_map, _1, sigma, actual_root ),
			boost::bind( impl::another_derivative, _1, sigma, actual_root ),
			initial_point,
			epsilon
		) ;

		std::cerr << "root is " << root << ".\n" ;
		TEST_ASSERT( impl::another_map( root, sigma, actual_root ).maxCoeff() < epsilon ) ;
	}
}


/**
 * @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_; }


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Adaptors/Zip.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Random.h"
#include "Molassembler/Temple/constexpr/Math.h"
#include "Molassembler/Temple/constexpr/Jsf.h"
#include "Molassembler/Temple/constexpr/FloatingPointComparison.h"
#include "Molassembler/Temple/Stringify.h"

#include <iomanip>
#include <iostream>

using namespace Scine::Molassembler;
extern Temple::Generator<> generator;

static_assert(Temple::Math::factorial(5) == 120, "Factorial is incorrect");
static_assert(Temple::Math::factorial(0) == 1, "Factorial is incorrect");

namespace {

constexpr unsigned numTests = 100;
constexpr double relativeAccuracy = 1e-12;

static_assert(
  relativeAccuracy >= std::numeric_limits<double>::epsilon(),
  "Testing relative accuracy must be greater than machine epsilon!"
);

template<typename F, typename G>
auto compareImplFn(F&& testFn, G&& referenceFn, double accuracy=relativeAccuracy) {
  return [=](const auto ... values) {
    const double testValue = testFn(values...);
    const double referenceValue = referenceFn(values...);

    BOOST_TEST_CONTEXT(
      "  x = " << std::setw(12) << Temple::stringify(std::tie(values...))
      << ", y = " << std::setw(12) << testValue
      << ", ref = " << std::setw(12) << referenceValue
      << ", |Δ| = " << std::setw(12) << std::fabs(testValue - referenceValue)
    ) {
      BOOST_CHECK(
        Temple::Floating::isCloseRelative(testValue, referenceValue, accuracy)
      );
    }
  };
}

} // namespace

BOOST_AUTO_TEST_CASE(ConstexprSqrt, *boost::unit_test::label("Temple")) {
  Temple::forEach(
    Temple::Random::getN<double>(0, 1e6, numTests, generator.engine),
    compareImplFn(
      [](double x) { return Temple::Math::sqrt(x); },
      [](double x) { return std::sqrt(x); }
    )
  );
}

BOOST_AUTO_TEST_CASE(ConstexprAsin, *boost::unit_test::label("Temple")) {
  // asin
  const auto randomInverseTrigNumbers = Temple::Random::getN<double>(
    std::nexttoward(-1.0, 0.0),
    std::nexttoward(1.0, 0.0),
    numTests,
    generator.engine
  );

  Temple::forEach(
    randomInverseTrigNumbers,
    compareImplFn(
      [](double x) { return Temple::Math::asin(x); },
      [](double x) { return std::asin(x); },
      1e-8
    )
  );
}

BOOST_AUTO_TEST_CASE(ConstexprPow, *boost::unit_test::label("Temple")) {
  Temple::forEach(
    Temple::Adaptors::zip(
      Temple::Random::getN<double>(-1e5, 1e5, numTests, generator.engine),
      Temple::Random::getN<int>(-40, 40, numTests, generator.engine)
    ),
    compareImplFn(
      [](double x, int y) { return Temple::Math::pow(x, y); },
      [](double x, int y) { return std::pow(x, y); }
    )
  );
}

BOOST_AUTO_TEST_CASE(ConstexprRecPow, *boost::unit_test::label("Temple")) {
  Temple::forEach(
    Temple::Adaptors::zip(
      Temple::Random::getN<double>(-1e5, 1e5, numTests, generator.engine),
      Temple::Random::getN<unsigned>(0, 40, numTests, generator.engine)
    ),
    compareImplFn(
      [](double x, unsigned y) { return Temple::Math::recPow(x, y); },
      [](double x, unsigned y) { return std::pow(x, y); }
    )
  );
}

BOOST_AUTO_TEST_CASE(ConstexprLn, *boost::unit_test::label("Temple")) {
  Temple::forEach(
    Temple::Random::getN<double>(1e-10, 1e10, numTests, generator.engine),
    compareImplFn(
      [](double x) { return Temple::Math::ln(x); },
      [](double x) { return std::log(x); }
    )
  );
}

BOOST_AUTO_TEST_CASE(ConstexprAtan, *boost::unit_test::label("Temple")) {
  Temple::forEach(
    Temple::Random::getN<double>(-M_PI / 2, M_PI / 2, numTests, generator.engine),
    compareImplFn(
      [](double x) { return Temple::Math::atan(x); },
      [](double x) { return std::atan(x); }
    )
  );
}

BOOST_AUTO_TEST_CASE(ConstexprFloorCeil, *boost::unit_test::label("Temple")) {
  BOOST_CHECK(
    Temple::all_of(
      Temple::Random::getN<double>(-100, 100, numTests, generator.engine),
      [](const double x) -> bool {
        return(Temple::Math::floor(x) <= x);
      }
    )
  );

  BOOST_CHECK(
    Temple::all_of(
      Temple::Random::getN<double>(-100, 100, numTests, generator.engine),
      [](const double x) -> bool {
        return(Temple::Math::ceil(x) >= x);
      }
    )
  );
}

namespace {

template<
  template<typename> class Comparator,
  typename T
> constexpr bool testComparison(const T a, const T b, const T tolerance) {
  Comparator<T> comparator { tolerance };

  return (
    Temple::Math::XOR(
      (
        comparator.isLessThan(a, b)
        && comparator.isMoreThan(b, a)
        && comparator.isUnequal(a, b)
      ),
      (
        comparator.isLessThan(b, a)
        && comparator.isMoreThan(a, b)
        && comparator.isUnequal(a, b)
      ),
      (
        !comparator.isLessThan(a, b)
        && !comparator.isMoreThan(a, b)
        && comparator.isEqual(a, b)
      )
    ) && Temple::Math::XOR(
      comparator.isEqual(a, b),
      comparator.isUnequal(a, b)
    )
  );
}

using namespace Temple::Floating;

static_assert(
  testComparison<ExpandedAbsoluteEqualityComparator>(4.3, 3.9, 1e-4)
  && testComparison<ExpandedAbsoluteEqualityComparator>(4.3, 3.9, 1.0)
  && testComparison<ExpandedAbsoluteEqualityComparator>(4.4, 4.4, 1e-10),
  "absolute comparison has inconsistent operators!"
);

static_assert(
  testComparison<ExpandedRelativeEqualityComparator>(4.3, 3.9, 1e-4)
  && testComparison<ExpandedRelativeEqualityComparator>(4.3, 3.9, 1.0)
  && testComparison<ExpandedRelativeEqualityComparator>(4.4, 4.4, 1e-10),
  "relative comparison has inconsistent operators!"
);

} // namespace


#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);
      }
    }
  }
}