/**
 * \file fknm.cpp
 * \author Jesse Haviland
 *
 *
 */

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include "fknm.h"
#include "methods.h"
#include "linalg.h"
#include "structs.h"

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

static PyMethodDef fknmMethods[] = {
    {"IK",
     (PyCFunction)IK,
     METH_VARARGS,
     "Link"},
    {"Robot_link_T",
     (PyCFunction)Robot_link_T,
     METH_VARARGS,
     "Link"},
    {"ETS_hessian0",
     (PyCFunction)ETS_hessian0,
     METH_VARARGS,
     "Link"},
    {"ETS_hessiane",
     (PyCFunction)ETS_hessiane,
     METH_VARARGS,
     "Link"},
    {"ETS_jacobe",
     (PyCFunction)ETS_jacobe,
     METH_VARARGS,
     "Link"},
    {"ETS_jacob0",
     (PyCFunction)ETS_jacob0,
     METH_VARARGS,
     "Link"},
    {"ETS_fkine",
     (PyCFunction)ETS_fkine,
     METH_VARARGS,
     "Link"},
    {"ETS_init",
     (PyCFunction)ETS_init,
     METH_VARARGS,
     "Link"},
    {"ET_update",
     (PyCFunction)ET_update,
     METH_VARARGS,
     "Link"},
    {"ET_init",
     (PyCFunction)ET_init,
     METH_VARARGS,
     "Link"},
    {"ET_T",
     (PyCFunction)ET_T,
     METH_VARARGS,
     "Link"},
    {"r2q",
     (PyCFunction)r2q,
     METH_VARARGS,
     "Link"},
    {NULL, NULL, 0, NULL} /* Sentinel */
};

static struct PyModuleDef fknmmodule =
    {
        PyModuleDef_HEAD_INIT,
        "fknm",
        "Fast Kinematics",
        -1,
        fknmMethods};

PyMODINIT_FUNC PyInit_fknm(void)
{
    import_array();
    return PyModule_Create(&fknmmodule);
}

