#include <Python.h>
#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <numpy/arrayobject.h>
#include "ID_Daw.h"

/*
This module uses IDDAW.cpp to find the energy of a set of interlayer dislocations
It can be built using setup_ID_DAW.py
and tested using test_IDDAW.py

https://dfm.io/posts/python-c-extensions/
*/

static char module_docstring[] =
    "This module provides an interface for calculating the Continuum interlayer energy.";
static char Energyf_sp_docstring[] =
    "Calculate the Energy of a set of interlayer Dislocations using Daw's formalism.";
static char visDuftot_docstring[] =
    "Calculate the Distortion field of a set of interlayer Dislocations using Daw's formalism.";
static char visuftot_docstring[] =
    "Calculate the in-plane displacement field of a set of interlayer Dislocations using Daw's formalism with stacking.";

static PyObject *IDDAW_Energyf_sp(PyObject *self, PyObject *args);
static PyObject *IDDAW_visDuftot(PyObject *self, PyObject *args);
static PyObject *IDDAW_visuftot(PyObject *self, PyObject *args);

MatrixXd unpacklocbur(double* , int);

static PyMethodDef module_methods[] = {
    {"_Energyf_sp", IDDAW_Energyf_sp, METH_VARARGS, Energyf_sp_docstring},
    {"_visDuftot", IDDAW_visDuftot, METH_VARARGS, visDuftot_docstring},
    {"_visuftot", IDDAW_visuftot, METH_VARARGS, visuftot_docstring},
    {NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC init_IDDAW(void)
{
    PyObject *m = Py_InitModule3("_IDDAW", module_methods, module_docstring);
    if (m == NULL)
        return;

    /* Load `numpy` functionality. */
    import_array();
}

static PyObject *IDDAW_Energyf_sp(PyObject *self, PyObject *args)
{
    double rc, kap, c33, z0, alpha;
    int ndisl, ndislout, pmax, qmax, ncoor;
    PyObject *Cijkl_obj, *Lxy_obj, *a1_obj, *a2_obj, *loc_obj, *burgers_obj, *fG1_obj, *fG2_obj, *locout_obj, *dirout_obj, *burgersout_obj, *rcout_obj, *M_obj, *X_obj;

    /* Parse the input tuple */
    if (!PyArg_ParseTuple(args, "OOOOOOdiOOiiOOOOidddOiOd", &Cijkl_obj, &Lxy_obj, &a1_obj, &a2_obj,
        &loc_obj, &burgers_obj, &rc, &ndisl, &fG1_obj, &fG2_obj, &pmax, &qmax,
        &locout_obj, &dirout_obj, &burgersout_obj, &rcout_obj, &ndislout, &kap, &c33, &z0, &M_obj, &ncoor, &X_obj, &alpha))
        return NULL;

    /* Interpret the input objects as numpy arrays. */
    PyObject *Cijkl_array = PyArray_FROM_OTF(Cijkl_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *Lxy_array = PyArray_FROM_OTF(Lxy_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a1_array = PyArray_FROM_OTF(a1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a2_array = PyArray_FROM_OTF(a2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *loc_array = PyArray_FROM_OTF(loc_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgers_array = PyArray_FROM_OTF(burgers_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG1_array = PyArray_FROM_OTF(fG1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG2_array = PyArray_FROM_OTF(fG2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *locout_array = PyArray_FROM_OTF(locout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *dirout_array = PyArray_FROM_OTF(dirout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgersout_array = PyArray_FROM_OTF(burgersout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *rcout_array = PyArray_FROM_OTF(rcout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *M_array = PyArray_FROM_OTF(M_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *X_array = PyArray_FROM_OTF(X_obj, NPY_DOUBLE, NPY_IN_ARRAY);

    /* If that didn't work, throw an exception. */
    if (Cijkl_array == NULL || Lxy_array == NULL || a1_array == NULL ||
        a2_array == NULL || loc_array == NULL || burgers_array == NULL ||
        fG1_array == NULL || fG2_array == NULL || locout_array == NULL ||
        burgersout_array == NULL || dirout_array == NULL || M_array == NULL || X_array == NULL) {
        Py_XDECREF(Cijkl_array);
        Py_XDECREF(Lxy_array);
        Py_XDECREF(a1_array);
        Py_XDECREF(a2_array);
        Py_XDECREF(loc_array);
        Py_XDECREF(burgers_array);
        Py_XDECREF(fG1_array);
        Py_XDECREF(fG2_array);
        Py_XDECREF(locout_array);
        Py_XDECREF(dirout_array);
        Py_XDECREF(burgersout_array);
        Py_XDECREF(rcout_array);
        Py_XDECREF(M_array);
        Py_XDECREF(X_array);
        return NULL;
    }

    /* Get pointers to the data as C-types. */
    double *Cijklp    = (double*)PyArray_DATA(Cijkl_array);
    double *Lxyp    = (double*)PyArray_DATA(Lxy_array);
    double *a1p = (double*)PyArray_DATA(a1_array);
    double *a2p = (double*)PyArray_DATA(a2_array);
    double *locp = (double*)PyArray_DATA(loc_array);
    double *burgersp = (double*)PyArray_DATA(burgers_array);
    double *fG1p = (double*)PyArray_DATA(fG1_array);
    double *fG2p = (double*)PyArray_DATA(fG2_array);
    double *locoutp = (double*)PyArray_DATA(locout_array);
    double *diroutp = (double*)PyArray_DATA(dirout_array);
    double *burgersoutp = (double*)PyArray_DATA(burgersout_array);
    double *rcoutp = (double*)PyArray_DATA(rcout_array);
    double *Mp = (double*)PyArray_DATA(M_array);
    double *Xp = (double*)PyArray_DATA(X_array);

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;

    //Convert Pointers into C Data Types
    MatrixXd C(2,8);
    C(0,0) = Cijklp[0]; C(0,1) = Cijklp[1]; C(0,2) = Cijklp[2]; C(0,3) = Cijklp[3];
    C(0,4) = Cijklp[4]; C(0,5) = Cijklp[5]; C(0,6) = Cijklp[6]; C(0,7) = Cijklp[7];
    C(1,0) = Cijklp[8]; C(1,1) = Cijklp[9]; C(1,2) = Cijklp[10]; C(1,3) = Cijklp[11];
    C(1,4) = Cijklp[12]; C(1,5) = Cijklp[13]; C(1,6) = Cijklp[14]; C(1,7) = Cijklp[15];

    MatrixXd burgers(ndisl,2);
    MatrixXd loc(ndisl,2);

    burgers = unpacklocbur(burgersp,ndisl);
    loc = unpacklocbur(locp,ndisl);

    MatrixXd burgersout(ndislout,2);
    MatrixXd locout(ndislout,2);
    MatrixXd dirout(ndislout,2);

    burgersout = unpacklocbur(burgersoutp,ndislout);
    locout = unpacklocbur(locoutp,ndislout);
    dirout = unpacklocbur(diroutp,ndislout);
    VectorXd rcout(2*ndislout);
    for (int k = 0; k<2*ndislout; k++){
        rcout(k) = rcoutp[k];
    }

    Vector2d Lxy(Lxyp[0],Lxyp[1]);
    Vector2d a1(a1p[0],a1p[1]);
    Vector2d a2(a2p[0],a2p[1]);

    Vector2d M(Mp[0],Mp[1]);

    MatrixXd fG1 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG1p,2*pmax+1,2*qmax+1);
    MatrixXd fG2 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG2p,2*pmax+1,2*qmax+1);

    MatrixXd X = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(Xp,ncoor,2);

    /*
    cout << "Cijkl" << endl << C << endl;
    cout << "kap" << endl << kap << endl;
    cout << "c33" << endl << c33 << endl;
    cout << "Lxy" << endl << Lxy << endl;
    cout << "a1" << endl << a1 << endl;
    cout << "a2" << endl << a2 << endl;
    cout << "fG1" << endl << fG1 << endl;
    cout << "fG2" << endl << fG2 << endl;
    cout << "loc" << endl << loc << endl;
    cout << "burgers" << endl << burgers << endl;
    cout << "rc" << endl << rc << endl;
    cout << "loc_out" << endl << locout << endl;
    cout << "dir_out" << endl << dirout << endl;
    cout << "burgers_out" << endl << burgersout << endl;
    cout << "rc_out" << endl << rcout << endl;
    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;
    cout << "z0" << endl << z0 << endl;
    cout << "M" << endl << M << endl;
    */
    Vector3d res = energyf_sp(C, kap, c33, Lxy, a1, a2, fG1, fG2, loc, burgers, rc, locout, dirout, burgersout, rcout, pmax, qmax, z0, M, ncoor, X, alpha);

    //double value = res[0];
    double ret_array[3];
    ret_array[0] = res[0];
    ret_array[1] = res[1];
    ret_array[2] = res[2];

    /* Clean up. */
    Py_XDECREF(Cijkl_array);
    Py_XDECREF(Lxy_array);
    Py_XDECREF(a1_array);
    Py_XDECREF(a2_array);
    Py_XDECREF(loc_array);
    Py_XDECREF(burgers_array);
    Py_XDECREF(fG1_array);
    Py_XDECREF(fG2_array);
    Py_XDECREF(locout_array);
    Py_XDECREF(dirout_array);
    Py_XDECREF(burgersout_array);
    Py_XDECREF(rcout_array);
    Py_XDECREF(M_array);
    Py_XDECREF(X_array);

    npy_intp dims[1] = {3};
    PyObject *ret = PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    memcpy(PyArray_DATA(ret), ret_array, sizeof(ret_array));

    /* Build the output tuple */
    //PyObject *ret = Py_BuildValue("d", value);
    return ret;
}

static PyObject *IDDAW_visDuftot(PyObject *self, PyObject *args)
{
    double rc, kap, c33, z0, alpha;
    int ndisl, ndislout, pmax, qmax, ncoor;
    PyObject *Cijkl_obj, *Lxy_obj, *a1_obj, *a2_obj, *loc_obj, *burgers_obj, *fG1_obj, *fG2_obj, *locout_obj, *dirout_obj, *burgersout_obj, *rcout_obj, *M_obj, *X_obj;

    /* Parse the input tuple */
    if (!PyArg_ParseTuple(args, "OOOOOOdiOOiiOOOOidddOiOd", &Cijkl_obj, &Lxy_obj, &a1_obj, &a2_obj,
        &loc_obj, &burgers_obj, &rc, &ndisl, &fG1_obj, &fG2_obj, &pmax, &qmax,
        &locout_obj, &dirout_obj, &burgersout_obj, &rcout_obj, &ndislout, &kap, &c33, &z0, &M_obj, &ncoor, &X_obj, &alpha))
        return NULL;

    /* Interpret the input objects as numpy arrays. */
    PyObject *Cijkl_array = PyArray_FROM_OTF(Cijkl_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *Lxy_array = PyArray_FROM_OTF(Lxy_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a1_array = PyArray_FROM_OTF(a1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a2_array = PyArray_FROM_OTF(a2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *loc_array = PyArray_FROM_OTF(loc_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgers_array = PyArray_FROM_OTF(burgers_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG1_array = PyArray_FROM_OTF(fG1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG2_array = PyArray_FROM_OTF(fG2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *locout_array = PyArray_FROM_OTF(locout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *dirout_array = PyArray_FROM_OTF(dirout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgersout_array = PyArray_FROM_OTF(burgersout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *rcout_array = PyArray_FROM_OTF(rcout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *M_array = PyArray_FROM_OTF(M_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *X_array = PyArray_FROM_OTF(X_obj, NPY_DOUBLE, NPY_IN_ARRAY);

    /* If that didn't work, throw an exception. */
    if (Cijkl_array == NULL || Lxy_array == NULL || a1_array == NULL ||
        a2_array == NULL || loc_array == NULL || burgers_array == NULL ||
        fG1_array == NULL || fG2_array == NULL || locout_array == NULL ||
        burgersout_array == NULL || dirout_array == NULL || M_array == NULL || X_array == NULL) {
        Py_XDECREF(Cijkl_array);
        Py_XDECREF(Lxy_array);
        Py_XDECREF(a1_array);
        Py_XDECREF(a2_array);
        Py_XDECREF(loc_array);
        Py_XDECREF(burgers_array);
        Py_XDECREF(fG1_array);
        Py_XDECREF(fG2_array);
        Py_XDECREF(locout_array);
        Py_XDECREF(dirout_array);
        Py_XDECREF(burgersout_array);
        Py_XDECREF(rcout_array);
        Py_XDECREF(M_array);
        Py_XDECREF(X_array);
        return NULL;
    }

    /* Get pointers to the data as C-types. */
    double *Cijklp    = (double*)PyArray_DATA(Cijkl_array);
    double *Lxyp    = (double*)PyArray_DATA(Lxy_array);
    double *a1p = (double*)PyArray_DATA(a1_array);
    double *a2p = (double*)PyArray_DATA(a2_array);
    double *locp = (double*)PyArray_DATA(loc_array);
    double *burgersp = (double*)PyArray_DATA(burgers_array);
    double *fG1p = (double*)PyArray_DATA(fG1_array);
    double *fG2p = (double*)PyArray_DATA(fG2_array);
    double *locoutp = (double*)PyArray_DATA(locout_array);
    double *diroutp = (double*)PyArray_DATA(dirout_array);
    double *burgersoutp = (double*)PyArray_DATA(burgersout_array);
    double *rcoutp = (double*)PyArray_DATA(rcout_array);
    double *Mp = (double*)PyArray_DATA(M_array);
    double *Xp = (double*)PyArray_DATA(X_array);

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;

    //Convert Pointers into C Data Types
    MatrixXd C(2,8);
    C(0,0) = Cijklp[0]; C(0,1) = Cijklp[1]; C(0,2) = Cijklp[2]; C(0,3) = Cijklp[3];
    C(0,4) = Cijklp[4]; C(0,5) = Cijklp[5]; C(0,6) = Cijklp[6]; C(0,7) = Cijklp[7];
    C(1,0) = Cijklp[8]; C(1,1) = Cijklp[9]; C(1,2) = Cijklp[10]; C(1,3) = Cijklp[11];
    C(1,4) = Cijklp[12]; C(1,5) = Cijklp[13]; C(1,6) = Cijklp[14]; C(1,7) = Cijklp[15];

    MatrixXd burgers(ndisl,2);
    MatrixXd loc(ndisl,2);

    burgers = unpacklocbur(burgersp,ndisl);
    loc = unpacklocbur(locp,ndisl);

    MatrixXd burgersout(ndislout,2);
    MatrixXd locout(ndislout,2);
    MatrixXd dirout(ndislout,2);

    burgersout = unpacklocbur(burgersoutp,ndislout);
    locout = unpacklocbur(locoutp,ndislout);
    dirout = unpacklocbur(diroutp,ndislout);
    VectorXd rcout(2*ndislout);
    for (int k = 0; k<2*ndislout; k++){
        rcout(k) = rcoutp[k];
    }

    Vector2d Lxy(Lxyp[0],Lxyp[1]);
    Vector2d a1(a1p[0],a1p[1]);
    Vector2d a2(a2p[0],a2p[1]);

    Vector2d M(Mp[0],Mp[1]);

    MatrixXd fG1 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG1p,2*pmax+1,2*qmax+1);
    MatrixXd fG2 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG2p,2*pmax+1,2*qmax+1);

    MatrixXd X = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(Xp,ncoor,2);

    /*
    cout << "Cijkl" << endl << C << endl;
    cout << "kap" << endl << kap << endl;
    cout << "c33" << endl << c33 << endl;
    cout << "Lxy" << endl << Lxy << endl;
    cout << "a1" << endl << a1 << endl;
    cout << "a2" << endl << a2 << endl;
    cout << "fG1" << endl << fG1 << endl;
    cout << "fG2" << endl << fG2 << endl;
    cout << "loc" << endl << loc << endl;
    cout << "burgers" << endl << burgers << endl;
    cout << "rc" << endl << rc << endl;
    cout << "loc_out" << endl << locout << endl;
    cout << "dir_out" << endl << dirout << endl;
    cout << "burgers_out" << endl << burgersout << endl;
    cout << "rc_out" << endl << rcout << endl;
    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;
    cout << "z0" << endl << z0 << endl;
    cout << "M" << endl << M << endl;
    */
    MatrixXd res = visD_uftot(C, kap, c33, Lxy, a1, a2, fG1, fG2, loc, burgers, rc, locout, dirout, burgersout, rcout, pmax, qmax, z0, M, ncoor, X, alpha);

    //cout << "Return: " << res << endl;
    //double value = res[0];

    double ret_array[8*ncoor];

    for (int i = 0; i < ncoor; i++){
        for (int j = 0; j < 8; j++){
            ret_array[8*i+j] = res(i,j);
        }
    }

    /* Clean up. */
    Py_XDECREF(Cijkl_array);
    Py_XDECREF(Lxy_array);
    Py_XDECREF(a1_array);
    Py_XDECREF(a2_array);
    Py_XDECREF(loc_array);
    Py_XDECREF(burgers_array);
    Py_XDECREF(fG1_array);
    Py_XDECREF(fG2_array);
    Py_XDECREF(locout_array);
    Py_XDECREF(dirout_array);
    Py_XDECREF(burgersout_array);
    Py_XDECREF(rcout_array);
    Py_XDECREF(M_array);
    Py_XDECREF(X_array);

    //cout << "Return: " << ret_array << endl;

    npy_intp dims[1] = {8*ncoor};
    PyObject *ret = PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    memcpy(PyArray_DATA(ret), ret_array, sizeof(ret_array));

    /* Build the output tuple */
    //PyObject *ret = Py_BuildValue("d", value);
    return ret;
}

static PyObject *IDDAW_visuftot(PyObject *self, PyObject *args)
{
    double rc, kap, c33, z0, alpha;
    int ndisl, ndislout, pmax, qmax, ncoor;
    PyObject *Cijkl_obj, *Lxy_obj, *a1_obj, *a2_obj, *loc_obj, *burgers_obj, *fG1_obj, *fG2_obj, *locout_obj, *dirout_obj, *burgersout_obj, *rcout_obj, *M_obj, *X_obj;

    /* Parse the input tuple */
    if (!PyArg_ParseTuple(args, "OOOOOOdiOOiiOOOOidddOiOd", &Cijkl_obj, &Lxy_obj, &a1_obj, &a2_obj,
        &loc_obj, &burgers_obj, &rc, &ndisl, &fG1_obj, &fG2_obj, &pmax, &qmax,
        &locout_obj, &dirout_obj, &burgersout_obj, &rcout_obj, &ndislout, &kap, &c33, &z0, &M_obj, &ncoor, &X_obj, &alpha))
        return NULL;

    /* Interpret the input objects as numpy arrays. */
    PyObject *Cijkl_array = PyArray_FROM_OTF(Cijkl_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *Lxy_array = PyArray_FROM_OTF(Lxy_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a1_array = PyArray_FROM_OTF(a1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a2_array = PyArray_FROM_OTF(a2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *loc_array = PyArray_FROM_OTF(loc_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgers_array = PyArray_FROM_OTF(burgers_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG1_array = PyArray_FROM_OTF(fG1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG2_array = PyArray_FROM_OTF(fG2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *locout_array = PyArray_FROM_OTF(locout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *dirout_array = PyArray_FROM_OTF(dirout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgersout_array = PyArray_FROM_OTF(burgersout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *rcout_array = PyArray_FROM_OTF(rcout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *M_array = PyArray_FROM_OTF(M_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *X_array = PyArray_FROM_OTF(X_obj, NPY_DOUBLE, NPY_IN_ARRAY);

    /* If that didn't work, throw an exception. */
    if (Cijkl_array == NULL || Lxy_array == NULL || a1_array == NULL ||
        a2_array == NULL || loc_array == NULL || burgers_array == NULL ||
        fG1_array == NULL || fG2_array == NULL || locout_array == NULL ||
        burgersout_array == NULL || dirout_array == NULL || M_array == NULL || X_array == NULL) {
        Py_XDECREF(Cijkl_array);
        Py_XDECREF(Lxy_array);
        Py_XDECREF(a1_array);
        Py_XDECREF(a2_array);
        Py_XDECREF(loc_array);
        Py_XDECREF(burgers_array);
        Py_XDECREF(fG1_array);
        Py_XDECREF(fG2_array);
        Py_XDECREF(locout_array);
        Py_XDECREF(dirout_array);
        Py_XDECREF(burgersout_array);
        Py_XDECREF(rcout_array);
        Py_XDECREF(M_array);
        Py_XDECREF(X_array);
        return NULL;
    }

    /* Get pointers to the data as C-types. */
    double *Cijklp    = (double*)PyArray_DATA(Cijkl_array);
    double *Lxyp    = (double*)PyArray_DATA(Lxy_array);
    double *a1p = (double*)PyArray_DATA(a1_array);
    double *a2p = (double*)PyArray_DATA(a2_array);
    double *locp = (double*)PyArray_DATA(loc_array);
    double *burgersp = (double*)PyArray_DATA(burgers_array);
    double *fG1p = (double*)PyArray_DATA(fG1_array);
    double *fG2p = (double*)PyArray_DATA(fG2_array);
    double *locoutp = (double*)PyArray_DATA(locout_array);
    double *diroutp = (double*)PyArray_DATA(dirout_array);
    double *burgersoutp = (double*)PyArray_DATA(burgersout_array);
    double *rcoutp = (double*)PyArray_DATA(rcout_array);
    double *Mp = (double*)PyArray_DATA(M_array);
    double *Xp = (double*)PyArray_DATA(X_array);

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;

    //Convert Pointers into C Data Types
    MatrixXd C(2,8);
    C(0,0) = Cijklp[0]; C(0,1) = Cijklp[1]; C(0,2) = Cijklp[2]; C(0,3) = Cijklp[3];
    C(0,4) = Cijklp[4]; C(0,5) = Cijklp[5]; C(0,6) = Cijklp[6]; C(0,7) = Cijklp[7];
    C(1,0) = Cijklp[8]; C(1,1) = Cijklp[9]; C(1,2) = Cijklp[10]; C(1,3) = Cijklp[11];
    C(1,4) = Cijklp[12]; C(1,5) = Cijklp[13]; C(1,6) = Cijklp[14]; C(1,7) = Cijklp[15];

    MatrixXd burgers(ndisl,2);
    MatrixXd loc(ndisl,2);

    burgers = unpacklocbur(burgersp,ndisl);
    loc = unpacklocbur(locp,ndisl);

    MatrixXd burgersout(ndislout,2);
    MatrixXd locout(ndislout,2);
    MatrixXd dirout(ndislout,2);

    burgersout = unpacklocbur(burgersoutp,ndislout);
    locout = unpacklocbur(locoutp,ndislout);
    dirout = unpacklocbur(diroutp,ndislout);
    VectorXd rcout(2*ndislout);
    for (int k = 0; k<2*ndislout; k++){
        rcout(k) = rcoutp[k];
    }

    Vector2d Lxy(Lxyp[0],Lxyp[1]);
    Vector2d a1(a1p[0],a1p[1]);
    Vector2d a2(a2p[0],a2p[1]);

    Vector2d M(Mp[0],Mp[1]);

    MatrixXd fG1 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG1p,2*pmax+1,2*qmax+1);
    MatrixXd fG2 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG2p,2*pmax+1,2*qmax+1);

    MatrixXd X = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(Xp,ncoor,2);

    /*
    cout << "Cijkl" << endl << C << endl;
    cout << "kap" << endl << kap << endl;
    cout << "c33" << endl << c33 << endl;
    cout << "Lxy" << endl << Lxy << endl;
    cout << "a1" << endl << a1 << endl;
    cout << "a2" << endl << a2 << endl;
    cout << "fG1" << endl << fG1 << endl;
    cout << "fG2" << endl << fG2 << endl;
    cout << "loc" << endl << loc << endl;
    cout << "burgers" << endl << burgers << endl;
    cout << "rc" << endl << rc << endl;
    cout << "loc_out" << endl << locout << endl;
    cout << "dir_out" << endl << dirout << endl;
    cout << "burgers_out" << endl << burgersout << endl;

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;
    cout << "z0" << endl << z0 << endl;
    cout << "M" << endl << M << endl;
    */
    cout << "rc_out" << endl << rcout << endl;
    MatrixXd res = visuftot(C, kap, c33, Lxy, a1, a2, fG1, fG2, loc, burgers, rc, locout, dirout, burgersout, rcout, pmax, qmax, z0, M, ncoor, X, alpha);

    //cout << "Return: " << res << endl;
    //double value = res[0];

    double ret_array[4*ncoor];

    for (int i = 0; i < ncoor; i++){
        for (int j = 0; j < 4; j++){
            ret_array[4*i+j] = res(i,j);
        }
    }

    /* Clean up. */
    Py_XDECREF(Cijkl_array);
    Py_XDECREF(Lxy_array);
    Py_XDECREF(a1_array);
    Py_XDECREF(a2_array);
    Py_XDECREF(loc_array);
    Py_XDECREF(burgers_array);
    Py_XDECREF(fG1_array);
    Py_XDECREF(fG2_array);
    Py_XDECREF(locout_array);
    Py_XDECREF(dirout_array);
    Py_XDECREF(burgersout_array);
    Py_XDECREF(rcout_array);
    Py_XDECREF(M_array);
    Py_XDECREF(X_array);

    //cout << "Return: " << ret_array << endl;

    npy_intp dims[1] = {4*ncoor};
    PyObject *ret = PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    memcpy(PyArray_DATA(ret), ret_array, sizeof(ret_array));

    /* Build the output tuple */
    //PyObject *ret = Py_BuildValue("d", value);
    return ret;
}

MatrixXd unpacklocbur(double* point, int ndisl){
	MatrixXd out(ndisl,2);
	for (int i=0;i<ndisl;i++){
		out(i,0) = point[i*2];
		out(i,1) = point[i*2+1];
	}

	return out;
}


#include <NTL/ZZ.h>
#include <NTL/ZZ_pXFactoring.h>
#include "nsgen.h"
#include "ring.h"

void print_poly (ZZX& f) {
  long d = deg(f);
  if (d == -1) {
    cout << "0" << endl;
  }
  else {
    for (long i = d; i >= 0; --i) {
      if (f[i] != 0)
	cout << f[i] << "*" << "X^" << i << " + ";
    }
    cout << endl;
  }
}

/***********************************************************************/
ZZX ring::apply_conj (ZZX& a, long i) {
  long d = deg(a);
  if (d == -1) return ZZX(0);

  ZZX phi = (ZZX(INIT_MONO, mR)-1)/(ZZX(INIT_MONO, 1)-1);
  ZZX w = ZZX(0);

  for (long j = 0; j <= d; j++) {
    if (a[j] != 0) {
      long t = (j*i) % mR;
      ZZX z = ZZX(INIT_MONO, t);
      rem(z, z, phi);
      z *= ZZX(a[j]);
      w += z;
    }
  }

  return w;
}

ZZX ring::trace (ZZX& w) {
  ZZX r = ZZX(0);

  for (long a = 0; a < dR; ++a) {
    ZZX v = apply_conj(w, GZ(a));
    r += v;
  }
  return r;
}

ZZX ring::eta (long j) {
  ZZX z = ZZX(INIT_MONO, 1);
  ZZX phi = (ZZX(INIT_MONO, mR)-1)/(ZZX(INIT_MONO, 1)-1);

  long t = GofZ(j);
  ZZX w = ZZX(INIT_MONO, t);  // w = X^t
  rem(w, w, phi);

  return trace(w);
}

ZZ_pX ring::delta (long j) {
  return conv<ZZ_pX>(eta(j) - dR);
}

ZZ ring::scalar_quo (ZZX& a, ZZX& b, ZZ& p) {
  long d = max(deg(a), deg(b));
  a.SetLength(d+1);
  b.SetLength(d+1);

  // ZZ_pContext context;
  // context.save();
  ZZ_p::init(p);

  ZZ_pX a1, b1;
  a1.SetLength(d+1);
  a1 = conv<ZZ_pX>(a);
  b1.SetLength(d+1);
  b1 = conv<ZZ_pX>(b);

  long i;
  ZZ w;
  for (i = 0; i <= d; ++i) {
    if (GCD(b[i]%p, p) == 1) {
      w = (a[i] * InvMod(b[i]%p, p)) % p;
      break;
    }
  }

  // ZZ_pX c1 = conv<ZZ_pX>(w) * b1;
  // if (c1 != a1) {
  //   cout << "Error: unexpected behaviour in scalar_quo" << endl;
  //   cout << "i = " << i << ", p = " << p << endl;
  //   // cout << "a1 = " << a1 << endl;
  //   // cout << "b1 = " << b1 << endl;
  //   // cout << "c1 = " << c1 << endl;
  //   exit(-1);
  // }

  //context.restore();
  return w;
}

/***********************************************************************/
ZZ compose_integer (const ZZ& r1, const ZZ& q1, const ZZ& r2, const ZZ& q2) {
  ZZ a, b, c;
  XGCD(a, b, c, q1, q2);  // 1 = a = b*q1 + c*q2
  ZZ w1 = c * q2;
  ZZ w2 = b * q1;
  return (r1*w1 + r2*w2) % (q1*q2);
}

void compose_vector (vec_ZZ& omega, vec_ZZ& omega1, ZZ& q1, vec_ZZ& omega2, ZZ& q2) {
  for (long i = 0; i < omega1.length(); i++)
    omega[i] = compose_integer(omega1[i], q1, omega2[i], q2);
}

void compose (ZZX& res, ring& rg, ZZX& res0, ZZ& q0, ZZX& res1, ZZ& q1) {
  vec_ZZ v0 = VectorCopy(res0, rg.mR-1);
  vec_ZZ v1 = VectorCopy(res1, rg.mR-1);
  vec_ZZ v; v.SetLength(rg.mR-1);
  compose_vector(v, v0, q0, v1, q1);
  res.SetLength(rg.mR-1);
  for (long i = 0; i < rg.mR-1; i++) SetCoeff(res, i, v[i]);
}

// assumes that 'm' is prime
// modulus of the resolution is p^r
void ring::resolution_of_one (ZZX& res, const long m, const ZZ& p) {
  ZZ_pContext context;
  context.save();
  ZZ_p::init(p);

  ZZ_pX phi = (ZZ_pX(INIT_MONO, m)-1)/(ZZ_pX(INIT_MONO, 1)-1);

  cout << "." << flush;

  vec_ZZ_pX factors;
  //SFBerlekamp(factors, phi);
  if (p==ZZ(2))
    SFCanZass(factors, phi);
  else
    RootEDF(factors, phi);

  ZZ_pX G = ZZ_pX(1);
  for (long i = 1; i < factors.length(); ++i) G *= factors[i];
  ZZ_pX a, b, c;
  XGCD(a, b, c, factors[0], G); // 1 = a = b * factors[0] + c * G
  ZZ_pX res1 = c*G;

  // ZZ_pX res2 = (res1*res1) % phi;
  // if (res1 != res2) {
  //   cout << "Error in resolution_of_one" << endl;
  //   cout << "res   = " << res1 << endl;
  //   cout << "res2  = " << res2 << endl;
  //   exit(-1);
  // }

  res = conv<ZZX>(res1);
  context.restore();
}

void ring::lift_resolution (ZZX& res, const long m, const ZZ& r, const long l) {
  ZZ_p::init(power(r, l));
  ZZ_pX phi = (ZZ_pX(INIT_MONO, m)-1)/(ZZ_pX(INIT_MONO, 1)-1);

  ZZ_pX x = conv<ZZ_pX>(-res);
  for (long j = 0; j < l-1; ++j) {
    if ((j % 100) == 0) cout << "." << flush;
    x = (power(x+1, conv<long>(r)) - 1) % phi;
  }
  res = conv<ZZX>(-x);
}

void ring::canonical_resolution_of_one (ZZX& res, long prec) {
  resolution_of_one(res, mR, ZZ(pR));
  lift_resolution(res, mR, ZZ(pR), prec);
}

void ring::init_resolution (long prec) {
  if (type == prime) {
    canonical_resolution_of_one(res, prec);
  }
  else { // ring is composite
    if (left->initialized == false)
      left->init_resolution(prec);
    if (right->initialized == false)
      right->init_resolution(prec);
  }
}

void ring::init (long prec, noise& ns) {
  if (initialized == true) return;

  cout << "Initializing ring " << label << flush;

  init_resolution(prec);

  cout << "computing omega..." << flush;
  init_omega(power(ZZ(2), prec));

  depth = get_subrings(subrings);

  initialized = true;
  cout << "done" << endl;
}

/***********************************************************************/
// Transpose a (m*n)-dim. vector 'a'
// into a m x n matrix (m-dim vector of n-dim vector)'w'
void mat (Vec<double*>& aa, const double* a, const long n, const long m) {
  for (long j = 0; j < m; ++j)
    for (long i = 0; i < n; ++i)
      aa[j][i] = a[i + n*j];
}

void transpose (Vec<double*>& b, Vec<double*>& a) {
  long m = a.length();
  long n = b.length();

  for (long i = 0; i < m; ++i)
    for (long j = 0; j < n; ++j)
      b[j][i] = a[i][j];
}

void vec (double* b, const Vec<double*>& a, long n) {
  long m = a.length();

  for (long i = 0; i < m; ++i)
    for (long j = 0; j < n; ++j)
      b[i*n+j] = a[i][j];
}

/***************************************************************************/
// (Omega)_{i,j} = (rho_{t_i}(eta_j))
void ring::comp_omega (vec_ZZ& o, ZZ& q) {
  ZZX phi = (ZZX(INIT_MONO, mR)-1)/(ZZX(INIT_MONO, 1)-1);

  ZZ_p::init(q);

  o.SetLength(gR);
  for (long i = 0; i < gR; ++i) {
    if ((i % 100) == 0) cout << "." << flush;
    ZZ_pX e = conv<ZZ_pX>(eta(i));
    ZZ_pX a = (e * conv<ZZ_pX>(res)) % conv<ZZ_pX>(phi);
    ZZX aa = conv<ZZX>(a);
    o[i] = scalar_quo(aa, res, q);
  }
}

void ring::comp_omega_inv (vec_ZZ& o, const vec_ZZ& _omega, const ZZ& q) {
  o.SetLength(gR);

  for (long i = 0; i < gR; ++i) {
    ZZ w, conj_eta;
    if ((dR % 2) == 1) conj_eta = _omega[(i+gR/2) % gR];
    else conj_eta = _omega[i];
    w = conj_eta - dR;
    w = (w * InvMod(ZZ(mR)%q, q)) % q;
    o[i] = w;
  }
}

// (Gamma)_{i,j} = (rho_{t_i}(eta_j - d))
void ring::comp_gamma (vec_ZZ& o, const vec_ZZ& _omega, const ZZ& q) {
  o.SetLength(gR);

  for (long i = 0; i < gR; ++i) {
    o[i] = (omega[i] - dR) % q;
  }
}

void ring::comp_gamma_inv (vec_ZZ& o, const vec_ZZ& _omega, const ZZ& q) {
  o.SetLength(gR);

  for (long i = 0; i < gR; ++i) {
    ZZ conj_eta;
    if ((dR % 2) == 1) conj_eta = _omega[(i+gR/2) % gR];
    else conj_eta = _omega[i];
    o[i] = (conj_eta * InvMod(ZZ(mR)%q, q)) % q;
  }
}

void ring::init_omega (ZZ q) {
  if (type == prime) {
    comp_omega(omega, q);
    comp_omega_inv(omega_inv, omega, q);
  }
  else { // ring type is composite
    if (left->initialized == false)
      left->init_omega(q);
    if (right->initialized == false)
      right->init_omega(q);
  }
}

void param::init (long prec) {
  rg.init(prec, ns);
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "harmonic.h"
#include "cotmatrix.h"
#include "massmatrix.h"
#include "invert_diag.h"
#include "min_quad_with_fixed.h"
#include <Eigen/Sparse>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::harmonic(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  using namespace Eigen;
  typedef typename DerivedV::Scalar Scalar;
  typedef Matrix<Scalar,Dynamic,1> VectorXS;
  SparseMatrix<Scalar> L,M,Mi;
  cotmatrix(V,F,L);
  switch(F.cols())
  {
    case 3:
      massmatrix(V,F,MASSMATRIX_TYPE_VORONOI,M);
      break;
    case 4:
    default:
      massmatrix(V,F,MASSMATRIX_TYPE_BARYCENTRIC,M);
      break;
  }
  invert_diag(M,Mi);
  SparseMatrix<Scalar> Q = -L;
  for(int p = 1;p<k;p++)
  {
    Q = (Q*Mi*-L).eval();
  }
  const VectorXS B = VectorXS::Zero(V.rows(),1);
  min_quad_with_fixed_data<Scalar> data;
  min_quad_with_fixed_precompute(Q,b,SparseMatrix<Scalar>(),true,data);
  W.resize(V.rows(),bc.cols());
  for(int w = 0;w<bc.cols();w++)
  {
    const VectorXS bcw = bc.col(w);
    VectorXS Ww;
    if(!min_quad_with_fixed_solve(data,B,bcw,VectorXS(),Ww))
    {
      return false;
    }
    W.col(w) = Ww;
  }
  return true;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


/* boost random/fisher_f_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_FISHER_F_DISTRIBUTION_HPP
#define BOOST_RANDOM_FISHER_F_DISTRIBUTION_HPP

#include <iosfwd>
#include <istream>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/chi_squared_distribution.hpp>

namespace boost {
namespace random {

/**
 * The Fisher F distribution is a real valued distribution with two
 * parameters m and n.
 *
 * It has \f$\displaystyle p(x) =
 *   \frac{\Gamma((m+n)/2)}{\Gamma(m/2)\Gamma(n/2)}
 *   \left(\frac{m}{n}\right)^{m/2}
 *   x^{(m/2)-1} \left(1+\frac{mx}{n}\right)^{-(m+n)/2}
 * \f$.
 */
template<class RealType = double>
class fisher_f_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef fisher_f_distribution distribution_type;

        /**
         * Constructs a @c param_type from the "m" and "n" parameters
         * of the distribution.
         *
         * Requires: m > 0 and n > 0
         */
        explicit param_type(RealType m_arg = RealType(1.0),
                            RealType n_arg = RealType(1.0))
          : _m(m_arg), _n(n_arg)
        {}

        /** Returns the "m" parameter of the distribtuion. */
        RealType m() const { return _m; }
        /** Returns the "n" parameter of the distribution. */
        RealType n() const { return _n; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._m << ' ' << parm._n; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._m >> std::ws >> parm._n; return is; }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._m == rhs._m && lhs._n == rhs._n; }

        /** Returns true if the two sets of parameters are the different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _m;
        RealType _n;
    };

    /**
     * Constructs a @c fisher_f_distribution from its "m" and "n" parameters.
     *
     * Requires: m > 0 and n > 0
     */
    explicit fisher_f_distribution(RealType m_arg = RealType(1.0),
                                   RealType n_arg = RealType(1.0))
      : _impl_m(m_arg), _impl_n(n_arg)
    {}
    /** Constructs an @c fisher_f_distribution from its parameters. */
    explicit fisher_f_distribution(const param_type& parm)
      : _impl_m(parm.m()), _impl_n(parm.n())
    {}

    /**
     * Returns a random variate distributed according to the
     * F distribution.
     */
    template<class URNG>
    RealType operator()(URNG& urng)
    {
        return (_impl_m(urng) * n()) / (_impl_n(urng) * m());
    }

    /**
     * Returns a random variate distributed according to the
     * F distribution with parameters specified by @c param.
     */
    template<class URNG>
    RealType operator()(URNG& urng, const param_type& parm) const
    {
        return fisher_f_distribution(parm)(urng);
    }

    /** Returns the "m" parameter of the distribution. */
    RealType m() const { return _impl_m.n(); }
    /** Returns the "n" parameter of the distribution. */
    RealType n() const { return _impl_n.n(); }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return std::numeric_limits<RealType>::infinity(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(m(), n()); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        typedef chi_squared_distribution<RealType> impl_type;
        typename impl_type::param_type m_param(parm.m());
        _impl_m.param(m_param);
        typename impl_type::param_type n_param(parm.n());
        _impl_n.param(n_param);
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /** Writes an @c fisher_f_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, fisher_f_distribution, fd)
    {
        os << fd.param();
        return os;
    }

    /** Reads an @c fisher_f_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, fisher_f_distribution, fd)
    {
        param_type parm;
        if(is >> parm) {
            fd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two instances of @c fisher_f_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(fisher_f_distribution, lhs, rhs)
    { return lhs._impl_m == rhs._impl_m && lhs._impl_n == rhs._impl_n; }

    /**
     * Returns true if the two instances of @c fisher_f_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(fisher_f_distribution)

private:
    chi_squared_distribution<RealType> _impl_m;
    chi_squared_distribution<RealType> _impl_n;
};

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_EXTREME_VALUE_DISTRIBUTION_HPP


/* boost random/mixmax.hpp header file
 *
 * Copyright Kostas Savvidis 2008-2019
 *
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2019-04-23 created
 */

#ifndef BOOST_RANDOM_MIXMAX_HPP
#define BOOST_RANDOM_MIXMAX_HPP

#include <sstream>
#include <boost/cstdint.hpp>
#include <boost/array.hpp>

#include <boost/random/detail/seed.hpp>
#include <boost/random/detail/seed_impl.hpp>

namespace boost {
namespace random {

/**
 * Instantiations of class template mixmax_engine model,
 * \pseudo_random_number_generator .
 *  It uses the  MIXMAX generator algorithms from:
 *
 *  @blockquote
 *  G.K.Savvidy and N.G.Ter-Arutyunian,
 *  On the Monte Carlo simulation of physical systems,
 *  J.Comput.Phys. 97, 566 (1991);
 *  Preprint EPI-865-16-86, Yerevan, Jan. 1986
 *  http://dx.doi.org/10.1016/0021-9991(91)90015-D
 *
 *  K.Savvidy
 *  The MIXMAX random number generator
 *  Comp. Phys. Commun. 196 (2015), pp 161–165
 *  http://dx.doi.org/10.1016/j.cpc.2015.06.003
 *
 *  K.Savvidy and G.Savvidy
 *  Spectrum and Entropy of C-systems. MIXMAX random number generator
 *  Chaos, Solitons & Fractals, Volume 91, (2016) pp. 33–38
 *  http://dx.doi.org/10.1016/j.chaos.2016.05.003
 *  @endblockquote
 *
 * The generator crucially depends on the choice of the
 * parameters. The valid sets of parameters are from the published papers above.
 *
 */

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> // MIXMAX TEMPLATE PARAMETERS
class mixmax_engine{
public:
    // Interfaces required by C++11 std::random and boost::random
    typedef boost::uint64_t result_type ;
    BOOST_STATIC_CONSTANT(boost::uint64_t,mixmax_min=0);
    BOOST_STATIC_CONSTANT(boost::uint64_t,mixmax_max=((1ULL<<61)-1));
    BOOST_STATIC_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() {return mixmax_min;}
    BOOST_STATIC_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() {return mixmax_max;}
    static const bool has_fixed_range = false;
    BOOST_STATIC_CONSTANT(int,N=Ndim);     ///< The main internal parameter, size of the defining MIXMAX matrix
    // CONSTRUCTORS:
    explicit mixmax_engine();                       ///< Constructor, unit vector as initial state, acted on by A^2^512
    explicit mixmax_engine(boost::uint64_t);          ///< Constructor, one 64-bit seed
    explicit mixmax_engine(uint32_t clusterID, uint32_t machineID, uint32_t runID, uint32_t  streamID );  ///< Constructor, four 32-bit seeds for 128-bit seeding flexibility
    void seed(boost::uint64_t seedval=default_seed){seed_uniquestream( &S, 0, 0, (uint32_t)(seedval>>32), (uint32_t)seedval );} ///< seed with one 64-bit seed

private: // DATATYPES
    struct rng_state_st{
        boost::array<boost::uint64_t, Ndim> V;
        boost::uint64_t sumtot;
        int counter;
    };

    typedef struct rng_state_st rng_state_t;     // struct alias
    rng_state_t S;

public: // SEEDING FUNCTIONS
    template<class It> mixmax_engine(It& first, It last) { seed(first,last); }
    BOOST_RANDOM_DETAIL_SEED_SEQ_CONSTRUCTOR(mixmax_engine,  SeedSeq, seq){ seed(seq); }

    /** Sets the state of the generator using values from an iterator range. */
    template<class It>
    void seed(It& first, It last){
        uint32_t v[4];
        detail::fill_array_int<32>(first, last, v);
        seed_uniquestream( &S, v[0], v[1], v[2], v[3]);
    }
    /** Sets the state of the generator using values from a seed_seq. */
    BOOST_RANDOM_DETAIL_SEED_SEQ_SEED(mixmax_engine, SeeqSeq, seq){
        uint32_t v[4];
        detail::seed_array_int<32>(seq, v);
        seed_uniquestream( &S, v[0], v[1], v[2], v[3]);
    }

    /** return one uint64 between min=0 and max=2^61-1 */
    boost::uint64_t operator()(){
        if (S.counter<=(Ndim-1) ){
            return S.V[S.counter++];
        }else{
            S.sumtot = iterate_raw_vec(S.V.data(), S.sumtot);
            S.counter=2;
            return S.V[1];
        }
    }

    /** Fills a range with random values */
    template<class Iter>
    void generate(Iter first, Iter last) { detail::generate_from_int(*this, first, last); }

    void discard(boost::uint64_t nsteps) { for(boost::uint64_t j = 0; j < nsteps; ++j)  (*this)(); } ///< discard n steps, required in boost::random

    /** save the state of the RNG to a stream */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<< (std::basic_ostream<CharT,Traits>& ost, const mixmax_engine& me){
        ost << Ndim << " " << me.S.counter << " " << me.S.sumtot << " ";
        for (int j=0; (j< (Ndim) ); j++) {
        ost <<  (boost::uint64_t)me.S.V[j] << " ";
        }
        ost << "\n";
        ost.flush();
        return ost;
        }

    /** read the state of the RNG from a stream */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>> (std::basic_istream<CharT,Traits> &in, mixmax_engine& me){
        // will set std::ios::failbit if the input format is not right
        boost::array<boost::uint64_t, Ndim> vec;
        boost::uint64_t sum=0, savedsum=0, counter=0;
        in >> counter >> std::ws;
        BOOST_ASSERT(counter==Ndim);
        in >> counter >> std::ws;
        in >> savedsum >> std::ws;
        for(int j=0;j<Ndim;j++) {
        in >> std::ws >> vec[j] ;
        sum=me.MOD_MERSENNE(sum+vec[j]);
    }
    if (sum == savedsum && counter>0 && counter<Ndim){
        me.S.V=vec; me.S.counter = counter; me.S.sumtot=savedsum;
    }else{
        in.setstate(std::ios::failbit);
    }
    return in;
    }

friend bool operator==(const mixmax_engine & x,
                       const mixmax_engine & y){return x.S.counter==y.S.counter && x.S.sumtot==y.S.sumtot && x.S.V==y.S.V ;}
friend bool operator!=(const mixmax_engine & x,
                       const mixmax_engine & y){return !operator==(x,y);}

private:
BOOST_STATIC_CONSTANT(int, BITS=61);
BOOST_STATIC_CONSTANT(boost::uint64_t, M61=2305843009213693951ULL);
BOOST_STATIC_CONSTANT(boost::uint64_t, default_seed=1);
inline boost::uint64_t MOD_MERSENNE(boost::uint64_t k) {return ((((k)) & M61) + (((k)) >> BITS) );}
inline boost::uint64_t MULWU(boost::uint64_t k);
inline void seed_vielbein(rng_state_t* X, unsigned int i); // seeds with the i-th unit vector, i = 0..Ndim-1,  for testing only
inline void seed_uniquestream( rng_state_t* Xin, uint32_t clusterID, uint32_t machineID, uint32_t runID, uint32_t  streamID );
inline boost::uint64_t iterate_raw_vec(boost::uint64_t* Y, boost::uint64_t sumtotOld);
inline boost::uint64_t apply_bigskip(boost::uint64_t* Vout, boost::uint64_t* Vin, uint32_t clusterID, uint32_t machineID, uint32_t runID, uint32_t  streamID );
inline boost::uint64_t modadd(boost::uint64_t foo, boost::uint64_t bar);
inline boost::uint64_t fmodmulM61(boost::uint64_t cum, boost::uint64_t s, boost::uint64_t a);
};

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::mixmax_engine()
///< constructor, with no params, seeds with seed=0,  random numbers are as good as from any other seed
{
    seed_uniquestream( &S, 0,  0, 0, default_seed);
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::mixmax_engine(boost::uint64_t seedval){
    ///< constructor, one uint64_t seed, random numbers are statistically independent from any two distinct seeds, e.g. consecutive seeds are ok
    seed_uniquestream( &S, 0,  0,  (uint32_t)(seedval>>32), (uint32_t)seedval );
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::mixmax_engine(uint32_t clusterID, uint32_t machineID, uint32_t runID, uint32_t  streamID){
    // constructor, four 32-bit seeds for 128-bit seeding flexibility
    seed_uniquestream( &S, clusterID,  machineID,  runID,  streamID );
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> uint64_t mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::MULWU (uint64_t k){ return (( (k)<<(SPECIALMUL) & M61) ^ ( (k) >> (BITS-SPECIALMUL))  )  ;}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> boost::uint64_t mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::iterate_raw_vec(boost::uint64_t* Y, boost::uint64_t sumtotOld){
    // operates with a raw vector, uses known sum of elements of Y
    boost::uint64_t  tempP=0, tempV=sumtotOld;
    Y[0] = tempV;
    boost::uint64_t sumtot = Y[0], ovflow = 0; // will keep a running sum of all new elements
    for (int i=1; i<Ndim; i++){
        boost::uint64_t tempPO = MULWU(tempP);
        tempV = (tempV+tempPO);
        tempP = modadd(tempP, Y[i]);
        tempV = modadd(tempV, tempP); // new Y[i] = old Y[i] + old partial * m
        Y[i] = tempV;
        sumtot += tempV; if (sumtot < tempV) {ovflow++;}
    }
    return MOD_MERSENNE(MOD_MERSENNE(sumtot) + (ovflow <<3 ));
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> void mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::seed_vielbein(rng_state_t* X, unsigned int index){
    for (int i=0; i < Ndim; i++){
        X->V[i] = 0;
    }
    if (index<Ndim) { X->V[index] = 1; }else{ X->V[0]=1; }
    X->counter = Ndim;  // set the counter to Ndim if iteration should happen right away
    X->sumtot = 1;
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> void mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::seed_uniquestream( rng_state_t* Xin, uint32_t clusterID, uint32_t machineID, uint32_t runID, uint32_t  streamID ){
    seed_vielbein(Xin,0);
    Xin->sumtot = apply_bigskip(Xin->V.data(), Xin->V.data(),  clusterID,  machineID,  runID,   streamID );
    Xin->counter = 1;
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> boost::uint64_t mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::apply_bigskip( boost::uint64_t* Vout, boost::uint64_t* Vin, uint32_t clusterID, uint32_t machineID, uint32_t runID, uint32_t  streamID ){
    /*
     makes a derived state vector, Vout, from the mother state vector Vin
     by skipping a large number of steps, determined by the given seeding ID's

     it is mathematically guaranteed that the substreams derived in this way from the SAME (!!!) Vin will not collide provided
     1) at least one bit of ID is different
     2) less than 10^100 numbers are drawn from the stream
     (this is good enough : a single CPU will not exceed this in the lifetime of the universe, 10^19 sec,
     even if it had a clock cycle of Planck time, 10^44 Hz )

     Caution: never apply this to a derived vector, just choose some mother vector Vin, for example the unit vector by seed_vielbein(X,0),
     and use it in all your runs, just change runID to get completely nonoverlapping streams of random numbers on a different day.

     clusterID and machineID are provided for the benefit of large organizations who wish to ensure that a simulation
     which is running in parallel on a large number of  clusters and machines will have non-colliding source of random numbers.

     did i repeat it enough times? the non-collision guarantee is absolute, not probabilistic

     */

    const    boost::uint64_t skipMat17[128][17] =
#include "boost/random/detail/mixmax_skip_N17.ipp"
    ;

    const boost::uint64_t* skipMat[128];
    BOOST_ASSERT(Ndim==17);
    for (int i=0; i<128; i++) { skipMat[i] = skipMat17[i];}

    uint32_t IDvec[4] = {streamID, runID, machineID, clusterID};
    boost::uint64_t Y[Ndim], cum[Ndim];
    boost::uint64_t sumtot=0;

    for (int i=0; i<Ndim; i++) { Y[i] = Vin[i]; sumtot = modadd( sumtot, Vin[i]); } ;
    for (int IDindex=0; IDindex<4; IDindex++) { // go from lower order to higher order ID
        uint32_t id=IDvec[IDindex];
        int r = 0;
        while (id){
            if (id & 1) {
                boost::uint64_t* rowPtr = (boost::uint64_t*)skipMat[r + IDindex*8*sizeof(uint32_t)];
                for (int i=0; i<Ndim; i++){ cum[i] = 0; }
                for (int j=0; j<Ndim; j++){              // j is lag, enumerates terms of the poly
                    // for zero lag Y is already given
                    boost::uint64_t coeff = rowPtr[j]; // same coeff for all i
                    for (int i =0; i<Ndim; i++){
                        cum[i] =  fmodmulM61( cum[i], coeff ,  Y[i] ) ;
                    }
                    sumtot = iterate_raw_vec(Y, sumtot);
                }
                sumtot=0;
                for (int i=0; i<Ndim; i++){ Y[i] = cum[i]; sumtot = modadd( sumtot, cum[i]); } ;
            }
            id = (id >> 1); r++; // bring up the r-th bit in the ID
        }
    }
    sumtot=0;
    for (int i=0; i<Ndim; i++){ Vout[i] = Y[i]; sumtot = modadd( sumtot, Y[i]); } ;  // returns sumtot, and copy the vector over to Vout
    return (sumtot) ;
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> inline boost::uint64_t mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::fmodmulM61(boost::uint64_t cum, boost::uint64_t s, boost::uint64_t a){
    // works on all platforms, including 32-bit linux, PPC and PPC64, ARM and Windows
    const boost::uint64_t MASK32=0xFFFFFFFFULL;
    boost::uint64_t o,ph,pl,ah,al;
    o=(s)*a;
    ph = ((s)>>32);
    pl = (s) & MASK32;
    ah = a>>32;
    al = a & MASK32;
    o = (o & M61) + ((ph*ah)<<3) + ((ah*pl+al*ph + ((al*pl)>>32))>>29) ;
    o += cum;
    o = (o & M61) + ((o>>61));
    return o;
}

template <int Ndim, unsigned int SPECIALMUL, boost::int64_t SPECIAL> boost::uint64_t mixmax_engine  <Ndim, SPECIALMUL, SPECIAL> ::modadd(boost::uint64_t foo, boost::uint64_t bar){
    return MOD_MERSENNE(foo+bar);
}

/* @copydoc boost::random::detail::mixmax_engine_doc */
/** Instantiation with a valid parameter set. */
typedef mixmax_engine<17,36,0>          mixmax;
}// namespace random
}// namespace boost

#endif // BOOST_RANDOM_MIXMAX_HPP


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

// Eigen based implementation of multi objective
// Differential Evolution using the DE/all/1 strategy.
//
// Can switch to NSGA-II like population update via parameter 'nsga_update'.
// Then it works essentially like NSGA-II but instead of the tournament selection
// the whole population is sorted and the best individuals survive. To do this
// efficiently the crowd distance ordering is slightly inaccurate.
//
// Supports parallel fitness function evaluation.
//
// Features enhanced multiple constraint ranking (https://www.jstage.jst.go.jp/article/tjpnsec/11/2/11_18/_article/-char/en/)
// improving its performance in handling constraints for engineering design optimization.
//
// Enables the comparison of DE and NSGA-II population update mechanism with everything else
// kept completely identical.
//
// Uses the following deviation from the standard DE algorithm:
// a) oscillating CR/F parameters.
//
// You may keep parameters F and CR at their defaults since this implementation works well with the given settings for most problems,
// since the algorithm oscillates between different F and CR settings.
//
// For expensive objective functions (e.g. machine learning parameter optimization) use the workers
// parameter to parallelize objective function evaluation. The workers parameter is limited by the
// population size.

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

namespace mode_optimizer {

class MoDeOptimizer {

public:

    MoDeOptimizer(long runid_, Fitness *fitfun_, callback_type log_, int dim_,
    		int nobj_, int ncon_, int seed_,
            int popsize_, int maxEvaluations_, double F_, double CR_,
            double pro_c_, double dis_c_, double pro_m_, double dis_m_,
            bool nsga_update_, double pareto_update_, int log_period_) {
        // runid used to identify a specific run
        runid = runid_;
        // fitness function to minimize
        fitfun = fitfun_;
        // callback to log progress
        log = log_;
        // Number of objective variables/problem dimension
        dim = dim_;
        // Number of objectives
        nobj = nobj_;
        // Number of constraints
        ncon = ncon_;
        // Population size
        popsize = popsize_ > 0 ? popsize_ : 128;
        // maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_ > 0 ? maxEvaluations_ : 500000;
        // DE population update parameters, ignored if nsga_update == true
        F = F0 = F_ > 0 ? F_ : 0.5;
        CR = CR0 = CR_ > 0 ? CR_ : 0.9;
        // Number of iterations already performed.
        iterations = 0;
        // Number of evaluations already performed.
        n_evals = 0;
        // position of current x/y
        pos = 0;
        //std::random_device rd;
        rs = new pcg64(seed_);
        // NSGA population update parameters, ignored if nsga_update == false
   	    // usually use pro_c = 1.0, dis_c = 20.0, pro_m = 1.0, dis_m = 20.0.
    	pro_c = pro_c_;
    	dis_c = dis_c_;
    	pro_m = pro_m_;
    	dis_m = dis_m_;
        // if true, use NSGA population update, if false, use DE population update
        // Use DE update to diversify your results.
    	nsga_update = nsga_update_;
        // DE population update parameter. Only applied if nsga_update = false.
    	// Use the pareto front for population update
    	// with probability pareto_update, else use the whole population.
    	// If pareto_update == 0: use always the whole population.
        // usually should be 0, optimization can get stuck in local minima otherwise.
    	pareto_update = pareto_update_;
        // The log callback is called each log_period iterations
        log_period = log_period_;
        if (log_period <= 0)
            log_period = 1000;
        init();
    }

    ~MoDeOptimizer() {
        delete rs;
    }

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

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

    vec nextX(int p) {
        if (p == 0) {
            iterations++;
        	if (iterations % log_period == 0) {
        		if (log(popX.cols(), popX.data(), popY.data()))
        			fitfun->setTerminate();
        	}
        }
    	if (nsga_update) {
    		vec x = vX.col(vp);
    		vp = (vp + 1) % popsize;
    		return x;
    	}
    	// use DE update strategy.
        if (p == 0) {
            CR = iterations % 2 == 0 ? 0.5 * CR0 : CR0;
            F = iterations % 2 == 0 ? 0.5 * F0 : F0;
        }
        int r3;
        if (rnd01() < pareto_update) {
			// sample from pareto front
            do {
            	r3 = rndInt(bestP.size());
            	r3 = bestP[r3];
            } while (r3 == p);
        } else {
			// sample from whole population
            do {
                r3 = rndInt(popsize);
            } while (r3 == p);
        }
        vec xp = popX.col(p);
        vec x3 = popX.col(r3);
        int r1, r2;
        do {
            r1 = rndInt(popsize);
        } while (r1 == p || r1 == r3);
        do {
            r2 = rndInt(popsize);
        } while (r2 == p || r2 == r3 || r2 == r1);
        vec x1 = popX.col(r1);
        vec x2 = popX.col(r2);
        vec x = x3 + (x1 - x2) * F;
        int r = rndInt(dim);
        for (int j = 0; j < dim; j++)
            if (j != r && rnd01() > CR)
                x[j] = xp[j];
        return fitfun->getClosestFeasible(x);
    }

    vec crowd_dist(mat& y) { // crowd distance for 1st objective
    	int n = y.cols();
    	vec y0 = y.row(0);
    	ivec si = sort_index(y0); // sort 1st objective
    	vec y0s = y0(si); // sorted y0
    	vec d(n-1);
        for (int i = 0; i < n-1; i++)
        	d(i) = y0s[i+1] - y0s[i]; // neighbor distance
        if (d.maxCoeff() == 0)
        	return zeros(n);
        vec dsum = zeros(n);
        for (int i = 0; i < n; i++) {
        	if (i > 0)
        		dsum(i) += d(i-1); // distance to left
        	if (i < n-1)
        		dsum(i) += d(i); //  distance to right
        }
        dsum(0) = DBL_MAX; // keep borders
        dsum(n-1) = DBL_MAX;
        vec ds(n);
        ds(si) = dsum;  // inverse order
        return ds;
    }

    bool is_dominated(const mat& y, int i, int index) {
    	for (int j = 0; j < y.rows(); j++)
    		if (y(j,i) < y(j,index))
    			return false;
    	return true;
    }

    vec pareto_levels(const mat& y) {
         int n = y.cols();
         ivec pareto(n);
         for (int i = 0; i < n; i++)
         	pareto(i) = i;
         vec domination = zeros(n);
         bool mask[n];
         for (int i = 0; i < n; i++)
        	 mask[i] = true;
         for (int index = 0; index < n;) {
    		for (int i = 0; i < n; i++) {
    			if (i != index && mask[i] && is_dominated(y, i, index))
    				mask[i] = false;
    		}
    		for (int i = 0; i < n; i++) {
    			if (mask[i])
    				domination[i] += 1;
    		}
    		index++;
    		while(!mask[index] && index < n)
    			index++;
         }
         return domination;
    }

    vec objranks(mat objs) {
    	imat ci(objs.cols(), objs.rows());
    	for (int i = 0; i < objs.rows(); i++)
    		ci.col(i) = sort_index(objs.row(i).transpose());
    	mat rank(objs.rows(), objs.cols());
    	for (int j = 0; j < objs.rows(); j++)
    		for (int i = 0; i < objs.cols(); i++) {
    			rank(j, ci(i,j)) = i;
    	}
    	return rank.colwise().sum();
    }

    vec ranks(mat cons) {
    	imat ci(cons.cols(), cons.rows());
    	for (int i = 0; i < cons.rows(); i++)
    		ci.col(i) = sort_index(cons.row(i).transpose());
    	mat rank(cons.rows(), cons.cols());
    	vec alpha = zeros(cons.rows());
    	for (int j = 0; j < cons.rows(); j++) {
    		for (int i = 0; i < cons.cols(); i++) {
    			if (cons(j,i) <= 0) {
    				rank(j, ci(i,j)) = 0;
    			} else {
    				rank(j, ci(i,j)) = i;
    				alpha[j]++;
    			}
    		}
    	}
    	for (int j = 0; j < cons.rows(); j++) {
    		for (int i = 0; i < cons.cols(); i++)
    			rank(j, ci(i,j)) *= alpha[j] / cons.rows();
    	}
    	return rank.colwise().sum();
    }

    vec pareto(const mat& ys) {
    	if (ncon == 0)
    		return pareto_levels(ys);
        int popn = ys.cols();
        mat yobj = ys(Eigen::seqN(0, nobj), Eigen::all);
        mat ycon = ys(Eigen::lastN(ncon), Eigen::all);
        vec csum = ranks(ycon);
        bool feasible[ys.cols()];
        bool hasFeasible = false;
        for (int i = 0; i < ys.cols(); i++) {
        	feasible[i] = ycon.col(i).maxCoeff() <= 0;
        	if (feasible[i])
        		hasFeasible = true;
        }
        if (hasFeasible)
        	csum += objranks(yobj);
		// first pareto front of feasible solutions
        vec domination = zeros(popn);
        std::vector<int> cyv;
        for (int i = 0; i < ys.cols(); i++) // collect feasibles
        	if (feasible[i]) cyv.push_back(i);
        ivec cy =  Eigen::Map<ivec, Eigen::Unaligned>(cyv.data(), cyv.size());
        if (hasFeasible) { // compute pareto levels only for feasible
        	vec ypar = pareto_levels(yobj(Eigen::all, cy));
        	domination(cy) += ypar;
        }
        // then constraint violations
        ivec ci = sort_index(csum);
        std::vector<int> civ;
        for (int i = 0; i < ci.size(); i++)
        	if (!feasible[ci(i)]) civ.push_back(ci(i));
        if (civ.size() > 0) {
        	ivec ci =  Eigen::Map<ivec, Eigen::Unaligned>(civ.data(), civ.size());
        	int maxcdom = ci.size();
        	// higher constraint violation level gets lower domination level assigned
        	for (int i = 0; i < ci.size(); i++)
        		domination(ci(i)) += maxcdom - i;
        	if (cy.size() > 0) { // priorize feasible solutions
            	for (int i = 0; i < cy.size(); i++)
            		domination(cy(i)) += maxcdom + 1;
        	}
        } // higher dominates lower
        return domination;
    }

    mat variation(const mat& x) {
    	int n2 = x.cols() / 2;
    	int n = 2 * n2;
        mat parent1 = x(Eigen::all, Eigen::seq(0, n2-1));
        mat parent2 = x(Eigen::all, Eigen::seq(n2, n-1));
        mat beta = mat(dim, n2);
        vec to1;
        if (pro_c < 1.0) {
        	to1 = uniformVec(dim, *rs);
        }
        for (int p = 0; p < n2; p++) {
			for (int i = 0; i < dim; i++) {
				if (rnd01() > 0.5 || (pro_c < 1.0 && to1(i) < pro_c))
					beta(i, p) = 1.0;
				else {
					double r = rnd01();
					if (r <= 0.5)
						beta(i, p) = pow(2 * r, 1.0 / (dis_c + 1.0));
					else
						beta(i, p) = pow(2 * r, -1.0 / (dis_c + 1.0));
					if (rnd01() > 0.5)
						beta(i, p) = -beta(i, p);
				}
			}
		}
        mat offspring1 = ((parent1 + parent2) * 0.5);
        mat offspring2 = mat(offspring1);
        mat delta = (beta.array() * (parent1 - parent2).array()).matrix() * 0.5;
        offspring1 += delta;
        offspring2 -= delta;
    	mat offspring = mat(dim, n);
    	offspring << offspring1, offspring2;

        double limit = pro_m / dim;
        vec scale = fitfun->scale();
        for (int p = 0; p < n; p++) {
            for (int i = 0; i < dim; i++) {
            	if (rnd01() < limit) { // site
            		double mu = rnd01();
        			double norm = fitfun->norm_i(i, offspring(i, p));
            		if (mu <= 0.5) // temp
            			offspring(i, p) += scale(i) *
                                (pow(2. * mu + (1. - 2. * mu) * pow(1. - norm, dis_m + 1.),
	                               1. / (dis_m + 1.)) - 1.);
            		else
            			offspring(i, p) += scale(i) *
                                (1. - pow(2. * (1. - mu) + 2. * (mu - 0.5) * pow(1. - norm, dis_m + 1.),
	                               1. / (dis_m + 1.)));
        		}
        	}
        }
        fitfun->setClosestFeasible(offspring);
    	return offspring;
    }

    void pop_update() {
    	vec domination = pareto(popY);
    	std::vector<vec> x;
    	std::vector<vec> y;
    	int maxdom = (int) domination.maxCoeff();
    	for (int dom = maxdom; dom >= 0; dom--) {
    		std::vector<int> level;
    		for (int i = 0; i < domination.size(); i++)
    			if (domination(i) == dom)
    				level.push_back(i);
            ivec domlevel =  Eigen::Map<ivec, Eigen::Unaligned>(level.data(), level.size());
			mat domx = popX(Eigen::all, domlevel);
			mat domy = popY(Eigen::all, domlevel);
			if (dom == maxdom) // store pareto front in bestP
				bestP = domlevel;

    		if ((int)(x.size() + domlevel.size()) <= popsize) {
				// whole level fits
    			for (int i = 0; i < domy.cols(); i++) {
    				x.push_back(domx.col(i));
    				y.push_back(domy.col(i));
    			}
    		} else {
    			std::vector<int> si;
    			si.push_back(0);
    			if (domy.cols() > 1) {
    				vec cd = crowd_dist(domy);
    				ivec si = sort_index(cd).reverse();
    				for (int i = 0; i < si.size(); i++) {
    					if (((int)x.size()) >= popsize)
    						break;
    					x.push_back(domx.col(si(i)));
    					y.push_back(domy.col(si(i)));
    				}
    			}
    			break;
    		}
    	}
    	for (int i = 0; i < popsize; i++) {
    		popX.col(i) = x[i];
       		popY.col(i) = y[i];
    	}
    	if (nsga_update)
    		vX = variation(popX(Eigen::all, Eigen::seqN(0, popsize)));
    }

    vec ask(int &p) {
		p = pos;
		vec x = nextX(p);
		pos = (pos + 1) % popsize;
		return x;
    }

    int tell(const vec &y, const vec &x, int p) {
    	long unsigned int dp = 0;
    	for (; dp < vdone.size(); dp++)
    		if (!vdone[dp]) break;
    	nX.col(dp) = x;
       	nY.col(dp) = y;
       	vdone[dp] = true;
    	int ndone = 0;
    	for (long unsigned int i = 0; i < vdone.size(); i++)
    		if (vdone[i]) ndone++;
    	if (ndone >= popsize) {
			int p = popsize;
        	for (dp = 0; dp < vdone.size(); dp++) {
        		if (vdone[dp]) {
					popX.col(p) = nX.col(dp);
					popY.col(p) = nY.col(dp);
					vdone[dp] = false;
					if (p >= popY.cols())
						break;
					p++;
        		}
        	}
        	pop_update();
    	}
    	n_evals += 1;
//        if (n_evals % 1000 == 999)
//        	std::cout << popY << std::endl;
    	return stop;
    }

    void doOptimize() {
    	iterations = 0;
    	fitfun->resetEvaluations();
    	while (fitfun->evaluations() < maxEvaluations && !fitfun->terminate()) {
            for (int p = 0; p < popsize; p++) {
            	vec x = nextX(p);
            	popX.col(popsize + p) = x;
            	popY.col(popsize + p) = fitfun->eval(x);
            }
            pop_update();
     	}
    }

    void do_optimize_delayed_update(int workers) {
    	 iterations = 0;
    	 fitfun->resetEvaluations();
         workers = std::min(workers, popsize); // workers <= popsize
    	 evaluator eval(fitfun, nobj, workers);
    	 vec evals_x[popsize];
	     // fill eval queue with initial population
    	 for (int i = 0; i < workers; i++) {
    		 int p;
    		 vec x = ask(p);
    		 eval.evaluate(x, p);
    		 evals_x[p] = x;
    	 }
    	 while (fitfun->evaluations() < maxEvaluations && !fitfun->terminate()) {
    		 vec_id* vid = eval.result();
    		 vec y = vec(vid->_v);
    		 int p = vid->_id;
    		 delete vid;
    		 vec x = evals_x[p];
    		 tell(y, x, p); // tell evaluated x
    		 if (fitfun->evaluations() >= maxEvaluations)
    			 break;
    		 x = ask(p);
    		 eval.evaluate(x, p);
    		 evals_x[p] = x;
    	 }
	}

    void init() {
        popX = mat(dim, 2*popsize);
        popY = mat(nobj + ncon, 2*popsize);
        for (int p = 0; p < popsize; p++) {
            popX.col(p) = fitfun->sample(*rs);
            popY.col(p) = constant(nobj + ncon, DBL_MAX);
        }
        next_size = 2*popsize;
        vdone = std::vector<bool>(next_size, false);
		nX = mat(dim, next_size);
		nY = mat(nobj + ncon, next_size);
		vX = mat(popX);
		vp = 0;
		bestP = ivec(popsize);
		for (int i = 0; i < popsize; i++)
			bestP(i) = i;
    }

    mat getX() {
        return popX;
    }

    mat getY() {
        return popY;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

    Fitness* getFitfun() {
        return fitfun;
    }

    int getDim() {
        return dim;
    }

private:
    long runid;
    Fitness *fitfun;
    callback_type log;
    int popsize; // population size
    int dim;
    int nobj;
    int ncon;
    int maxEvaluations;
    double keep;
    double stopfitness;
    int iterations;
    int n_evals;
    ivec bestP;
    int stop;
    double F0;
    double CR0;
    double F;
    double CR;
	double pro_c;
	double dis_c;
	double pro_m;
	double dis_m;
    pcg64 *rs;
    mat popX;
    mat popY;
    mat nX;
    mat nY;
    mat vX;
    int vp;
    int next_size;
    std::vector<bool> vdone;
    int pos;
    bool nsga_update;
    double pareto_update;
    int log_period;
};
}

using namespace mode_optimizer;

extern "C" {
void optimizeMODE_C(long runid, callback_type func, callback_type log,
		int dim, int nobj, int ncon, int seed,
        double *lower, double *upper, int maxEvals,
		int popsize, int workers, double F, double CR,
	    double pro_c, double dis_c, double pro_m, double dis_m,
        bool nsga_update, double pareto_update, int log_period, double* res) {
    vec lower_limit(dim), upper_limit(dim);
    for (int i = 0; i < dim; i++) {
        lower_limit[i] = lower[i];
        upper_limit[i] = upper[i];
    }
    Fitness fitfun(func, dim, nobj + ncon, lower_limit, upper_limit);
    MoDeOptimizer opt(runid, &fitfun, log, dim, nobj, ncon,
    		seed, popsize, maxEvals, F, CR,
            pro_c, dis_c, pro_m, dis_m,
            nsga_update, pareto_update, log_period);
    try {
        if (workers <= 1)
            opt.doOptimize();
        else
            opt.do_optimize_delayed_update(workers);
        double* xdata = opt.getX().data();
        for (int i = 0; i < opt.getX().size(); i++)
            res[i] = xdata[i];
    } catch (std::exception &e) {
    	std::cout << e.what() << std::endl;
    }
   }
}


#ifndef NUMERICAL_OPTIMIZATION_HPP
#define NUMERICAL_OPTIMIZATION_HPP

#include <mathtoolbox/bfgs.hpp>
#include <mathtoolbox/l-bfgs.hpp>
#include <functional>
#include <stdexcept>
#include <Eigen/Core>

namespace mathtoolbox
{
    namespace optimization
    {
        enum class Algorithm
        {
            Bfgs, LBfgs
        };

        enum class Type
        {
            Min, Max
        };

        struct Setting
        {
            Algorithm algorithm                                      = Algorithm::Bfgs;
            Eigen::VectorXd x_init                                   = Eigen::VectorXd(0);
            std::function<double(const Eigen::VectorXd&)> f          = nullptr;
            std::function<Eigen::VectorXd(const Eigen::VectorXd&)> g = nullptr;
            double epsilon                                           = 1e-05;
            unsigned int max_num_iterations                          = 1000;
            Type type                                                = Type::Min;
        };

        struct Result
        {
            Eigen::VectorXd x_star;
            unsigned        num_iterations;
        };

        inline Result RunOptimization(const Setting& input)
        {
            const auto f = (input.type == Type::Min) ? input.f : [&input](const Eigen::VectorXd& x) { return - input.f(x); };
            const auto g = (input.type == Type::Min) ? input.g : [&input](const Eigen::VectorXd& x) { return - input.g(x); };

            switch (input.algorithm) {
                case Algorithm::Bfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunBfgs(input.x_init, f, g, input.epsilon, input.max_num_iterations, result.x_star, result.num_iterations);
                    return result;
                }
                case Algorithm::LBfgs:
                {
                    if (!input.f || !input.g || input.x_init.rows() == 0)
                    {
                        throw std::invalid_argument("Invalid setting.");
                    }

                    Result result;
                    RunLBfgs(input.x_init, f, g, input.epsilon, input.max_num_iterations, result.x_star, result.num_iterations);
                    return result;
                }
            }
        }
    }
}

#endif // NUMERICAL_OPTIMIZATION_HPP


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/vision/sfm/pose/five_point_relative_pose.h"

#include <Eigen/Dense>
#include <glog/logging.h>

#include <cmath>
#include <ctime>
#include <vector>

#include "theia/math/matrix/gauss_jordan.h"
#include "theia/math/polynomial.h"
#include "theia/vision/sfm/pose/util.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Matrix;
using Eigen::RowVector3d;
using Eigen::RowVector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

typedef Matrix<double, 3, 3, Eigen::RowMajor> RowMatrix3d;

namespace {
// Multiplies two polynomials over the same variable.
template <int n1, int n2>
Matrix<double, 1, n1 + n2 - 1> MultiplyPoly(const Matrix<double, 1, n1>& a,
                                            const Matrix<double, 1, n2>& b) {
  Matrix<double, 1, n1 + n2 - 1> poly = Matrix<double, 1, n1 + n2 - 1>::Zero();
  for (int i = 0; i < a.cols(); i++)
    for (int j = 0; j < b.cols(); j++)
      poly[i + j] += a[i] * b[j];

  return poly;
}

// Evaluates a given polynomial at the value x.
template <int n>
double EvaluatePoly(const Matrix<double, 1, n>& poly, double x) {
  double val = 0;
  for (int i = poly.cols() - 1; i > 0; i--) {
    val += poly[i];
    val *= x;
  }
  val += poly[0];
  return val;
}

// Multiply two degree one polynomials of variables x, y, z.
// E.g. p1 = a[0]x + a[1]y + a[2]z + a[3]
// x^2 y^2 z^2 xy xz yz x y z 1
Matrix<double, 1, 10> MultiplyDegOnePoly(const RowVector4d& a,
                                         const RowVector4d& b) {
  Matrix<double, 1, 10> output;
  output(0) = a(0) * b(0);
  output(1) = a(1) * b(1);
  output(2) = a(2) * b(2);
  output(3) = a(0) * b(1) + a(1) * b(0);
  output(4) = a(0) * b(2) + a(2) * b(0);
  output(5) = a(1) * b(2) + a(2) * b(1);
  output(6) = a(0) * b(3) + a(3) * b(0);
  output(7) = a(1) * b(3) + a(3) * b(1);
  output(8) = a(2) * b(3) + a(3) * b(2);
  output(9) = a(3) * b(3);
  return output;
}

// Multiply a 2 deg poly (in x, y, z) and a one deg poly.
// x^3 y^3 x^2y xy^2 x^2z x^2 y^2z y^2 xyz xy | z^2x zx x z^2y zy y z^3 z^2 z 1
// NOTE: after the | all are variables along z.
Matrix<double, 1, 20> MultiplyDegTwoDegOnePoly(const Matrix<double, 1, 10>& a,
                                               const RowVector4d& b) {
  Matrix<double, 1, 20> output;
  output(0) = a(0) * b(0);
  output(1) = a(1) * b(1);
  output(2) = a(0) * b(1) + a(3) * b(0);
  output(3) = a(1) * b(0) + a(3) * b(1);
  output(4) = a(0) * b(2) + a(4) * b(0);
  output(5) = a(0) * b(3) + a(6) * b(0);
  output(6) = a(1) * b(2) + a(5) * b(1);
  output(7) = a(1) * b(3) + a(7) * b(1);
  output(8) = a(3) * b(2) + a(4) * b(1) + a(5) * b(0);
  output(9) = a(3) * b(3) + a(6) * b(1) + a(7) * b(0);
  output(10) = a(2) * b(0) + a(4) * b(2);
  output(11) = a(4) * b(3) + a(8) * b(0) + a(6) * b(2);
  output(12) = a(6) * b(3) + a(9) * b(0);
  output(13) = a(2) * b(1) + a(5) * b(2);
  output(14) = a(5) * b(3) + a(8) * b(1) + a(7) * b(2);
  output(15) = a(7) * b(3) + a(9) * b(1);
  output(16) = a(2) * b(2);
  output(17) = a(2) * b(3) + a(8) * b(2);
  output(18) = a(8) * b(3) + a(9) * b(2);
  output(19) = a(9) * b(3);
  return output;
}

// Shorthand for multiplying the Essential matrix with its transpose according
// to Eq. 20 in Nister paper.
Matrix<double, 1, 10> EETranspose(
    const Matrix<double, 9, 4>& null_matrix, int i, int j) {
  return MultiplyDegOnePoly(null_matrix.row(3 * i), null_matrix.row(3 * j)) +
      MultiplyDegOnePoly(null_matrix.row(3 * i + 1),
                         null_matrix.row(3 * j + 1)) +
      MultiplyDegOnePoly(null_matrix.row(3 * i + 2),
                         null_matrix.row(3 * j + 2));
}

// Builds the 10x20 constraint matrix according to Section 3.2.2 of Nister
// paper. Constraints are built based on the singularity of the Essential
// matrix, and the trace equation (Eq. 6). This builds the 10x20 matrix such
// that the columns correspond to: x^3, yx^2, y^2x, y^3, zx^2, zyx, zy^2, z^2x,
// z^2y, z^3, x^2, yx, y^2, zx, zy, z^2, x, y, z, 1.
Matrix<double, 10, 20> BuildConstraintMatrix(
    const Matrix<double, 9, 4>& null_space) {
  Matrix<double, 10, 20> constraint_matrix;
  // Singularity constraint.
  constraint_matrix.row(0) =
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space.row(1), null_space.row(5)) -
          MultiplyDegOnePoly(null_space.row(2), null_space.row(4)),
          null_space.row(6)) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space.row(2), null_space.row(3)) -
          MultiplyDegOnePoly(null_space.row(0), null_space.row(5)),
          null_space.row(7)) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space.row(0), null_space.row(4)) -
          MultiplyDegOnePoly(null_space.row(1), null_space.row(3)),
          null_space.row(8));

  // Trace Constraint. Only need to compute the upper triangular part of the
  // symmetric polynomial matrix
  Matrix<double, 1, 10> symmetric_poly[3][3];
  symmetric_poly[0][0] = EETranspose(null_space, 0, 0);
  symmetric_poly[1][1] = EETranspose(null_space, 1, 1);
  symmetric_poly[2][2] = EETranspose(null_space, 2, 2);

  Matrix<double, 1, 10> half_trace = 0.5*(symmetric_poly[0][0] +
                                          symmetric_poly[1][1] +
                                          symmetric_poly[2][2]);

  symmetric_poly[0][0] -= half_trace;
  symmetric_poly[1][1] -= half_trace;
  symmetric_poly[2][2] -= half_trace;
  symmetric_poly[0][1] = EETranspose(null_space, 0, 1);
  symmetric_poly[0][2] = EETranspose(null_space, 0, 2);
  symmetric_poly[1][0] = symmetric_poly[0][1];
  symmetric_poly[1][2] = EETranspose(null_space, 1, 2);
  symmetric_poly[2][0] = symmetric_poly[0][2];
  symmetric_poly[2][1] = symmetric_poly[1][2];

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      constraint_matrix.row(3*i + j + 1) =
          MultiplyDegTwoDegOnePoly(symmetric_poly[i][0],
                                   null_space.row(j)) +
          MultiplyDegTwoDegOnePoly(symmetric_poly[i][1],
                                   null_space.row(3 + j)) +
          MultiplyDegTwoDegOnePoly(symmetric_poly[i][2],
                                   null_space.row(6 + j));
    }
  }
  return constraint_matrix;
}

// Efficient nullspace extraction based on the idea of QR decomposition on the 5
// correspondences of the epipolar constraint. Instead of QR, we use
// Gauss-Jordan for the same effect.
Matrix<double, 9, 4> EfficientNullspaceExtraction(
    const Matrix<double, 5, 9>& constraint) {
  Matrix<double, 5, 9> constraint_copy(constraint);
  GaussJordan(&constraint_copy);
  Matrix<double, 4, 9> null_space;
  null_space << constraint_copy.rightCols(4).transpose(),
      -Matrix4d::Identity();
  return null_space.transpose();
}

void EfficientSVDDecomp(const Matrix3d& essential_mat,
                        Vector3d* null_space,
                        Matrix3d rotation[4],
                        Vector3d translation[4]) {
  Matrix3d d;
  d << 0, 1, 0,
      -1, 0, 0,
      0, 0, 1;

  const Vector3d& ea = essential_mat.row(0);
  const Vector3d& eb = essential_mat.row(1);
  const Vector3d& ec = essential_mat.row(2);

  // Generate cross products.
  Matrix3d cross_products;
  cross_products << ea.cross(eb), ea.cross(ec), eb.cross(ec);

  // Choose the cross product with the largest norm (for numerical accuracy).
  const Vector3d cf_scales(cross_products.col(0).squaredNorm(),
                           cross_products.col(1).squaredNorm(),
                           cross_products.col(2).squaredNorm());
  int max_index;
  cf_scales.maxCoeff(&max_index);

  // For index 0, 1, we want ea and for index 2 we want eb.
  const int max_e_index = max_index / 2;

  // Construct v of the SVD.
  Matrix3d v = Matrix3d::Zero();
  v.col(2) = cross_products.col(max_index).normalized();
  v.col(0) = essential_mat.row(max_e_index).normalized();
  v.col(1) = v.col(2).cross(v.col(0));

  // Construct U of the SVD.
  Matrix3d u = Matrix3d::Zero();
  u.col(0) = (essential_mat * v.col(0)).normalized();
  u.col(1) = (essential_mat * v.col(1)).normalized();
  u.col(2) = u.col(0).cross(u.col(1));

  // Possible rotation configurations.
  const RowMatrix3d ra =
      Eigen::Quaterniond(u * d * v.transpose()).normalized().toRotationMatrix();
  const RowMatrix3d rb = Eigen::Quaterniond(u * d.transpose() * v.transpose())
      .normalized().toRotationMatrix();

  // Scale t to be proper magnitude. Scale factor is derived from the fact that
  // U*diag*V^t = E. We simply choose to scale it such that the last terms will
  // be equal.
  const Vector3d t = u.col(2).normalized();
  const Vector3d t_neg = -t;

  // Copy the 4 possible decompositions into the output arrays.
  rotation[0] = ra;
  translation[0] = t;
  rotation[1] = ra;
  translation[1] = t_neg;
  rotation[2] = rb;
  translation[2] = t;
  rotation[3] = rb;
  translation[3] = t_neg;

  *null_space = v.col(2);
}

void DecomposeWithIdealCorrespondence(const Vector2d& image_point1,
                                      const Vector2d& image_point2,
                                      const Matrix3d& essential_mat,
                                      Matrix3d* rotation,
                                      Vector3d* translation) {
  const Vector3d image_point1_homog = image_point1.homogeneous();
  const Vector3d image_point2_homog = image_point2.homogeneous();

  // Map the image points to vectors.
  Matrix3d candidate_rotation[4];
  Vector3d candidate_translation[4];
  Vector3d null_space;
  EfficientSVDDecomp(essential_mat, &null_space, candidate_rotation,
                     candidate_translation);

  Matrix<double, 3, 4> projection_mat;
  projection_mat.block<3, 3>(0, 0) = candidate_rotation[0];
  projection_mat.block<3, 1>(0, 3) = candidate_translation[0];

  // Compute c.
  Matrix3d temp_diag = Matrix3d::Identity();
  temp_diag(2, 2) = 0.0;
  const Vector3d c =
      image_point2_homog.cross(temp_diag * essential_mat * image_point1_homog);

  // Compute C.
  const Vector4d C = projection_mat.transpose() * c;
  const Vector4d Q(image_point1_homog(0) * C(3),
                   image_point1_homog(1) * C(3),
                   image_point1_homog(2) * C(3),
                   -(image_point1_homog.dot(C.head<3>())));
  // We only care about the sign of the depth because it informs us of the
  // direction of the point (i.e. if it is in front of the camera). Use a
  // multiply instead of divide for speed.
  const double scaled_depth_1  = Q(2) * Q(3);
  const double scaled_depth_2 = projection_mat.row(2).dot(Q) * Q(3);

  // Create the twisted pair transformation.
  const Vector4d twisted_transformation(-2.0 * null_space(0),
                                        -2.0 * null_space(1),
                                        -2.0 * null_space(2),
                                        -1.0);

  // Determine the proper configuration for the R,t decomposition.
  int best_index;
  if (scaled_depth_1 > 0 && scaled_depth_2 > 0) {
    best_index = 0;
  } else if (scaled_depth_1 < 0 && scaled_depth_2 < 0) {
    best_index = 1;
  } else if (Q(2) * twisted_transformation.dot(Q) > 0) {
    best_index = 2;
  } else {
    best_index = 3;
  }

  *rotation = candidate_rotation[best_index];
  *translation = candidate_translation[best_index];
}

}  // namespace

// Implementation of Nister from "An Efficient Solution to the Five-Point
// Relative Pose Problem"
bool FivePointRelativePose(const Vector2d image1_points[5],
                           const Vector2d image2_points[5],
                           std::vector<Matrix3d>* rotation,
                           std::vector<Vector3d>* translation) {
  // Step 1. Create the 5x9 matrix containing epipolar constraints.
  //   Essential matrix is a linear combination of the 4 vectors spanning the
  //   null space of this matrix (found by SVD).
  Matrix<double, 5, 9> epipolar_constraint;
  for (int i = 0; i < 5; i++) {
    // Fill matrix with the epipolar constraint from q'_t*E*q = 0. Where q is
    // from the first image, and q' is from the second. Eq. 8 in the Nister
    // paper.
    epipolar_constraint.row(i) <<
        image1_points[i].x() * image2_points[i].x(),
        image1_points[i].y() * image2_points[i].x(),
        image2_points[i].x(),
        image1_points[i].x() * image2_points[i].y(),
        image1_points[i].y() * image2_points[i].y(),
        image2_points[i].y(),
        image1_points[i].x(),
        image1_points[i].y(),
        1.0;
  }

  // Solve for right null space of the 5x9 matrix. NOTE: We use a
  // super-efficient method that is a variation of the QR decomposition
  // described in the Nister paper.  by roughly 5x.
  Matrix<double, 9, 4> null_space =
      EfficientNullspaceExtraction(epipolar_constraint);

  // Step 2. Expansion of the epipolar constraints Eq. 5 and 6 from Nister
  // paper.
  Matrix<double, 10, 20> constraint_matrix = BuildConstraintMatrix(null_space);

  // Step 3. Gauss-Jordan Elimination with partial pivoting on constraint
  // matrix.
  GaussJordan(&constraint_matrix);

  // Step 4. Expand determinant polynomial of 3x3 polynomial B.
  // Create matrix B. Horribly ugly, but not sure if there's a better way to do
  // it!
  RowVector4d b11(constraint_matrix(4, 12),
                  constraint_matrix(4, 11) - constraint_matrix(5, 12),
                  constraint_matrix(4, 10) - constraint_matrix(5, 11),
                  -constraint_matrix(5, 10));
  RowVector4d b12(constraint_matrix(4, 15),
                  constraint_matrix(4, 14) - constraint_matrix(5, 15),
                  constraint_matrix(4, 13) - constraint_matrix(5, 14),
                  -constraint_matrix(5, 13));
  Matrix<double, 1, 5> b13;
  b13 << constraint_matrix(4, 19),
      constraint_matrix(4, 18) - constraint_matrix(5, 19),
      constraint_matrix(4, 17) - constraint_matrix(5, 18),
      constraint_matrix(4, 16) - constraint_matrix(5, 17),
      -constraint_matrix(5, 16);
  RowVector4d b21(constraint_matrix(6, 12),
                  constraint_matrix(6, 11) - constraint_matrix(7, 12),
                  constraint_matrix(6, 10) - constraint_matrix(7, 11),
                  -constraint_matrix(7, 10));
  RowVector4d b22(constraint_matrix(6, 15),
                  constraint_matrix(6, 14) - constraint_matrix(7, 15),
                  constraint_matrix(6, 13) - constraint_matrix(7, 14),
                  -constraint_matrix(7, 13));
  Matrix<double, 1, 5> b23;
  b23 << constraint_matrix(6, 19),
      constraint_matrix(6, 18) - constraint_matrix(7, 19),
      constraint_matrix(6, 17) - constraint_matrix(7, 18),
      constraint_matrix(6, 16) - constraint_matrix(7, 17),
      -constraint_matrix(7, 16);
  RowVector4d b31(constraint_matrix(8, 12),
                  constraint_matrix(8, 11) - constraint_matrix(9, 12),
                  constraint_matrix(8, 10) - constraint_matrix(9, 11),
                  -constraint_matrix(9, 10));
  RowVector4d b32(constraint_matrix(8, 15),
                  constraint_matrix(8, 14) - constraint_matrix(9, 15),
                  constraint_matrix(8, 13) - constraint_matrix(9, 14),
                  -constraint_matrix(9, 13));
  Matrix<double, 1, 5> b33;
  b33 << constraint_matrix(8, 19),
      constraint_matrix(8, 18) - constraint_matrix(9, 19),
      constraint_matrix(8, 17) - constraint_matrix(9, 18),
      constraint_matrix(8, 16) - constraint_matrix(9, 17),
      -constraint_matrix(9, 16);

  // Eq. 24.
  Matrix<double, 1, 8> p1 = MultiplyPoly(b12, b23) - MultiplyPoly(b13, b22);
  // Eq. 25.
  Matrix<double, 1, 8> p2 = MultiplyPoly(b13, b21) - MultiplyPoly(b11, b23);
  // Eq. 26.
  Matrix<double, 1, 7> p3 = MultiplyPoly(b11, b22) - MultiplyPoly(b12, b21);

  // Eq. 27. Form determinant of B as a 10th degree polynomial.
  Matrix<double, 1, 11> n = MultiplyPoly(p1, b31) + MultiplyPoly(p2, b32) +
                            MultiplyPoly(p3, b33);

  // Step 5. Extract real roots of the 10th degree polynomial.
  Eigen::VectorXd roots;
  FindRealPolynomialRoots(n.transpose().reverse(), &roots);

  rotation->reserve(roots.size());
  translation->reserve(roots.size());
  static const double kTolerance = 1e-12;
  for (int i = 0; i < roots.size(); i++) {
    // We only want non-zero roots
    if (fabs(roots(i)) < kTolerance)
      continue;

    double x = EvaluatePoly(p1, roots(i)) / EvaluatePoly(p3, roots(i));
    double y = EvaluatePoly(p2, roots(i)) / EvaluatePoly(p3, roots(i));
    Matrix<double, 9, 1> temp_sum =
        x * null_space.col(0) + y * null_space.col(1) +
        roots(i) * null_space.col(2) + null_space.col(3);
    // Need to do it like this because temp_sum is a row vector and recasting
    // it as a 3x3 will load it column-major.
    Matrix3d candidate_essential_mat;
    candidate_essential_mat << temp_sum.head<3>().transpose(),
        temp_sum.segment(3, 3).transpose(), temp_sum.tail(3).transpose();

    Matrix3d rotation_soln;
    Vector3d translation_soln;
    // Decompose into R, t using the first point correspondence.
    DecomposeWithIdealCorrespondence(image1_points[0], image2_points[0],
                                     candidate_essential_mat, &rotation_soln,
                                     &translation_soln);
    rotation->push_back(rotation_soln);
    translation->push_back(translation_soln);
  }
  return (roots.size() > 0);
}

}  // namespace theia