extern "C"
{
    static PyObject *IK(PyObject *self, PyObject *args)
    {
        npy_float64 *q, *Tep, *ret;
        PyObject *py_q, *py_Tep;
        PyArrayObject *py_np_q, *py_np_Tep;
        PyObject *ets;
        int n;
        PyObject *py_ret;
        npy_intp dim1[2] = {2, 4};

        if (!PyArg_ParseTuple(
                args, "iOOO",
                &n,
                &ets,
                &py_q,
                &py_Tep))
            return NULL;

        // Make sure q is number array
        // Cast to numpy array
        // Get data out
        if (!_check_array_type(py_q))
            return NULL;
        py_np_q = (PyArrayObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT);
        q = (npy_float64 *)PyArray_DATA(py_np_q);

        if (!_check_array_type(py_Tep))
            return NULL;

        py_np_Tep = (PyArrayObject *)PyArray_FROMANY(py_Tep, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT);
        Tep = (npy_float64 *)PyArray_DATA(py_np_Tep);

        py_ret = PyArray_EMPTY(2, dim1, NPY_DOUBLE, 0);
        ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);

        // __mult4(2, 4, )

        _ETS_IK(ets, n, q, Tep, ret);

        // _angle_axis(Te, Tep, ret);

        // Free the memory
        Py_DECREF(py_np_q);
        Py_DECREF(py_np_Tep);

        return py_ret;
    }

    static PyObject *Robot_link_T(PyObject *self, PyObject *args)
    {
        ETS *ets;
        npy_float64 *q;
        PyObject *py_q, *py_np_q;
        PyArrayObject *py_self_q;
        PyObject *ets_list, *T_list;
        int q_used = 0;
        Py_ssize_t n_links;

        if (!PyArg_ParseTuple(
                args, "OOO!O",
                &ets_list,
                &T_list,
                &PyArray_Type, &py_self_q,
                &py_q))
            return NULL;

        // Make sure q is number array
        // Cast to numpy array
        // Get data out
        if (py_q == Py_None || !_check_array_type(py_q))
        {
            q = (npy_float64 *)PyArray_DATA(py_self_q);
        }
        else
        {
            py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT);
            q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q);
            q_used = 1;
        }

        n_links = PyList_GET_SIZE(ets_list);
        for (int i = 0; i < n_links; i++)
        {
            PyObject *py_ets = PyList_GET_ITEM(ets_list, i);
            // Extract the ETS object from the python object
            if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
                return NULL;

            npy_float64 *T = (npy_float64 *)PyArray_DATA((PyArrayObject *)PyList_GET_ITEM(T_list, i));
            MapMatrix4dc eT(T);

            // TODO Add this back
            _ETS_fkine(ets, q, NULL, NULL, eT);
        }

        // Free the memory
        if (q_used)
        {
            Py_DECREF(py_np_q);
        }

        Py_RETURN_NONE;
    }

    static PyObject *ETS_hessian0(PyObject *self, PyObject *args)
    {
        ETS *ets;
        npy_float64 *H, *J, *q, *tool = NULL;
        PyObject *py_q, *py_J, *py_tool, *py_np_q, *py_np_tool, *py_np_J;
        PyObject *py_ets;
        int tool_used = 0, J_used = 0, q_used = 0;

        if (!PyArg_ParseTuple(
                args, "OOOO",
                &py_ets,
                &py_q,
                &py_J,
                &py_tool))
            return NULL;

        // Extract the ETS object from the python object
        if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
            return NULL;

        MapMatrixJc eJ(NULL, 6, ets->n);

        // Check if J is None
        // Make sure J is number array
        // Cast to numpy array
        // Get data out
        if (py_J != Py_None)
        {
            if (!_check_array_type(py_J))
                return NULL;
            J_used = 1;
            py_np_J = (PyObject *)PyArray_FROMANY(py_J, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            J = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_J);
            // MapMatrixJc eJ(J, 6, ets->n);
            new (&eJ) MapMatrixJc(J, 6, ets->n);
        }
        else
        {
            // Now we must use q instead
            // Make sure q is number array
            // Cast to numpy array
            // Get data out
            if (!_check_array_type(py_q))
                return NULL;
            q_used = 1;
            py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q);

            // Make our empty Jacobian
            npy_intp dimsJ[2] = {6, ets->n};
            PyObject *py_J = PyArray_EMPTY(2, dimsJ, NPY_DOUBLE, 1);
            J = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_J);
            // MapMatrixJc eJ(J, 6, ets->n);
            new (&eJ) MapMatrixJc(J, 6, ets->n);

            // Check if tool is None
            // Make sure tool is number array
            // Cast to numpy array
            // Get data out
            if (py_tool != Py_None)
            {
                if (!_check_array_type(py_tool))
                    return NULL;
                tool_used = 1;
                py_np_tool = (PyObject *)PyArray_FROMANY(py_tool, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
                tool = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_tool);
            }

            // Calculate the Jacobian
            _ETS_jacob0(ets, q, tool, eJ);
        }

        // Make our empty Hessian
        npy_intp dimsH[3] = {ets->n, 6, ets->n};
        PyObject *py_H = PyArray_EMPTY(3, dimsH, NPY_DOUBLE, 0);
        H = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_H);
        MapMatrixHr eH(H, ets->n * 6, ets->n);

        // Do the job
        _ETS_hessian(ets->n, eJ, eH);

        // Free the memory
        if (q_used)
        {
            Py_DECREF(py_np_q);
        }

        if (J_used)
        {
            Py_DECREF(py_np_J);
        }

        if (tool_used)
        {
            Py_DECREF(py_np_tool);
        }

        return py_H;
        // return Py_None;
    }

    static PyObject *ETS_hessiane(PyObject *self, PyObject *args)
    {
        ETS *ets;
        npy_float64 *H, *J, *q, *tool = NULL;
        PyObject *py_q, *py_J, *py_tool, *py_np_q, *py_np_tool, *py_np_J;
        PyObject *py_ets;
        int tool_used = 0, J_used = 0, q_used = 0;

        if (!PyArg_ParseTuple(
                args, "OOOO",
                &py_ets,
                &py_q,
                &py_J,
                &py_tool))
            return NULL;

        // Extract the ETS object from the python object
        if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
            return NULL;

        MapMatrixJc eJ(NULL, 6, ets->n);

        // Check if J is None
        // Make sure J is number array
        // Cast to numpy array
        // Get data out
        if (py_J != Py_None)
        {
            if (!_check_array_type(py_J))
                return NULL;
            J_used = 1;
            py_np_J = (PyObject *)PyArray_FROMANY(py_J, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            J = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_J);
            // MapMatrixJc eJ(J, 6, ets->n);
            new (&eJ) MapMatrixJc(J, 6, ets->n);
        }
        else
        {
            // Now we must use q instead
            // Make sure q is number array
            // Cast to numpy array
            // Get data out
            if (!_check_array_type(py_q))
                return NULL;
            q_used = 1;
            py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q);

            // Make our empty Jacobian
            npy_intp dimsJ[2] = {6, ets->n};
            PyObject *py_J = PyArray_EMPTY(2, dimsJ, NPY_DOUBLE, 1);
            J = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_J);
            // MapMatrixJc eJ(J, 6, ets->n);
            new (&eJ) MapMatrixJc(J, 6, ets->n);

            // Check if tool is None
            // Make sure tool is number array
            // Cast to numpy array
            // Get data out
            if (py_tool != Py_None)
            {
                if (!_check_array_type(py_tool))
                    return NULL;
                tool_used = 1;
                py_np_tool = (PyObject *)PyArray_FROMANY(py_tool, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
                tool = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_tool);
            }

            // Calculate the Jacobian
            _ETS_jacobe(ets, q, tool, eJ);
        }

        // Make our empty Hessian
        npy_intp dimsH[3] = {ets->n, 6, ets->n};
        PyObject *py_H = PyArray_EMPTY(3, dimsH, NPY_DOUBLE, 0);
        H = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_H);
        MapMatrixHr eH(H, ets->n * 6, ets->n);

        // Do the job
        _ETS_hessian(ets->n, eJ, eH);

        // Free the memory
        if (q_used)
        {
            Py_DECREF(py_np_q);
        }

        if (J_used)
        {
            Py_DECREF(py_np_J);
        }

        if (tool_used)
        {
            Py_DECREF(py_np_tool);
        }

        return py_H;
        // return Py_None;
    }

    static PyObject *ETS_jacob0(PyObject *self, PyObject *args)
    {
        ETS *ets;
        npy_float64 *J, *q, *tool = NULL;
        PyObject *py_q, *py_tool, *py_np_q, *py_np_tool;
        PyObject *py_ets;
        int tool_used = 0;

        if (!PyArg_ParseTuple(
                args, "OOO",
                &py_ets,
                &py_q,
                &py_tool))
            return NULL;

        // Extract the ETS object from the python object
        if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
            return NULL;

        // Inputs can be:
        // None - Even q
        // Not arrays - Will raise exception
        // Have symbolic data - Will raise exception
        // q can be 1D or 2D, assumes dimesnions correct (n, 1xn or nx1)
        // tool can be SE3s or 4x4 numpy array

        // Make our empty Jacobian
        npy_intp dims[2] = {6, ets->n};
        PyObject *py_J = PyArray_EMPTY(2, dims, NPY_DOUBLE, 1);
        J = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_J);
        MapMatrixJc eJ(J, 6, ets->n);

        // Make sure q is number array
        // Cast to numpy array
        // Get data out
        if (!_check_array_type(py_q))
            return NULL;
        py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
        q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q);

        // Check if tool is None
        // Make sure tool is number array
        // Cast to numpy array
        // Get data out
        if (py_tool != Py_None)
        {
            if (!_check_array_type(py_tool))
                return NULL;
            tool_used = 1;
            py_np_tool = (PyObject *)PyArray_FROMANY(py_tool, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            tool = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_tool);
        }

        // Do the job
        _ETS_jacob0(ets, q, tool, eJ);

        // Free the memory
        Py_DECREF(py_np_q);

        if (tool_used)
        {
            Py_DECREF(py_np_tool);
        }

        return py_J;
    }

    static PyObject *ETS_jacobe(PyObject *self, PyObject *args)
    {
        ETS *ets;
        npy_float64 *J, *q, *tool = NULL;
        PyObject *py_q, *py_tool, *py_np_q, *py_np_tool;
        PyObject *py_ets;
        int tool_used = 0;

        if (!PyArg_ParseTuple(
                args, "OOO",
                &py_ets,
                &py_q,
                &py_tool))
            return NULL;

        // Extract the ETS object from the python object
        if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
            return NULL;

        // Inputs can be:
        // None - Even q
        // Not arrays - Will raise exception
        // Have symbolic data - Will raise exception
        // q can be 1D or 2D, assumes dimesnions correct (n, 1xn or nx1)
        // tool can be SE3s or 4x4 numpy array

        // Make our empty Jacobian
        npy_intp dims[2] = {6, ets->n};
        PyObject *py_J = PyArray_EMPTY(2, dims, NPY_DOUBLE, 1);
        J = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_J);
        MapMatrixJc eJ(J, 6, ets->n);

        // Make sure q is number array
        // Cast to numpy array
        // Get data out
        if (!_check_array_type(py_q))
            return NULL;
        py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
        q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q);

        // Check if tool is None
        // Make sure tool is number array
        // Cast to numpy array
        // Get data out
        if (py_tool != Py_None)
        {
            if (!_check_array_type(py_tool))
                return NULL;
            tool_used = 1;
            py_np_tool = (PyObject *)PyArray_FROMANY(py_tool, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            tool = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_tool);
        }

        // Do the job
        // for (int i = 0; i < 1000000; i++)
        // {
        //     _ETS_jacobe(ets, q, tool, eJ);
        // }
        _ETS_jacobe(ets, q, tool, eJ);

        // Free the memory
        Py_DECREF(py_np_q);

        if (tool_used)
        {
            Py_DECREF(py_np_tool);
        }

        return py_J;
    }

    static PyObject *ETS_fkine(PyObject *self, PyObject *args)
    {
        ETS *ets;
        npy_intp dim2[2] = {4, 4}, dim3[3] = {1, 4, 4};
        int include_base, n = 0, q_nd, trajn = 1, tool_used = 0, base_used = 0;
        npy_float64 *ret, *retp, *q, *qp, *base = NULL, *tool = NULL;
        PyObject *py_q, *py_base, *py_tool, *py_np_q, *py_np_tool, *py_np_base;
        PyObject *py_ret, *py_ets;
        npy_intp *q_shape;

        if (!PyArg_ParseTuple(
                args, "OOOOi",
                &py_ets,
                &py_q,
                &py_base,
                &py_tool,
                &include_base))
            return NULL;

        // Extract the ETS object from the python object
        if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS")))
            return NULL;

        // Inputs can be:
        // None - Even q
        // Not arrays - Will raise exception
        // Have symbolic data - Will raise exception
        // q can be 2D or 1D, but assumes dimesnions correct (n, 1xn or nx1)
        // base and tool can be SE3s or 4x4 numpy array

        // Make sure q is number array
        // Cast to numpy array
        // Get data out
        if (!_check_array_type(py_q))
            return NULL;
        py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
        q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q);

        // Check the dimesnions of q
        q_nd = PyArray_NDIM((PyArrayObject *)py_np_q);
        q_shape = PyArray_SHAPE((PyArrayObject *)py_np_q);

        // Work out how long the trajectory is
        if (q_nd > 1)
        {
            if (q_shape[0] == 1)
            {
                // We have a single q vector
                trajn = 1;
                n = q_shape[1];
            }
            else if (q_shape[1] == 1)
            {
                // We have a single q vector
                trajn = 1;
                n = q_shape[0];
            }
            else
            {
                // We have a trajectory of q
                trajn = q_shape[0];
                n = q_shape[1];
            }
        }

        // Allocate return array
        if (trajn == 1)
        {
            py_ret = PyArray_EMPTY(2, dim2, NPY_DOUBLE, 1);
        }
        else
        {
            dim3[0] = trajn;
            py_ret = PyArray_EMPTY(3, dim3, NPY_DOUBLE, 1);
        }

        // Get numpy reference to return array
        ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);

        // Check if base is None
        // Make sure base is number array
        // Cast to numpy array
        // Get data out
        if (py_base != Py_None)
        {
            if (!_check_array_type(py_base))
                return NULL;

            if (include_base)
            {
                base_used = 1;
                py_np_base = (PyObject *)PyArray_FROMANY(py_base, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
                base = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_base);
            }
        }

        if (py_tool != Py_None)
        {
            if (!_check_array_type(py_tool))
                return NULL;
            tool_used = 1;
            py_np_tool = (PyObject *)PyArray_FROMANY(py_tool, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS);
            tool = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_tool);
        }

        // Do the actual job
        for (int i = 0; i < trajn; i++)
        {
            // Get pointers to the new section of return array and q array
            retp = ret + (4 * 4 * i);
            MapMatrix4dc e_retp(retp);
            qp = q + (n * i);
            _ETS_fkine(ets, qp, base, tool, e_retp);
        }

        // Free memory
        Py_DECREF(py_np_q);

        if (tool_used)
            Py_DECREF(py_np_tool);

        if (base_used)
            Py_DECREF(py_np_base);

        return py_ret;
    }

    static PyObject *ETS_init(PyObject *self, PyObject *args)
    {
        ETS *ets;
        PyObject *etsl, *ret;

        ets = (ETS *)PyMem_RawMalloc(sizeof(ETS));

        if (!PyArg_ParseTuple(args, "Oii",
                              &etsl,
                              &ets->n,
                              &ets->m))
            return NULL;

        ets->ets = (ET **)PyMem_RawMalloc(ets->m * sizeof(ET *));

        PyObject *iter_et = PyObject_GetIter(etsl);

        for (int i = 0; i < ets->m; i++)
        {
            if (!(ets->ets[i] = (ET *)PyCapsule_GetPointer(PyIter_Next(iter_et), "ET")))
                return NULL;
        }

        Py_DECREF(iter_et);

        ret = PyCapsule_New(ets, "ETS", NULL);
        return ret;
    }

    static PyObject *ET_update(PyObject *self, PyObject *args)
    {
        ET *et;
        int jointtype;
        PyObject *ret, *py_et;
        PyArrayObject *py_T, *py_qlim;
        int isjoint, isflip, jindex;

        et = (ET *)PyMem_RawMalloc(sizeof(ET));

        if (!PyArg_ParseTuple(args, "OiiiiO!O!",
                              &py_et,
                              &isjoint,
                              &isflip,
                              &jindex,
                              &jointtype,
                              &PyArray_Type, &py_T,
                              &PyArray_Type, &py_qlim))
            return NULL;

        if (!(et = (ET *)PyCapsule_GetPointer(py_et, "ET")))
            return NULL;

        et->T = (npy_float64 *)PyArray_DATA(py_T);
        new (&et->Tm) MapMatrix4dc(et->T);
        et->qlim = (npy_float64 *)PyArray_DATA(py_qlim);
        et->axis = jointtype;

        et->isjoint = isjoint;
        et->isflip = isflip;
        et->jindex = jindex;

        if (jointtype == 0)
        {
            et->op = rx;
        }
        else if (jointtype == 1)
        {
            et->op = ry;
        }
        else if (jointtype == 2)
        {
            et->op = rz;
        }
        else if (jointtype == 3)
        {
            et->op = tx;
        }
        else if (jointtype == 4)
        {
            et->op = ty;
        }
        else if (jointtype == 5)
        {
            et->op = tz;
        }

        ret = PyCapsule_New(et, "ET", NULL);
        return ret;
    }

    static PyObject *ET_init(PyObject *self, PyObject *args)
    {
        ET *et;
        int jointtype;
        PyObject *ret;
        PyArrayObject *py_T, *py_qlim;

        et = (ET *)PyMem_RawMalloc(sizeof(ET));

        if (!PyArg_ParseTuple(args, "iiiiO!O!",
                              &et->isjoint,
                              &et->isflip,
                              &et->jindex,
                              &jointtype,
                              &PyArray_Type, &py_T,
                              &PyArray_Type, &py_qlim))
            return NULL;

        et->T = (npy_float64 *)PyArray_DATA(py_T);
        new (&et->Tm) MapMatrix4dc(et->T);
        et->qlim = (npy_float64 *)PyArray_DATA(py_qlim);

        et->axis = jointtype;

        if (jointtype == 0)
        {
            et->op = rx;
        }
        else if (jointtype == 1)
        {
            et->op = ry;
        }
        else if (jointtype == 2)
        {
            et->op = rz;
        }
        else if (jointtype == 3)
        {
            et->op = tx;
        }
        else if (jointtype == 4)
        {
            et->op = ty;
        }
        else if (jointtype == 5)
        {
            et->op = tz;
        }

        ret = PyCapsule_New(et, "ET", NULL);
        return ret;
    }

    static PyObject *ET_T(PyObject *self, PyObject *args)
    {
        npy_intp dims[2] = {4, 4};
        int nd = 2;
        ET *et;
        PyObject *py_et, *py_eta;
        PyObject *py_ret = PyArray_EMPTY(nd, dims, NPY_DOUBLE, 1);
        double eta = 0;
        npy_float64 *ret;

        if (!PyArg_ParseTuple(args, "OO", &py_et, &py_eta))
            return NULL;

        if (!(et = (ET *)PyCapsule_GetPointer(py_et, "ET")))
            return NULL;

        if (py_eta != Py_None)
        {
            if (PyFloat_Check(py_eta))
            {
                eta = (double)PyFloat_AsDouble(py_eta);
            }
            else
            {
                PyErr_SetString(PyExc_TypeError, "Symbolic value");
                return NULL;
            }
        }

        ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret);
        // MapMatrix4dc e_ret(ret);

        _ET_T(et, ret, eta);

        return py_ret;
    }

    static PyObject *r2q(PyObject *self, PyObject *args)
    {
        // r is actually an SE3
        npy_float64 *r, *q;
        PyArrayObject *py_r, *py_q;

        if (!PyArg_ParseTuple(
                args, "O!O!",
                &PyArray_Type, &py_r,
                &PyArray_Type, &py_q))
            return NULL;

        r = (npy_float64 *)PyArray_DATA(py_r);
        q = (npy_float64 *)PyArray_DATA(py_q);

        _r2q(r, q);

        Py_RETURN_NONE;
    }

    int _check_array_type(PyObject *toCheck)
    {
        PyArray_Descr *desc;

        desc = PyArray_DescrFromObject(toCheck, NULL);

        // Check if desc is a number or a sympy symbol
        if (!PyDataType_ISNUMBER(desc))
        {
            PyErr_SetString(PyExc_TypeError, "Symbolic value");
            return 0;
        }

        return 1;
    }

    void rx(npy_float64 *data, double eta)
    {
        double st, ct;

        ct = cos(eta);
        st = sin(eta);

        data[0] = 1;
        data[4] = 0;
        data[8] = 0;
        data[12] = 0;
        data[1] = 0;
        data[5] = ct;
        data[9] = -st;
        data[13] = 0;
        data[2] = 0;
        data[6] = st;
        data[10] = ct;
        data[14] = 0;
        data[3] = 0;
        data[7] = 0;
        data[11] = 0;
        data[15] = 1;

        // data[0] = 1;
        // data[1] = 0;
        // data[2] = 0;
        // data[3] = 0;
        // data[4] = 0;
        // data[5] = ct;
        // data[6] = -st;
        // data[7] = 0;
        // data[8] = 0;
        // data[9] = st;
        // data[10] = ct;
        // data[11] = 0;
        // data[12] = 0;
        // data[13] = 0;
        // data[14] = 0;
        // data[15] = 1;
    }

    void ry(npy_float64 *data, double eta)
    {
        double st, ct;

        ct = cos(eta);
        st = sin(eta);

        data[0] = ct;
        data[4] = 0;
        data[8] = st;
        data[12] = 0;
        data[1] = 0;
        data[5] = 1;
        data[9] = 0;
        data[13] = 0;
        data[2] = -st;
        data[6] = 0;
        data[10] = ct;
        data[14] = 0;
        data[3] = 0;
        data[7] = 0;
        data[11] = 0;
        data[15] = 1;

        // data[0] = ct;
        // data[1] = 0;
        // data[2] = st;
        // data[3] = 0;
        // data[4] = 0;
        // data[5] = 1;
        // data[6] = 0;
        // data[7] = 0;
        // data[8] = -st;
        // data[9] = 0;
        // data[10] = ct;
        // data[11] = 0;
        // data[12] = 0;
        // data[13] = 0;
        // data[14] = 0;
        // data[15] = 1;
    }

    void rz(npy_float64 *data, double eta)
    {
        double st, ct;

        ct = cos(eta);
        st = sin(eta);

        data[0] = ct;
        data[4] = -st;
        data[8] = 0;
        data[12] = 0;
        data[1] = st;
        data[5] = ct;
        data[9] = 0;
        data[13] = 0;
        data[2] = 0;
        data[6] = 0;
        data[10] = 1;
        data[14] = 0;
        data[3] = 0;
        data[7] = 0;
        data[11] = 0;
        data[15] = 1;

        // data[0] = ct;
        // data[1] = -st;
        // data[2] = 0;
        // data[3] = 0;
        // data[4] = st;
        // data[5] = ct;
        // data[6] = 0;
        // data[7] = 0;
        // data[8] = 0;
        // data[9] = 0;
        // data[10] = 1;
        // data[11] = 0;
        // data[12] = 0;
        // data[13] = 0;
        // data[14] = 0;
        // data[15] = 1;
    }

    void tx(npy_float64 *data, double eta)
    {
        data[0] = 1;
        data[1] = 0;
        data[2] = 0;
        data[12] = eta;
        data[4] = 0;
        data[5] = 1;
        data[6] = 0;
        data[7] = 0;
        data[8] = 0;
        data[9] = 0;
        data[10] = 1;
        data[11] = 0;
        data[3] = 0;
        data[13] = 0;
        data[14] = 0;
        data[15] = 1;

        // data[0] = 1;
        // data[1] = 0;
        // data[2] = 0;
        // data[3] = eta;
        // data[4] = 0;
        // data[5] = 1;
        // data[6] = 0;
        // data[7] = 0;
        // data[8] = 0;
        // data[9] = 0;
        // data[10] = 1;
        // data[11] = 0;
        // data[12] = 0;
        // data[13] = 0;
        // data[14] = 0;
        // data[15] = 1;
    }

    void ty(npy_float64 *data, double eta)
    {
        data[0] = 1;
        data[1] = 0;
        data[2] = 0;
        data[3] = 0;
        data[4] = 0;
        data[5] = 1;
        data[6] = 0;
        data[13] = eta;
        data[8] = 0;
        data[9] = 0;
        data[10] = 1;
        data[11] = 0;
        data[12] = 0;
        data[7] = 0;
        data[14] = 0;
        data[15] = 1;

        // data[0] = 1;
        // data[1] = 0;
        // data[2] = 0;
        // data[3] = 0;
        // data[4] = 0;
        // data[5] = 1;
        // data[6] = 0;
        // data[7] = eta;
        // data[8] = 0;
        // data[9] = 0;
        // data[10] = 1;
        // data[11] = 0;
        // data[12] = 0;
        // data[13] = 0;
        // data[14] = 0;
        // data[15] = 1;
    }

    void tz(npy_float64 *data, double eta)
    {
        data[0] = 1;
        data[1] = 0;
        data[2] = 0;
        data[3] = 0;
        data[4] = 0;
        data[5] = 1;
        data[6] = 0;
        data[7] = 0;
        data[8] = 0;
        data[9] = 0;
        data[10] = 1;
        data[14] = eta;
        data[12] = 0;
        data[13] = 0;
        data[11] = 0;
        data[15] = 1;

        // data[0] = 1;
        // data[1] = 0;
        // data[2] = 0;
        // data[3] = 0;
        // data[4] = 0;
        // data[5] = 1;
        // data[6] = 0;
        // data[7] = 0;
        // data[8] = 0;
        // data[9] = 0;
        // data[10] = 1;
        // data[11] = eta;
        // data[12] = 0;
        // data[13] = 0;
        // data[14] = 0;
        // data[15] = 1;
    }

} /* extern "C" */


#include <libs/util/util.hpp>

#include <boost/algorithm/string.hpp>

#include <iostream>
#include <fstream>
#include <deque>

namespace util {

double interpolate(const std::vector<double> &x, const std::vector<double> &y, const double x_n)
{
  // If x_n is outside, return extreme
  if(x_n <= *x.cbegin())  { return *y.cbegin(); }
  if(x_n >= *x.crbegin()) { return *y.crbegin(); }

  for(size_t i = 0; i < x.size()-1; ++i) {
    if(x_n >= x[i] && x_n < x[i+1]) {
      double m = (y[i+1] - y[i])/(x[i+1] - x[i]);
      return y[i] + m*(x_n - x[i]);
    }
  }

  return 0.0;
}

std::vector<std::string> read_text_from_file(const std::string& filename)
{
  std::vector<std::string> text;
  std::ifstream ifs;
  ifs.open(filename, std::ifstream::in);

  std::string line;
  while(std::getline(ifs, line)) {
    text.push_back(line);
  }

  ifs.close();
  return text;
}

std::map<std::string, std::vector<double>> read_data_from_csv(const std::string& csv_file)
{
  // Read all lines fom file
  std::vector<std::string> text = read_text_from_file(csv_file);

  // Parse first line (i.e. the header) to get column names
  std::string header = text[0];
  text.erase(text.begin());

  std::vector<std::string> columns;
  boost::split(columns, header, boost::is_any_of(","));

  // Parse each line of file to get data
  std::vector<std::vector<double>> raw_data;
  for(size_t i = 0; i < columns.size(); ++i)
    raw_data.push_back(std::vector<double>());

  for(auto &line : text) {
    std::vector<std::string> data_str;
    boost::algorithm::trim(line);
    boost::split(data_str, line, boost::is_any_of(","));

    for(size_t i = 0; i < data_str.size(); ++i) {
      raw_data[i].push_back(std::stod(data_str[i]));
    }
  }

  // Combine header and data in single container
  std::map<std::string, std::vector<double>> data;
  for(size_t i = 0; i < columns.size(); ++i) {
    boost::algorithm::trim(columns[i]);
    data[columns[i]] = raw_data[i];
  }
  return data;
}

void write_data_to_csv(const std::string &csv_file, std::map<std::string, std::vector<double>> data)
{
  std::ofstream ofs;
  ofs.open(csv_file, std::ios::out | std::ios::trunc);

  // Parse column names and write first line (header)
  std::vector<std::string> headers;
  for(auto const &x : data) {
    headers.push_back(x.first);
    ofs << x.first;
    if(x.first == data.rbegin()->first) {
      ofs << "\n";
    } else {
      ofs << ",";
    }
  }

  // Use smallest vector of data if they aren't the same size.
  size_t n = (*std::min_element(data.begin(), data.end(), [](
    const std::pair<std::string, std::vector<double>> &i,
    const std::pair<std::string, std::vector<double>> &j)
    { return i.second.size() < j.second.size(); })).second.size();

  // Write data to file
  for(size_t i = 0; i < n ; ++i) {
    for(const auto &name : headers) {
      ofs << data[name][i];
      if(name == (*headers.rbegin())) {
        ofs << "\n";
      } else {
        ofs << ",";
      }
    }
  }

  ofs.close();
}

// Credit: Tatu Ylonen <ylo@cs.hut.di>
// See: https://opensource.apple.com/source/OpenSSH/OpenSSH-7.1/openssh/ttymodes.c?txt
int speed_to_baud(speed_t speed)
{
	switch (speed) {
	case B0:
		return 0;
	case B50:
		return 50;
	case B75:
		return 75;
	case B110:
		return 110;
	case B134:
		return 134;
	case B150:
		return 150;
	case B200:
		return 200;
	case B300:
		return 300;
	case B600:
		return 600;
	case B1200:
		return 1200;
	case B1800:
		return 1800;
	case B2400:
		return 2400;
	case B4800:
		return 4800;
	case B9600:
		return 9600;

#ifdef B19200
	case B19200:
		return 19200;
#else // B19200
#ifdef EXTA
	case EXTA:
		return 19200;
#endif // EXTA
#endif // B19200

#ifdef B38400
	case B38400:
		return 38400;
#else // B38400
#ifdef EXTB
	case EXTB:
		return 38400;
#endif // EXTB
#endif // B38400

#ifdef B7200
	case B7200:
		return 7200;
#endif // B7200
#ifdef B14400
	case B14400:
		return 14400;
#endif // B14400
#ifdef B28800
	case B28800:
		return 28800;
#endif // B28800
#ifdef B57600
	case B57600:
		return 57600;
#endif // B57600
#ifdef B76800
	case B76800:
		return 76800;
#endif // B76800
#ifdef B115200
	case B115200:
		return 115200;
#endif // B115200
#ifdef B230400
	case B230400:
		return 230400;
#endif // B230400
	default:
		return 9600;
	}
}

} // namespace util


// 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.

#ifndef MTL_TAG_INCLUDE
#define MTL_TAG_INCLUDE

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

namespace mtl { namespace tag {

/** @defgroup Tags Tags for concept-free dispatching
 *  @{
 */

// For internal use (e.g., to invalidate range generators)
// Is this the same as bottom?
struct unsupported {};

// Name says it
struct dummy3 {};
struct dummy4 {};

/// Tag for all types
struct universe {};

/// Tag used to flatten categories
/** The virtual derivation causes perceivable run-time overhead that can be avoided with this struct using traits::flatcat1 and such. **/
template <typename T> struct flat : universe {};

// Tag for any scalar value
/** At the moment default for unknown types (will be precised later) */
struct scalar : virtual universe {};

/// For non-MTL types with category not explicitly defined
/** At the moment all treated as scalars (will be precised later) */
struct unknown : virtual scalar {};

/// Tag for intermediate objects that require explicit evaluation
struct unevaluated : virtual universe {};

/// Any collection, i.e. vectors, matrices or higher-dimensional tensor
struct collection : virtual universe {};

/// Tag for any MTL vector (and user-declared MTL vectors)
struct vector : virtual collection {};

/// Tag for references to vector
/** For instance to not access memory directly but use functions, e.g. in set_to_zero. **/
struct vector_ref : virtual vector {};

/// Tag for any MTL column vector (and user-declared MTL vectors)
struct col_vector : virtual vector {};

/// Tag for any MTL row vector (and user-declared MTL vectors)
struct row_vector : virtual vector {};

/// Tag for any MTL matrix (and user-declared MTL matrices)
struct matrix : virtual collection {};

/// Tag for any dense collection
struct dense : virtual universe {};

/// Tag for vectors with one-dimensional memory addressing
/** offet v_i is x*i for some x */
struct has_1D_layout : virtual dense {};

/// Tag for matrices with two-dimensional memory addressing
/** offet a_ij is x*i + y*j for some x and y */
struct has_2D_layout : virtual dense {};

/// Tag for any sparse collection
struct sparse : virtual universe {};

// for distinction between dense and sparse matrices
struct dense_matrix : virtual dense, virtual matrix {};
struct sparse_matrix : virtual sparse, virtual matrix {};

/// Tag for collections where values are stored contigously in memory
struct contiguous_memory : virtual universe {};

/// Tag for dense and contiguous collections
/** Only short cut */
struct contiguous_dense : virtual dense, virtual contiguous_memory {};

/// Collection with iterator
struct has_iterator : virtual universe {};

/// Collection with random-access iterator
struct has_ra_iterator : virtual has_iterator {};

/// Collection with fast random-access iterator
/** Meaning: unrolling is probably beneficial. Counter-example: Morton-ordered matrices have
    random access but this is so slow that regular traversal is favorable */
struct has_fast_ra_iterator : virtual has_ra_iterator {};

/// Collection with cursor
struct has_cursor : virtual universe {};

/// Collection with random-access cursor
struct has_ra_cursor : virtual has_cursor {};

/// Collection with fast random-access cursor
/** Meaning: unrolling is probably beneficial. Counter-example: Morton-ordered matrices have
    random access but this is so slow that regular traversal is favorable */
struct has_fast_ra_cursor : virtual has_ra_cursor {};

/// Tag for matrices with sub_matrix function exist and doesn't say for which ranges it is defined
struct has_sub_matrix : virtual universe {};

/// Sub-divisible into quadrants, i.e. arbitrary sub-matrices not necessarily supported but recursion works
//  more explanation needed
struct qsub_divisible : virtual has_sub_matrix {};

/// Tag for sub-divisible matrix, i.e. sub_matrix works
struct sub_divisible : virtual qsub_divisible {};

/// Tag for dense row vector in the category lattice
struct dense_row_vector
  : virtual row_vector, virtual contiguous_dense,
    virtual has_fast_ra_iterator, virtual has_fast_ra_cursor, virtual has_1D_layout
{};

/// Tag for dense column vector in the category lattice
struct dense_col_vector
  : virtual col_vector, virtual contiguous_dense,
    virtual has_fast_ra_iterator, virtual has_fast_ra_cursor, virtual has_1D_layout
{};

/// Tag for strided row vector in the category lattice
struct strided_row_vector
  : virtual row_vector, virtual vector_ref,
    virtual has_fast_ra_iterator, virtual has_fast_ra_cursor, virtual has_1D_layout
{};

/// Tag for strided column vector in the category lattice
struct strided_col_vector
  : virtual col_vector, virtual vector_ref,
    virtual has_fast_ra_iterator, virtual has_fast_ra_cursor, virtual has_1D_layout
{};

/// Tag for sparse row vector in the category lattice
struct sparse_row_vector
  : virtual row_vector, virtual sparse
{};

/// Tag for sparse column vector in the category lattice
struct sparse_col_vector
  : virtual col_vector, virtual sparse
{};

/// Tag to handle std::vector in the category lattice
struct std_vector
  : virtual vector, virtual contiguous_dense, virtual has_1D_layout
{};

/// Tag for a view on a (regular) dense matrix in the category lattice
/** The map perform address computation and has therefore no 2D-layout.
    It is also not (yet) assumed that the view provides iterators. */
struct dense2D_view
  : virtual matrix, virtual contiguous_dense, virtual has_fast_ra_cursor
    // , virtual sub_divisible // is currently not sub-divisible
{};

/// Tag for (regular) dense matrix in the category lattice
struct dense2D
  : virtual dense2D_view, virtual has_fast_ra_iterator, virtual has_2D_layout, virtual sub_divisible
{};

struct implicit_dense
  : virtual matrix, virtual dense, virtual has_fast_ra_cursor
{};

/// Tag for a view on a Morton-order matrix in the category lattice
/** It is not (yet) assumed that the view provides iterators. */
struct morton_view
  : virtual matrix, virtual contiguous_dense,
    virtual has_ra_cursor // , virtual qsub_divisible // is currently not sub-divisible
{};

/// Tag for Morton-order matrix in the category lattice
struct morton_dense
  : virtual morton_view, virtual has_ra_iterator, virtual qsub_divisible
{};

/// Tag for a view on a compressed matrix in the category lattice
/** It is not (yet) assumed that the view provides iterators. */
struct compressed2D_view
  : virtual matrix, virtual sparse, virtual has_cursor
{};

/// Tag for compressed matrix in the category lattice
struct compressed2D
  : virtual compressed2D_view, virtual has_iterator
{};

/// Tag for multi_vector
// Maybe splitting later into sparse and dense form
struct multi_vector
  : virtual matrix, virtual dense
{};

/// Tag for transposed multi_vector
// Maybe splitting later into sparse and dense form
struct transposed_multi_vector
  : virtual matrix, virtual dense
{};

/// Tag for transposed multi_vector
// Maybe splitting later into sparse and dense form
struct hermitian_multi_vector
  : virtual matrix, virtual dense
{};

/// Tag for ell_matrix (preliminary)
struct ell_matrix
  : sparse_matrix
{};

/// Tag for element structure matrix
struct element_structure
  : sparse_matrix
{};

/// Tag for element structure matrix
struct sparse_banded_matrix
  : sparse_matrix
{};

/// Tag for mat_cvec_multiplier
struct mat_cvec_multiplier
  : col_vector
{};

/// Tag for implicit dense matrices

/// Tag for bottom of the category lattice
/** Only for completeness; probably not needed in practice. */
struct bottom
  : virtual compressed2D, virtual morton_dense, virtual dense2D,
    virtual dense_col_vector, virtual dense_row_vector, virtual unknown,
    virtual multi_vector
{};

template <typename Tag1, typename Tag2, typename Tag3= dummy3, typename Tag4= dummy4>
struct join
  : virtual Tag1, virtual Tag2, virtual Tag3, virtual Tag4
{};

// =====================
// Types for orientation
// =====================

/// Characterizes row-major orientation in matrices and row vector in 1D
struct row_major {};

/// Characterizes column-major orientation in matrices and column vector in 1D
struct col_major {};

/// Common base for diagonal tags
struct universe_diagonal {};

/// Tag indicating that diagonal is stored regularly
struct regular_diagonal : universe_diagonal {};

/// Tag indicating that diagonal contains unit elements
struct unit_diagonal : universe_diagonal {};

/// Tag indicating that diagonal entries are stored as inverses
/** Storing value in different ways can be faster in several algorithms.
    By the time of this writing it is experimental and only used
    in upper_trisolve and lower_trisolve. **/
struct inverse_diagonal : universe_diagonal {};

/*@}*/ // end of group Tags

} // namespace mtl::tag

/** @addtogroup Tags
 *  @{
 */

/// Characterizes row-major orientation in matrices and row vector in 1D
using tag::row_major;

/// Characterizes column-major orientation in matrices and column vector in 1D
using tag::col_major;

/*@}*/ // end of group Tags

// =====================
// Tags for traversals
// Import some from GLAS
// =====================

namespace tag {

/** @addtogroup Tags
 *  @{
 */

    /// Tag for cursor traversal of non-zero elements of a collection
    /** Also used for elements within rows and columns */
    using glas::tag::nz;

    /// Tag for cursor traversal of all elements of a collection
    /** Also used for elements within rows and columns */
    using glas::tag::all;

    /// Tag for traversal of all rows in a matrix
    using glas::tag::row;
    /// Tag for traversal of all columns in a matrix
    using glas::tag::col;

    /// Tag for traversal of a matrix's major dimension
    /** Is equivalent to glas::tag::row for row-major matrices and
	glas::tag::col for column-major matrices */
    using glas::tag::major;

    /// Tag for traversal of a matrix's minor dimension
    /** Is equivalent to glas::tag::row for row-major matrices and
	glas::tag::col for column-major matrices */
    using glas::tag::minor;

    // To define iterators over matrices or rows/cols of it, vectors

    namespace iter {

	/// Tag for iterator traversal of non-zero elements of a collection
	/** Also used for elements within rows and columns */
	struct nz {};

	/// Tag for iterator traversal of all elements of a collection
	/** Also used for elements within rows and columns */
	struct all {};

    } // namespace mtl::tag::iter

    // Same with const iterators

    namespace const_iter {

	/// Tag for const-iterator traversal of non-zero elements of a collection
	/** Also used for elements within rows and columns */
	struct nz {};

	/// Tag for const-iterator traversal of all elements of a collection
	/** Also used for elements within rows and columns */
	struct all {};

    } // namespace mtl::tag::const_iter

/*@}*/ // end of group Tags

} // namespace mtl::tag

} // namespace mtl

#endif // MTL_TAG_INCLUDE


/* Copyright (c) 2019, Acme Robotics, Ethan Quist, Corbyn Yhap
 * 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 <organization> 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 <COPYRIGHT HOLDER> 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.
 *
 * @file DHTable.hpp
 *
 * @brief The class header for our DH Table Class
 *
 * @author Corbyn Yhap (driver) and Ethan Quist (Navigator)
 *
 * @copyright Acme Robotics, Ethan Quist, Corbyn Yhap
 */
#pragma once

#include <Eigen/Dense>

#include <memory>
#include <vector>

#include "PrismaticJoint.hpp"
#include "RevoluteJoint.hpp"

class DHTable {
 public:
  // Default values prevent these pointers from being null.
  struct Frame {
    std::shared_ptr<PrismaticJoint> d = std::make_shared<PrismaticJoint>(
        PrismaticJoint());
    std::shared_ptr<RevoluteJoint> theta = std::make_shared<RevoluteJoint>(
        RevoluteJoint());
    std::shared_ptr<PrismaticJoint> a = std::make_shared<PrismaticJoint>(
        PrismaticJoint());
    std::shared_ptr<RevoluteJoint> alpha = std::make_shared<RevoluteJoint>(
        RevoluteJoint());
  };

  /**

   * @brief DHTable Constructor with no Parameters.

   * @param None.

   * @return None.

   */
  DHTable();

  /**

   * @brief Destructor for the DHTable Class

   * @param None.

   * @return None.

   */
  ~DHTable();

  /**

   * @brief addFrame Method for the user to describe the Robot
   * The Frame access is 1 indexed. (Preserving 0 for the
   *  base frame) Frames must be added in the order of relation. i.e. frame 1
   *  describes the relationship from frame 0 to frame 1 and frame 2 the
   *  relationship from frame 1 to frame 2.

   * @param const Frame &, The Frame that will be added to the DH Table

   * @return None.

   */
  void addFrame(const Frame&);

  /**

   * @brief getFrame returns a copy of the frame that was stored

   * @param std::vector<Frame>::size_type 1 Indexed index of desired frame

   * @return A copy of the frame.

   */
  Frame getFrame(std::vector<Frame>::size_type);

  /**

   * @brief getTransform This function returns the Transformation Matrix from
   *  one frame to the another. Currently it is required that the first frame
   *  must precede the second frame.

   * @param size_type,  The index of the frame being transformed from.

   * @param size_type,  The index of the frame being transformed to.

   * @return Eigen::Matrix4d The Transformation Matrix to go from frame one to
   * frame two.

   */
  Eigen::Matrix4d getTransform(std::vector<Frame>::size_type,
                               std::vector<Frame>::size_type);

 private:
  std::vector<Frame> frames;
  /**

   * @brief getTransform This function acts as a helper by taking the index of
   * a frame, retrieving that frame and then using those parameters to build a
   * tranformation matrix.

   * @param size_type,  The index of the frame being transformed to. This
   * function assumed that the tranform from is from the frame that preceded it.

   * @return Eigen::Matrix4d The Transformation Matrix to go to frame index from
   * the previous frame.

   */
  Eigen::Matrix4d getTransform(std::vector<Frame>::size_type);
};