#include <chrono>
#include <ctime>
#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Geometry>
using namespace std;

//default input values
string inputFile = "trajBig.pdb";
string outputFile = "output.txt";
int spheresAllocationFrame = 0;
double RMSDThreshPercent = 50;

//additional parameters
double sphereRadius = 8;

int SPHERES;
int ATOMS;
int FRAMES;
double highestSize;

//Atoms[<frame>][<atom>][<coordinate>]
vector<vector<vector<double>>> A;

//RMSDValue[<frame>][<sphere>]
vector<vector<double>> C;

//Maps sphere to CA; CAAtomNumber[<sphere>]
vector<int> sphereCA;

//Numer of atoms in [<sphere>]
vector<int> sphereSize;

struct sphere
{
    int frame;
    int index;
    double RMSD;
};

vector<vector<int>> sphereAtoms;
vector<vector<int>> atomToSphere;
vector<vector<double>> highestRMSD;
vector<sphere> highestRMSDbase;

bool operator <(const sphere& lhs, const sphere& rhs)
{
    return lhs.RMSD < rhs.RMSD;
}

vector<string> result;

//--------------------------------------------------

//reading data from input pdb file.
void readFile(string filename)
{
    string line;
    ifstream myfile(filename);
    if (myfile.is_open())
    {
        int frame = 0;
        int atom;
        SPHERES = 0;
        FRAMES = 0;
        ATOMS = 0;
        A = {};
        sphereCA = {};
        sphereSize = {};
        while (getline(myfile, line))
        {
            if (line[0] == 'M')
            {
                frame = stoi(line.substr(9, 5));
                frame--;
                A.push_back({});
                C.push_back({});
                FRAMES++;
            }
            else if (line[0] == 'A')
            {
                atom = stoi(line.substr(6, 5));
                atom--;
                A[frame].push_back({});
                A[frame][atom].push_back(stod(line.substr(30, 8)));
                A[frame][atom].push_back(stod(line.substr(38, 8)));
                A[frame][atom].push_back(stod(line.substr(46, 8)));
                if (frame == 0)
                {
                    ATOMS++;
                    if (line[14] == 'A' and line[13] == 'C')
                    {
                        sphereCA.push_back(atom);
                        sphereSize.push_back(0);
                        SPHERES++;
                    }
                }
            }
        }
        myfile.close();
    }
    else
    {
        cout << "Nie odnaleziono pliku!" << endl;
    }
}

//calculating distance between 2 atoms, used when allocating atoms to spheres.
double atomsDistanceCalc(int atom1, int atom2)
{
    double result = sqrt(pow(A[spheresAllocationFrame][atom1][0] - A[spheresAllocationFrame][atom2][0], 2) +
        pow(A[spheresAllocationFrame][atom1][1] - A[spheresAllocationFrame][atom2][1], 2) +
        pow(A[spheresAllocationFrame][atom1][2] - A[spheresAllocationFrame][atom2][2], 2));
    return result;
}

//allocating atoms into spheres, based on sphereRadius
void atomsAllocation()
{
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    int checkpoint = 0;
    sphereAtoms = {};
    atomToSphere = {};
    for (int i = 0; i < SPHERES; i++)
    {
        sphereAtoms.push_back({});
    }
    for (int i = 0; i < ATOMS; i++)
    {
        atomToSphere.push_back({});
        for (int j = 0; j < SPHERES; j++)
        {
            if (atomsDistanceCalc(i, sphereCA[j]) <= sphereRadius)
            {
                atomToSphere[i].push_back(j);
                sphereAtoms[j].push_back(i);
                sphereSize[j]++;
            }
        }
        if ((i - checkpoint) >= 0.1 * ATOMS)
        {
            checkpoint = i;
            timestamp = std::chrono::system_clock::now();
            timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
            cout << "[RMSD] Atom number " << i + 1 << " of " << ATOMS << " allocated to spheres at " << std::ctime(&timestamp_print) << endl;
        }
    }
}

// Find3DAffineTransform is from oleg-alexandrov repository on github, available here https://github.com/oleg-alexandrov/projects/blob/master/eigen/Kabsch.cpp [as of 27.01.2022]
// Given two sets of 3D points, find the rotation + translation + scale
// which best maps the first set to the second.
// Source: http://en.wikipedia.org/wiki/Kabsch_algorithm

// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {

    // Default output
    Eigen::Affine3d A;
    A.linear() = Eigen::Matrix3d::Identity(3, 3);
    A.translation() = Eigen::Vector3d::Zero();

    if (in.cols() != out.cols())
        throw "Find3DAffineTransform(): input data mis-match";

    // First find the scale, by finding the ratio of sums of some distances,
    // then bring the datasets to the same scale.
    double dist_in = 0, dist_out = 0;
    for (int col = 0; col < in.cols() - 1; col++) {
        dist_in += (in.col(col + 1) - in.col(col)).norm();
        dist_out += (out.col(col + 1) - out.col(col)).norm();
    }
    if (dist_in <= 0 || dist_out <= 0)
        return A;
    double scale = dist_out / dist_in;
    out /= scale;

    // Find the centroids then shift to the origin
    Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
    Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
    for (int col = 0; col < in.cols(); col++) {
        in_ctr += in.col(col);
        out_ctr += out.col(col);
    }
    in_ctr /= in.cols();
    out_ctr /= out.cols();
    for (int col = 0; col < in.cols(); col++) {
        in.col(col) -= in_ctr;
        out.col(col) -= out_ctr;
    }

    // SVD
    Eigen::MatrixXd Cov = in * out.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Find the rotation
    double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
    if (d > 0)
        d = 1.0;
    else
        d = -1.0;
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
    I(2, 2) = d;
    Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

    // The final transform
    A.linear() = scale * R;
    A.translation() = scale * (out_ctr - R * in_ctr);

    return A;
}

//superpose changes vector of atoms frame 2 to map atoms from frame 1 in the way to minimise RMSD between both frames
void superpose(const vector<vector<double>>& frame1, vector<vector<double>>& frame2)
{
    int atomsInSphere = frame1.size();
    Eigen::Matrix3Xd S1(3, atomsInSphere);
    Eigen::Matrix3Xd S2(3, atomsInSphere);
    for (int j = 0; j < atomsInSphere; j++)
    {
        for (int k = 0; k < 3; k++)
        {
            S1(k, j) = frame1[j][k];
            S2(k, j) = frame2[j][k];
        }
    }
    Eigen::Affine3d RT = Find3DAffineTransform(S2, S1);
    S2 = RT.linear() * S2;
    for (int j = 0; j < atomsInSphere; j++)
    {
        S2.block<3, 1>(0, j) += RT.translation();
    }
    for (int j = 0; j < atomsInSphere; j++)
    {
        for (int k = 0; k < 3; k++)
        {
            frame2[j][k] = S2(k, j);
        }
    }
}

//initializing vector for RMSD values
void initializeC()
{
    C = {};
    for (int i = 0; i < FRAMES; i++)
    {
        C.push_back({});
        for (int j = 0; j < SPHERES; j++)
            C[i].push_back(0);
    }
}

//calculating RMSD on spheres, on adjacent frames
void calculateRMSDSuperpose()
{
    initializeC();
    vector<vector<vector<double>>> sphereMatrix;
    vector<vector<double>> tempMatrix;
    double tempRMSD;
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    int checkpoint = 0;
    for (int s = 0; s < SPHERES; s++)
    {
        int atomsInSphere = sphereAtoms[s].size();
        sphereMatrix = {};
        for (int i = 0; i < FRAMES; i++)
        {
            sphereMatrix.push_back({});
            for (int j = 0; j < atomsInSphere; j++)
            {
                sphereMatrix[i].push_back(A[i][sphereAtoms[s][j]]);
            }
            if (i > 0)
            {
                tempMatrix = sphereMatrix[i];
                superpose(sphereMatrix[i - 1], tempMatrix);
                for (int j = 0; j < atomsInSphere; j++)
                {
                    for (int k = 0; k < 3; k++)
                    {
                        tempRMSD = pow(tempMatrix[j][k] - sphereMatrix[i - 1][j][k], 2);
                        C[i - 1][s] += tempRMSD;
                    }
                }
                C[i - 1][s] /= ((long)atomsInSphere * (long)3);
                C[i - 1][s] = sqrt(C[i - 1][s]);
            }
        }
        if ((s - checkpoint) >= 0.1 * SPHERES)
        {
            checkpoint = s;
            timestamp = std::chrono::system_clock::now();
            timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
            cout << "[RMSD] RMSD calculated for " << s + 1 << " of " << SPHERES << " spheres at " << std::ctime(&timestamp_print) << endl;
        }
    }
}

//initializing vector for storing information about spheres with highest RMSD
void initializeHighestRMSD()
{
    highestRMSD = {};
    highestSize = (double)SPHERES / 100;
    highestSize *= (long)(FRAMES - 1);
    highestSize *= RMSDThreshPercent;
    for (int i = 0; i < highestSize; i++)
    {
        highestRMSD.push_back({});
        for (int j = 0; j < 3; j++)
            highestRMSD[i].push_back(-1);
    }
}

//choosing frames and spheres with highest RMSD, they will be colored later
void chooseHighestRMSD()
{
    initializeHighestRMSD();
    for (int i = 0; i < FRAMES - 1; i++)
    {
        for (int j = 0; j < SPHERES; j++)
        {
            for (unsigned int k = 0; k < highestRMSD.size(); k++)
            {
                if (C[i][j] > highestRMSD[k][2])
                {
                    for (int l = highestRMSD.size() - 1; l > k; l--)
                    {
                        for (int m = 0; m < 3; m++)
                        {
                            highestRMSD[l][m] = highestRMSD[l - 1][m];
                        }
                    }
                    highestRMSD[k][0] = i;
                    highestRMSD[k][1] = j;
                    highestRMSD[k][2] = C[i][j];
                    break;
                }
            }
        }
    }
}

//choosing frames and spheres with highest RMSD, they will be colored later
void chooseHighestRMSDquick()
{
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    initializeHighestRMSD();
    int counter = 0;
    for (int i = 0; i < FRAMES - 1; i++)
    {
        for (int j = 0; j < SPHERES; j++)
        {
            highestRMSDbase.push_back(sphere());
            highestRMSDbase[counter].frame = i;
            highestRMSDbase[counter].index = j;
            highestRMSDbase[counter].RMSD = C[i][j];
            counter++;
        }
    }
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] RMSD started being sorted at " << std::ctime(&timestamp_print) << endl;
    sort(highestRMSDbase.begin(), highestRMSDbase.end());
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] RMSD sorted at " << std::ctime(&timestamp_print) << endl;
    for (unsigned int k = 0; k < highestRMSD.size(); k++)
    {
        highestRMSD[k][0] = highestRMSDbase[k].frame;
        highestRMSD[k][1] = highestRMSDbase[k].index;
        highestRMSD[k][2] = highestRMSDbase[k].RMSD;
    }
}

//calculates result variable content, basen on highestRMSD
void calculateResult()
{
    result = {};
    int numberOfSpheresToColor = highestRMSD.size();
    vector<vector<int>> resultSpheres;
    for (int i = 0; i < FRAMES - 1; i++)
    {
        resultSpheres.push_back({});
    }
    for (int i = 0; i < numberOfSpheresToColor; i++)
    {
        resultSpheres[highestRMSD[i][0]].push_back(highestRMSD[i][1]);
    }
    string subResult;
    for (int i = 0; i < FRAMES - 1; i++)
    {
        subResult = "";
        for (int j = 0; j < numberOfSpheresToColor; j++)
        {
            if (highestRMSD[j][0] == i)
            {
                stringstream ss;
                string s;
                ss << sphereCA[highestRMSD[j][1]];
                ss >> s;
                subResult += s + " ";
            }
        }
        result.push_back(subResult);
    }
}

//saving results to an output file
void saveToFile(string filename)
{
    ofstream myfile(filename);
    if (myfile.is_open())
    {
        for (int i = 0; i < FRAMES - 1; i++)
        {
            if (!result[i].empty())
            {
                myfile << i << ' ' << result[i] << endl;
            }
        }
        myfile.close();
    }
}

//debug function for checking superpose results
void makePDB(string filename)
{
    vector<vector<vector<double>>> sphereMatrix;
    int atomsInSphere = sphereAtoms[0].size();
    sphereMatrix = {};
    for (int i = 0; i < 2; i++)
    {
        sphereMatrix.push_back({});
        for (int j = 0; j < atomsInSphere; j++)
        {
            sphereMatrix[i].push_back(A[i][sphereAtoms[0][j]]);
        }
        if (i > 0)
        {
            superpose(sphereMatrix[i - 1], sphereMatrix[i]);
        }
    }
    string line;
    ifstream myfilein(filename);
    filename += "out";
    ofstream myfileout("testBase.pdb");
    ofstream myfilesuper("testSuperpose.pdb");
    int counter = 0;
    if (myfilein.is_open() && myfileout.is_open())
    {
        int frame = 0;
        int atom;
        while (getline(myfilein, line))
        {
            if (line[0] == 'M')
            {
                myfileout << line << endl;
                myfilesuper << line << endl;
            }

            if (line[0] == 'E')
            {
                myfileout << line << endl;
                myfilesuper << line << endl;
                frame++;
                if (frame > 1)
                {
                    break;
                }
            }
            if (line[0] == 'A')
            {
                atom = stoi(line.substr(6, 5));
                atom--;
                for (unsigned int i = 0; i < sphereAtoms[0].size(); i++)
                {
                    if (sphereAtoms[0][i] == atom)
                    {
                        myfileout << line << endl;
                        if (frame == 1)
                        {
                            line.replace(32, 6, to_string(sphereMatrix[1][counter][0]), 0, 6);
                            line.replace(40, 6, to_string(sphereMatrix[1][counter][1]), 0, 6);
                            line.replace(48, 6, to_string(sphereMatrix[1][counter][2]), 0, 6);
                            myfilesuper << line << endl;
                            counter++;
                        }
                        else
                        {
                            myfilesuper << line << endl;
                        }
                    }
                }
            }
        }
        myfilein.close();
        myfileout.close();
        myfilesuper.close();
    }
}

int main(int argc, char* argv[])
{
    auto start = chrono::high_resolution_clock::now();
    if (argc == 5)
    {
        inputFile = string(argv[1]);
        outputFile = string(argv[2]);
        spheresAllocationFrame = stoi(argv[3]) - 1;
        RMSDThreshPercent = stod(argv[4]);
    }
    readFile(inputFile);
    if (spheresAllocationFrame < 0)
    {
        spheresAllocationFrame = 1;
    }
    else if (spheresAllocationFrame >= FRAMES)
    {
        spheresAllocationFrame = FRAMES - 1;
    }
    auto timestamp = std::chrono::system_clock::now();
    std::time_t timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Input file: " << inputFile << " loaded at " << std::ctime(&timestamp_print) << endl;
    atomsAllocation();
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Atoms allocated to spheres at " << std::ctime(&timestamp_print) << endl;
    calculateRMSDSuperpose();
    chooseHighestRMSDquick();
    calculateResult();
    saveToFile(outputFile);
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Output file: " << outputFile << " saved at " << std::ctime(&timestamp_print) << endl;
    //makePDB(inputFile);
    auto stop = chrono::high_resolution_clock::now();
    chrono::duration<double> elapsed = stop - start;
    timestamp = std::chrono::system_clock::now();
    timestamp_print = std::chrono::system_clock::to_time_t(timestamp);
    cout << "[RMSD] Computation time: " << elapsed.count() << "s, ended at: " << std::ctime(&timestamp_print) << endl;
}


#include "WebotsSource.h"

#include "Utils/TimeMeasurement.h"

#include <spdlog/spdlog.h>
#include <cstdlib>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/math/constants/constants.hpp>

#include <algorithm>
#include <iostream>
#include <random>
#include <cmath>

using namespace LpSlam;

/** roomba controller
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
*/

#define BUMPER_LEFT 0
#define BUMPER_RIGHT 1
static const char *bumpers_name[BUMPERS_NUMBER] = {"bumper_left", "bumper_right"};

#define CLIFF_SENSOR_LEFT 0
#define CLIFF_SENSOR_FRONT_LEFT 1
#define CLIFF_SENSOR_FRONT_RIGHT 2
#define CLIFF_SENSOR_RIGHT 3
static const char *cliff_sensors_name[CLIFF_SENSORS_NUMBER] = {"cliff_left", "cliff_front_left", "cliff_front_right",
                                                               "cliff_right"};

#define MAX_SPEED 16.0
#define NULL_SPEED 0
#define HALF_SPEED 8
#define MIN_SPEED -16

#define WHEEL_RADIUS 0.031
#define AXLE_LENGTH 0.271756
#define ENCODER_RESOLUTION 507.9188

namespace NavigationHelper {

    inline double computeDistanceTarget( Position3 roboPos, Position3 target ){
        return (target.value - roboPos.value).norm();
    }

    inline double computeDirectionAngle( Position3 roboPos, Orientation roboOrientation, Position3 target ){

        //std::cout << "roboPos " << roboPos.value << std::endl;
        //std::cout << "target " << target.value << std::endl;
        //std::cout << "roboOrientation " << roboOrientation.value.x() << std::endl;

        // compute relative distance between robo and target
        const Vector3 targetDirection = (target.value - roboPos.value).normalized();

        //std::cout << "targetDirection " << targetDirection << std::endl;

        // construct direction vector of robot
        const Vector3 roboDirection = roboOrientation.value.inverse() * Vector3(0.0, 0.0, 1.0);

        //std::cout << "roboDirection " << roboDirection << std::endl;

        // compute angle between vectors, unsigned
        double angle = std::acos(targetDirection.dot(roboDirection));

        // compute direction of angle
        const Vector3 cross = targetDirection.cross(roboDirection);

        if (cross.x() < 0) {
            angle = - angle;
        }
        //std::cout << "angle " << angle << std::endl;
        return angle;
    }
}

WebotsSource::WebotsSource() :
    m_worker( [](WebotsSource::WorkerThreadParams & params) -> bool {

        std::random_device rd;
        std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
        std::uniform_real_distribution<> dis(0.0, 0.8);
        std::uniform_real_distribution<> dis_rot(0.0, 0.2);

        std::normal_distribution<> dis_pos_mod(0.0, 0.02);
        std::normal_distribution<> dis_quat_mod(0.0, 0.01);
        std::normal_distribution<> dis_vel_mod(0.0, 0.02);

        bool applySmearing = false;

        double speed[2];
        double sensors_value[8];
        double braitenberg_coefficients[8][2] = {{0.942, -0.22}, {0.63, -0.1}, {0.5, -0.06},  {-0.06, -0.06},
                                                {-0.06, -0.06}, {-0.06, 0.5}, {-0.19, 0.63}, {-0.13, 0.942}};

        const double cRange = (1024 / 2);

        if (params.robo->step(params.timestep) == -1) {
            return false;
        }
        *params.total_time += params.timestep;

        const double *a = params.accelerometer->getValues();

        // unit is rad/s
        const double *g = params.gyro->getValues();

        // dump out sensor values
        SPDLOG_DEBUG("Got Imu measurement from Webots: {}, {}, {}", g[0],
            g[1], g[2]);

        auto thisTimestampMs = std::chrono::milliseconds( *params.total_time );
        auto absoluteTimestamp = params.initialTime + thisTimestampMs;

        TrackerCoordinateSystemBase base;
        SensorQueueEntry qentry(absoluteTimestamp,
            base,
            Acceleration3(a[0], a[1], a[2]),
            AngularVelocity3(g[0], g[1], g[2]));
        params.sensorQ->push(qentry);

        // get all the statistics from the camera node
        // because that is where we run the position reconstruction
        const double * veloArray = params.camera_node->getVelocity();
        const double * posArray = params.camera_node->getPosition();
        const double * orientArray = params.camera_node->getOrientation();

        Vector3 velo_w( veloArray[0], veloArray[1], veloArray[2]);
        //std::cout << "Robot Velocity (unrot)" << velo_w << std::endl;

        /*
        Webots coordinates system is:
        +z : forward
        +x : left
        +y : up
        in the robot direction if the robot is rotated by 180 degrees
        around the y axis. In the robots unrotated coordinate frame
        it would drive backwards because forward is in the -z direction
        */

        Vector3 pos_w( posArray[1], -posArray[0], posArray[2]);
        //std::cout << "Robot Position (unrot)" << pos_w << std::endl;

        Eigen::Matrix3d Rwc;
        // transforms a point from the robo frame in the world frame
        Rwc << orientArray[0], orientArray[1], orientArray[2],
            orientArray[3], orientArray[4], orientArray[5],
            orientArray[6], orientArray[7], orientArray[8];
        //std::cout << "Robot Rwc" << Rwc << std::endl;

        // push robot position on queue
        GlobalState gs_sim;
        // switch orientation, we need orientation world frame -> robot frame
        Quaternion quat_webots_frame(Rwc.transpose());
        // rotate y around 180 degrees because the iRobots forward direction is in the wrong direction
        Quaternion quat_rot_to_forward;
        quat_rot_to_forward = Eigen::AngleAxisd(3.1415, Eigen::Vector3d::UnitY());
        quat_webots_frame = quat_rot_to_forward * quat_webots_frame;

        Quaternion quat_lpgf_frame(quat_webots_frame.w(),
            quat_webots_frame.y(), -quat_webots_frame.x(), quat_webots_frame.z());

        //std::cout << "Robot Output quaternion " << quat_lpgf_frame.w() << " " << quat_lpgf_frame.x() << " "
        //    << quat_lpgf_frame.y() << " "<< quat_lpgf_frame.z() << " "<< std::endl;
        //std::cout << "Robot Output Matrix " << quat_lpgf_frame.toRotationMatrix() << std::endl;
        gs_sim.orientation = LpSlam::Orientation(quat_lpgf_frame);
        gs_sim.position = LpSlam::Position3(pos_w);
        // velocity is in relation to the robot orientation
        gs_sim.velocity = LpSlam::Vector3(quat_lpgf_frame * velo_w);
        gs_sim.velocityValid = true;

        if (applySmearing) {
            gs_sim.position.value = gs_sim.position.value +
                LpSlam::Vector3(dis_pos_mod(gen),
                    dis_pos_mod(gen),
                    dis_pos_mod(gen));
            gs_sim.orientation.value = Quaternion(
                gs_sim.orientation.value.w() + dis_quat_mod(gen),
                gs_sim.orientation.value.x() + dis_quat_mod(gen),
                gs_sim.orientation.value.y() + dis_quat_mod(gen),
                gs_sim.orientation.value.z() + dis_quat_mod(gen)
            );
            gs_sim.orientation.value.normalize();
            gs_sim.velocity.value = gs_sim.velocity.value +
            LpSlam::Vector3(dis_vel_mod(gen),
                    dis_vel_mod(gen),
                    dis_vel_mod(gen));
        }

        SensorQueueEntry qentry_gs(absoluteTimestamp,
            base,
            gs_sim);
        params.sensorQ->push(qentry_gs);

        // get camera image
        // we do 10 Hz with the camera
        // total time is in milliseconds
        // TODO: for some reason I don't understand, the Webots simulation gets very slow
        // as soon as the camera image get read out. But its not the reading out the camera
        // image call but every simulation step gets very slow ...
        // Also seems to be not related to the camera readout rate
        if (((*params.total_time) % 50) == 0) {
            TimingBase t_meas_all_image("all_webot_image");
            //std::cout << "Storing image at time " << (*params.total_time) << std::endl;
            auto w = params.cam->getWidth();
            auto h = params.cam->getHeight();
            // image is BGRA format
            auto byteSizeImage = w * h * 4;
            //std::cout << "Image has size " << w << " x " << h << std::endl;
            const auto camSize = cv::Size(w, h);

            TimingBase t_meas_load("loading_webot_image");
            t_meas_load.report();

             {
                TimingBase t_meas("convert_webot_image");

                auto lmdGetImage = [camSize, &params](webots::Camera * cam) {
                    const unsigned char * imageData = cam->getImage();
                    cv::Mat camImageBGRA(camSize, CV_8UC4, (void*)imageData);
                    cv::Mat camGray;
                    cv::cvtColor(camImageBGRA, camGray, cv::COLOR_BGRA2GRAY);
                    return camGray;
                };

                auto camImageFirst = lmdGetImage(params.cam);
                //std::cout << "Convert Image of size " << w << " x " << h << std::endl;
                // convert to
                //cv::Mat camImage(320, 240, CV_8UC3, cv::Scalar(0, 0, 0));

                // do we have a second camera ?
                CameraQueueEntry camEntry;

                if (params.cam_second) {
                    auto camImageSecond = lmdGetImage(params.cam_second);

                    TrackerCoordinateSystemBase base;
                    camEntry = CameraQueueEntry(absoluteTimestamp,
                        0,
                        base,
                        camImageFirst,
                        1,
                        base,
                        camImageSecond
                        );

                } else {
                    TrackerCoordinateSystemBase base;
                    camEntry = CameraQueueEntry(absoluteTimestamp,
                        base,
                        camImageFirst
                        );
                }

                camEntry.globalState = gs_sim;

                params.camQ->push(camEntry);
                //std::cout << "Image pushed on queue" << std::endl;
                t_meas.report();
            }
            t_meas_all_image.report();
        }

    if (params.waitFor > 0.0f) {
      params.waitFor -= float(params.timestep) * 0.001f;
      return true;
    }

    // no movement any more
    if (params.state == RoboState::Stop) {
        return true;
    }

    if (params.state == RoboState::TurnRight) {
      params.left_motor->setVelocity(-HALF_SPEED);
      params.right_motor->setVelocity(HALF_SPEED);
      params.waitFor = 0.94 + dis_rot(gen);

        if (params.waypoints.size() == 0) {
            params.state = RoboState::Driving;
        } else {
            params.state = RoboState::DriveToWaypoint;
        }
      return true;
    }

    if (params.state == RoboState::TurnLeft) {
      params.left_motor->setVelocity(HALF_SPEED);
      params.right_motor->setVelocity(-HALF_SPEED);
      params.waitFor = 0.94 + dis_rot(gen);
        if (params.waypoints.size() == 0) {
            params.state = RoboState::Driving;
        } else {
            params.state = RoboState::DriveToWaypoint;
        }
      return true;
    }

    if (params.bumpers[BUMPER_LEFT]->getValue() != 0.0) {
        params.left_motor->setVelocity(-HALF_SPEED);
        params.right_motor->setVelocity(-HALF_SPEED);
      params.waitFor = 0.2 + dis(gen);
      params.state = RoboState::TurnRight;
      return true;
    } else if (params.bumpers[BUMPER_RIGHT]->getValue() != 0.0) {
        params.left_motor->setVelocity(-HALF_SPEED);
        params.right_motor->setVelocity(-HALF_SPEED);
      params.waitFor = 0.2 + dis(gen);
      params.state = RoboState::TurnLeft;
      return true;
    }

    // regular driving
    if (params.state == RoboState::DriveToWaypoint) {
        // check if we are at waypoint
        if (params.waypoints.size() == 0) {
            params.state = RoboState::Stop;

            params.left_motor->setVelocity(0.0);
            params.right_motor->setVelocity(0.0);

            return true;
        }

        auto thisWapyoint = params.waypoints.front();
        auto dist = NavigationHelper::computeDistanceTarget(gs_sim.position, thisWapyoint);
        std::cout << "dis " << dist << std::endl;
        if ( dist < 0.15) {
            // next waypoint !
            params.waypoints.erase(params.waypoints.begin());
            std::cout << "moving next waypoint: " << params.waypoints.front().value << std::endl;
            return true;
        }

        // compute driving direction to this waypoint
        auto direction = NavigationHelper::computeDirectionAngle(gs_sim.position, gs_sim.orientation,
            thisWapyoint);

        double minSpeed = 0.5 * MAX_SPEED;
        //double directionRatio = std::abs(direction) / boost::math::constants::pi<double>();

        double left_mod = 1.0;
        double right_mod = 1.0;

        double control = - params.pid->computeControl(0.0, direction);
        std::cout << " direction : " << direction << " control : " << control << std::endl;
        std::cout << " Robot position " << gs_sim.position.value << std::endl;
        std::cout << " Driving to waypoint " << thisWapyoint.value << std::endl;
        // only steer if we are not on target yet.
        /*if (std::abs(direction) > 0.05)*/ //{
/*            std::cout << " std::sin(direction) " << std::sin(direction) << std::endl;
            if (direction > 0) {
                right_mod = std::max(control, 0.4);
            } else {
                left_mod = std::max(control, 0.4);
            }
        }*/
        double controlRatioLeft = 1.0 - (control / 3.14);
        double controlRatioRight = 1.0 + (control / 3.14);

        std::cout << "mods " << left_mod << " " << right_mod << std::endl;
        // motor are switched
        params.left_motor->setVelocity(std::clamp( MAX_SPEED * controlRatioLeft, 0.4, MAX_SPEED));
        params.right_motor->setVelocity(std::clamp( MAX_SPEED * controlRatioRight, 0.4, MAX_SPEED));

    } else if (params.state == RoboState::Driving) {
        params.left_motor->setVelocity(MAX_SPEED);
        params.right_motor->setVelocity(MAX_SPEED);
    }

        return true;
    } ), m_pid(1.5, 0.2, 0.2) {

        getConfigOptions().optional(m_configWaypoints, std::vector<Position3>());
}

bool WebotsSource::startSensor(SensorQueue & q) {
    m_sensorQ = &q;
    return true;
}

bool WebotsSource::start(CameraQueue & q) {
    WorkerThreadParams threadParams;

    threadParams.waypoints = getConfigOptions().getPositionList(m_configWaypoints);
    std::cout << "Loaded " << threadParams.waypoints.size() << " waypoints" << std::endl;

    std::cout << "creating robot class" << std::endl;
    m_robo = std::make_unique<webots::Supervisor>();
    std::cout << " robot class created " << std::endl;

    if (threadParams.waypoints.size() > 0) {
        threadParams.state = RoboState::DriveToWaypoint;
    }

    threadParams.pid = &m_pid;
    threadParams.robo = m_robo.get();

    // its important to get the Robots's main object (which is of type Robot)
    // which is a solid type and not to use the m_robo->getSelf()
    // The DEF name ROBOT1 needs to be set for the Robot in the Webots GUI
    // also make sure that the supervisor flag is on for the robot
    std::cout << "ROBOT1 will be loaded" << std::endl;
    auto rootBot = m_robo->getFromDef("ROBOT1");
    // for this function to work we need at least Webots 2020B
    auto cameraSolid = m_robo->getFromProtoDef("ROBOT1.CREATE_CAMERA");

    if (rootBot == nullptr) {
        spdlog::error("Cannot load ROBOT1");
        return false;
    }

    if (cameraSolid == nullptr) {
        spdlog::error("Cannot load CREATE_CAMERA");
        return false;
    }

    std::cout << "ROBOT1 loaded" << std::endl;

    threadParams.robo_node = rootBot;
    threadParams.camera_node = cameraSolid;
    threadParams.trans_field = rootBot->getField("translation");
    threadParams.rot_field = rootBot->getField("rotation");

    if (m_sensorQ == nullptr) {
        spdlog::error("Sensore queue not set in Websot source");
        return false;
    }
    threadParams.camQ = &q;
    threadParams.sensorQ = m_sensorQ;
    m_total_time = 0;
    threadParams.total_time = &m_total_time;
    threadParams.initialTime = std::chrono::high_resolution_clock::now();

    spdlog::info("Starting to enable sensors and actuators on robot");
    threadParams.cam = m_robo->getCamera("camera");
    // cam has 20 Hz, we only read out the frame every 50 ms
    threadParams.cam->enable(50);
    std::cout << "CAM name " << threadParams.cam->getName() << std::endl;

    // check for second camera
    threadParams.cam_second = m_robo->getCamera("camera_second");
    if (threadParams.cam_second) {
        threadParams.cam_second->enable(50);
    }

    threadParams.accelerometer = m_robo->getAccelerometer("accelerometer");
    threadParams.accelerometer->enable(threadParams.timestep);
    threadParams.gyro = m_robo->getGyro("gyro");
    threadParams.gyro->enable(threadParams.timestep);

    for (int i = 0; i < BUMPERS_NUMBER; i++) {
        threadParams.bumpers[i] = m_robo->getTouchSensor(bumpers_name[i]);
        threadParams.bumpers[i]->enable(threadParams.timestep);
    }

    for (int i = 0; i < CLIFF_SENSORS_NUMBER; i++) {
        threadParams.cliff_sensors[i] = m_robo->getDistanceSensor(cliff_sensors_name[i]);
        threadParams.cliff_sensors[i]->enable(threadParams.timestep);
    }

    threadParams.left_motor = m_robo->getMotor("left wheel motor");
    threadParams.right_motor = m_robo->getMotor("right wheel motor");
    threadParams.left_motor->setPosition(INFINITY);
    threadParams.right_motor->setPosition(INFINITY);
    threadParams.left_motor->setVelocity(0.0);
    threadParams.right_motor->setVelocity(0.0);

    threadParams.left_position_sensor = m_robo->getPositionSensor("left wheel sensor");
    threadParams.right_position_sensor = m_robo->getPositionSensor("right wheel sensor");
    threadParams.left_position_sensor->enable(threadParams.timestep);
    threadParams.right_position_sensor->enable(threadParams.timestep);

    spdlog::info("Robot activation complete");
/*
    for (int i = 0; i < 8; i++) {
        char device_name[4];

        sprintf(device_name, "ps%d", i);
        threadParams.distance_sensor[i] = m_robo->getDistanceSensor(device_name);
        threadParams.distance_sensor[i]->enable(threadParams.timestep);
    }
*/
    m_worker.start(threadParams);
    spdlog::info("Frame aquisition with Webots started");
    return true;
}

void WebotsSource::stop() {
    m_worker.stop();

    spdlog::info("Frame aquisition with Webots stopped");
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_ENVELOPE_REJECTION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_ENVELOPE_REJECTION_HPP

#include <random>
#include <iterator>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>
#include <boost/geometry/extensions/random/strategies/cartesian/uniform_point_distribution_box.hpp>
#include <boost/geometry/extensions/random/strategies/spherical/uniform_inverse_transform_sampling.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

template
<
    typename Point,
    typename DomainGeometry,
    typename BoxStrategy = services::default_strategy
        <
            typename point_type<DomainGeometry>::type,
            model::box<typename point_type<DomainGeometry>::type>
        >
>
struct uniform_envelope_rejection
{
private:
	typedef model::box<typename point_type<DomainGeometry>::type> envelope_type;
	envelope_type m_env;
	BoxStrategy m_env_strat;
public:
    uniform_envelope_rejection(DomainGeometry const& g) :
		m_env(return_envelope<envelope_type>(g)),
		m_env_strat(m_env) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_envelope_rejection const& r_strategy) const
    {
        return boost::geometry::equals(l_domain, r_domain);
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        typedef typename point_type<DomainGeometry>::type domain_point;
        domain_point p;
        do {
            p = m_env_strat.apply(g, m_env);
        }while( !::boost::geometry::within(p, d) );
        Point r;
        boost::geometry::transform(p, r);
        return r;
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry,
    typename MultiOrSingle,
    typename CsTag
>
struct default_strategy
<
    Point,
    DomainGeometry,
    areal_tag,
    MultiOrSingle,
    2,
    CsTag
> : public uniform_envelope_rejection<Point, DomainGeometry> {
    typedef uniform_envelope_rejection<Point, DomainGeometry> base;
    using base::base;
};

template
<
    typename Point,
    typename DomainGeometry,
    typename MultiOrSingle,
    typename CsTag
>
struct default_strategy
<
    Point,
    DomainGeometry,
    volumetric_tag,
    MultiOrSingle,
    3,
    CsTag
> : public uniform_envelope_rejection<Point, DomainGeometry> {
    typedef uniform_envelope_rejection<Point, DomainGeometry> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif //  BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_ENVELOPE_REJECTION_HPP


/**
*   PerimeterOperation.hpp
*
*   Copyright 2015 The Climate Corporation
*   Copyright 2015 Tony Stone
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*   http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*
*   Created by Tony Stone on 6/11/15.
*
*   MODIFIED 2015 BY Tony Stone. Modifications licensed under Apache License, Version 2.0.
*
*/

#ifndef __PerimeterOperation_HPP_
#define __PerimeterOperation_HPP_

#include <boost/variant/variant.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>

#include "GeometryVariant.hpp"

namespace geofeatures {
    namespace operators {

        class PerimeterOperation : public  boost::static_visitor<double> {

        public:
            template <typename T>
            double operator()(const T * v) const {
                return boost::geometry::perimeter(*v);
            }

            double operator()(const GeometryCollection<> * v) const {
                return 0.0;
            }
        };

    }   // namespace operators
}   // namespace geofeatures

#endif //__PerimeterOperation_HPP_


#include <boost/bind.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/utility/in_place_factory.hpp>

#include <eigen_conversions/eigen_msg.h>
#include <message_filters/subscriber.h>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <tf/message_filter.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <pluginlib/class_list_macros.hpp>

#include <mil_msgs/DepthStamped.h>
#include <mil_msgs/VelocityMeasurements.h>

#include <odom_estimator/Info.h>
#include <odom_estimator/SetIgnoreMagnetometer.h>

#include "odom_estimator/earth.h"
#include "odom_estimator/kalman.h"
#include "odom_estimator/magnetic.h"
#include "odom_estimator/odometry.h"
#include "odom_estimator/state.h"
#include "odom_estimator/unscented_transform.h"
#include "odom_estimator/util.h"

namespace odom_estimator
{
static magnetic::MagneticModel const magnetic_model(ros::package::getPath("odom_estimator") + "/data/WMM.COF");

GaussianDistribution<State> init_state(sensor_msgs::Imu const &msg, Vec<3> last_mag, Vec<3> pos_ecef, Vec<3> vel_ecef,
                                       Vec<3> rel_pos_ecef)
{
  Vec<3> pos_eci = inertial_from_ecef(msg.header.stamp.toSec(), pos_ecef);
  Vec<3> vel_eci = inertial_vel_from_ecef_vel(msg.header.stamp.toSec(), vel_ecef, pos_eci);

  Vec<3> mag_eci = magnetic_model.getField(pos_eci, msg.header.stamp.toSec());
  Vec<3> predicted_acc_eci = inertial_acc_from_ecef_acc(msg.header.stamp.toSec(), Vec<3>::Zero(), pos_eci);
  Vec<3> predicted_accelerometer_eci = predicted_acc_eci - gravity::gravity(pos_eci);
  Vec<3> accel_body = xyz2vec(msg.linear_acceleration);
  Quaternion orient_eci = triad(predicted_accelerometer_eci, mag_eci, accel_body, last_mag);

  Vec<State::RowsAtCompileTime> stdev = (Vec<State::RowsAtCompileTime>(18) << 100, 100, 100, 100, 100, 100, .05, .05,
                                         .05, 10, 10, 10, 1e-3, 1e-3, 1e-3, 1e-2, 1e-2, 1e-2)
                                            .finished();
  SqMat<State::RowsAtCompileTime> tmp = stdev.asDiagonal();

  return GaussianDistribution<State>(State(msg.header.stamp, msg.header.stamp, pos_eci,
                                           inertial_from_ecef(msg.header.stamp.toSec(), rel_pos_ecef), orient_eci,
                                           vel_eci, Vec<3>::Zero(), Vec<3>::Zero()),
                                     tmp * tmp);
}

class NodeImpl
{
private:
  boost::function<const std::string &()> getName;
  ros::NodeHandle &nh;
  ros::NodeHandle &private_nh;
  double start_x_ecef, start_y_ecef, start_z_ecef;
  std::string local_frame;
  ros::ServiceServer set_ignore_magnetometer_srv;
  bool ignoreMagnetometer;
  Vec<3> last_rel_pos_ecef_;

public:
  NodeImpl(boost::function<const std::string &()> getName, ros::NodeHandle *nh_, ros::NodeHandle *private_nh_)
    : getName(getName)
    , nh(*nh_)
    , private_nh(*private_nh_)
    , local_frame("/enu")
    , ignoreMagnetometer(false)
    , mag_sub(nh, "imu/mag", 1)
    , mag_filter(mag_sub, tf_listener, "", 10)
    , dvl_sub(nh, "dvl", 1)
    , dvl_filter(dvl_sub, tf_listener, "", 10)
    , depth_sub(nh, "depth", 1)
    , depth_filter(depth_sub, tf_listener, "", 10)
    , last_mag(boost::none)
    , last_good_dvl(boost::none)
    , state(boost::none)
  {
    private_nh.getParam("start_x_ecef", start_x_ecef);
    private_nh.getParam("start_y_ecef", start_y_ecef);
    private_nh.getParam("start_z_ecef", start_z_ecef);
    private_nh.getParam("local_frame", local_frame);

    imu_sub = nh.subscribe<sensor_msgs::Imu>("imu/data_raw", 10, boost::bind(&NodeImpl::got_imu, this, _1));
    mag_filter.registerCallback(boost::bind(&NodeImpl::got_mag, this, _1));
    dvl_filter.registerCallback(boost::bind(&NodeImpl::got_dvl, this, _1));
    depth_filter.registerCallback(boost::bind(&NodeImpl::got_depth, this, _1));
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
    absodom_pub = nh.advertise<nav_msgs::Odometry>("absodom", 10);
    info_pub = private_nh.advertise<odom_estimator::Info>("info", 10);
    set_ignore_magnetometer_srv =
        private_nh.advertiseService("set_ignore_magnetometer", &NodeImpl::setIgnoreMagnetometer, this);

    last_rel_pos_ecef_ = Vec<3>::Zero();
  }

private:
  void got_imu(const sensor_msgs::ImuConstPtr &msgp)
  {
    // const sensor_msgs::Imu &msg = *msgp;
    sensor_msgs::Imu msg = *msgp;
    Eigen::Map<SqMat<3>>(msg.angular_velocity_covariance.data()) = pow(0.02, 2) * SqMat<3>::Identity();
    Eigen::Map<SqMat<3>>(msg.linear_acceleration_covariance.data()) = pow(0.06, 2) * SqMat<3>::Identity();

    mag_filter.setTargetFrame(msg.header.frame_id);
    dvl_filter.setTargetFrame(msg.header.frame_id);
    depth_filter.setTargetFrame(msg.header.frame_id);
    last_gyro = xyz2vec(msg.angular_velocity);
    local_frame_id = msg.header.frame_id;

    if (state && (msg.header.stamp < state->mean.t || msg.header.stamp > state->mean.t + ros::Duration(2)))
    {
      NODELET_ERROR("reset due to invalid stamp");
      last_mag = boost::none;
      state = boost::none;
    }

    if (!state)
    {
      if (!last_mag)
      {
        std::cout << "mag missing" << std::endl;
        return;
      }
      if (last_good_dvl && *last_good_dvl > msg.header.stamp - ros::Duration(1.5) &&
          *last_good_dvl < msg.header.stamp + ros::Duration(1.5))
      {
        state = init_state(msg, *last_mag, Vec<3>(start_x_ecef, start_y_ecef, start_z_ecef), Vec<3>::Zero(),
                           last_rel_pos_ecef_);
      }
      else
      {
        std::cout << "dvl missing" << std::endl;
        return;
      }
    }
    else
    {
      state = StateUpdater(msg)(*state);
    }

    GaussianDistribution<Vec<3>> gyro_bias_dist = EasyDistributionFunction<State, Vec<3>>(
        [](State const &state, Vec<0> const &) { return state.gyro_bias; })(*state);
    GaussianDistribution<Vec<3>> accel_bias_dist = EasyDistributionFunction<State, Vec<3>>(
        [](State const &state, Vec<0> const &) { return state.accel_bias; })(*state);

    if (state->mean.gyro_bias.norm() > .5)
    {
      NODELET_ERROR("reset due to bad gyro biases");
      last_mag = boost::none;
      state = boost::none;
      return;
    }

    last_rel_pos_ecef_ = state->mean.getRelPosECEF();

    {
      EasyDistributionFunction<State, Odom> transformer([this, &msg](State const &state, Vec<0> const &) {
        SqMat<3> m = enu_from_ecef_mat(state.getPosECEF());
        return Odom(state.t, local_frame, msg.header.frame_id, m * state.getRelPosECEF(),
                    Quaternion(m) * state.getOrientECEF(),
                    state.getOrientECEF().conjugate()._transformVector(state.getVelECEF()),
                    xyz2vec(msg.angular_velocity) - state.gyro_bias);
      });

      odom_pub.publish(msg_from_odom(transformer(*state)));
    }

    {
      EasyDistributionFunction<State, Odom> transformer([&msg](State const &state, Vec<0> const &) {
        return Odom(state.t, "/ecef", msg.header.frame_id, state.getPosECEF(), state.getOrientECEF(),
                    state.getOrientECEF().conjugate()._transformVector(state.getVelECEF()),
                    xyz2vec(msg.angular_velocity) - state.gyro_bias);
      });

      absodom_pub.publish(msg_from_odom(transformer(*state)));
    }

    {
      odom_estimator::Info output;
      output.header.stamp = msg.header.stamp;

      tf::vectorEigenToMsg(gyro_bias_dist.mean, output.gyro_bias);
      tf::vectorEigenToMsg(gyro_bias_dist.cov.diagonal().array().sqrt().eval(), output.gyro_bias_stddev);

      tf::vectorEigenToMsg(accel_bias_dist.mean, output.accel_bias);
      tf::vectorEigenToMsg(accel_bias_dist.cov.diagonal().array().sqrt().eval(), output.accel_bias_stddev);

      info_pub.publish(output);
    }
  }

  void got_mag(const sensor_msgs::MagneticFieldConstPtr &msgp)
  {
    const sensor_msgs::MagneticField &msg = *msgp;

    tf::StampedTransform transform;
    try
    {
      tf_listener.lookupTransform(local_frame_id, msg.header.frame_id, msg.header.stamp, transform);
    }
    catch (tf::TransformException ex)
    {
      NODELET_ERROR("Error in got_mag: %s", ex.what());
      return;
    }
    Quaternion local_mag_orientation;
    tf::quaternionTFToEigen(transform.getRotation(), local_mag_orientation);

    last_mag = xyz2vec(msg.magnetic_field);

    if (!state)
      return;
    if (ignoreMagnetometer)
      return;

    SqMat<3> cov = Eigen::Map<const SqMat<3>>(msg.magnetic_field_covariance.data());
    if (cov == SqMat<3>::Zero())
    {
      Vec<3> stddev(2e-7, 2e-7, 2e-7);
      stddev *= 100;
      cov = stddev.cwiseProduct(stddev).asDiagonal();
    }

    Vec<3> mag_eci = magnetic_model.getField(state->mean.pos_eci, state->mean.t.toSec());
    state = kalman_update(
        EasyDistributionFunction<State, Vec<1>, Vec<3>>(
            [&msg, &mag_eci, &local_mag_orientation, this](State const &state, Vec<3> const &measurement_noise) {
              SqMat<3> enu_from_ecef = enu_from_ecef_mat(state.getPosECEF());
              Vec<3> predicted = state.orient.conjugate()._transformVector(mag_eci) +
                                 local_mag_orientation._transformVector(measurement_noise);
              Vec<3> predicted_enu = enu_from_ecef * state.getOrientECEF()._transformVector(predicted);
              double predicted_angle = atan2(predicted_enu(1), predicted_enu(0));
              Vec<3> measured_enu = enu_from_ecef *
                                    state.getOrientECEF()._transformVector(
                                        local_mag_orientation._transformVector(xyz2vec(msg.magnetic_field)));
              double measured_angle = atan2(measured_enu(1), measured_enu(0));
              double error_angle = measured_angle - predicted_angle;
              double pi = boost::math::constants::pi<double>();
              while (error_angle < -pi)
                error_angle += 2 * pi;
              while (error_angle > pi)
                error_angle -= 2 * pi;
              return scalar_matrix(error_angle);
            },
            GaussianDistribution<Vec<3>>(Vec<3>::Zero(), cov)),
        *state);
  }

  void got_dvl(const mil_msgs::VelocityMeasurementsConstPtr &msgp)
  {
    mil_msgs::VelocityMeasurements const &msg = *msgp;

    tf::StampedTransform transform;
    try
    {
      tf_listener.lookupTransform(local_frame_id, msg.header.frame_id, msg.header.stamp, transform);
    }
    catch (tf::TransformException ex)
    {
      NODELET_ERROR("Error in got_dvl: %s", ex.what());
      return;
    }
    Vec<3> local_dvl_pos;
    tf::vectorTFToEigen(transform.getOrigin(), local_dvl_pos);
    Quaternion local_dvl_orientation;
    tf::quaternionTFToEigen(transform.getRotation(), local_dvl_orientation);

    std::vector<mil_msgs::VelocityMeasurement> good;
    for (unsigned int i = 0; i < msg.velocity_measurements.size(); i++)
    {
      mil_msgs::VelocityMeasurement const &vm = msg.velocity_measurements[i];
      if (!std::isnan(vm.velocity))
      {
        good.push_back(vm);
      }
    }

    if (good.size() >= 3)
    {
      last_good_dvl = msg.header.stamp;
      // std::cout << "got dvl" << std::endl;
    }
    else
    {
      std::cout << "bad dvl" << std::endl;
    }

    if (!state)
      return;

    state = kalman_update(
        EasyDistributionFunction<State, Vec<Dynamic>, Vec<Dynamic>>(
            [&good, &local_dvl_pos, &local_dvl_orientation, this](State const &state,
                                                                  Vec<Dynamic> const &measurement_noise) {
              Vec<3> dvl_vel = local_dvl_orientation.inverse()._transformVector(
                  state.getOrientECEF().inverse()._transformVector(state.getVelECEF(local_dvl_pos, *last_gyro)));

              Vec<Dynamic> res(good.size());
              for (unsigned int i = 0; i < good.size(); i++)
              {
                mil_msgs::VelocityMeasurement const &vm = good[i];
                res(i) = (xyz2vec(vm.direction).dot(dvl_vel) + measurement_noise(i)) - vm.velocity;
              }
              return res;
            },
            GaussianDistribution<Vec<Dynamic>>(Vec<Dynamic>::Zero(good.size()),
                                               pow(.05, 2) * Vec<Dynamic>::Ones(good.size()).asDiagonal())),
        *state);
  }

  void got_depth(const mil_msgs::DepthStampedConstPtr &msgp)
  {
    mil_msgs::DepthStamped const &msg = *msgp;

    tf::StampedTransform transform;
    try
    {
      tf_listener.lookupTransform(local_frame_id, msg.header.frame_id, msg.header.stamp, transform);
    }
    catch (tf::TransformException ex)
    {
      NODELET_ERROR("Error in got_depth: %s", ex.what());
      return;
    }
    Vec<3> local_depth_pos;
    tf::vectorTFToEigen(transform.getOrigin(), local_depth_pos);

    if (!state)
      return;

    state = kalman_update(EasyDistributionFunction<State, Vec<1>, Vec<1>>(
                              [&](State const &state, Vec<1> const &measurement_noise) {
                                SqMat<3> m = enu_from_ecef_mat(state.getPosECEF());
                                double estimated =
                                    -(m * state.getRelPosECEF(local_depth_pos))(2) + measurement_noise(0);
                                return scalar_matrix(estimated - msg.depth);
                              },
                              GaussianDistribution<Vec<1>>(Vec<1>::Zero(), pow(.1, 2) * Vec<1>::Ones().asDiagonal())),
                          *state);
  }

  bool setIgnoreMagnetometer(SetIgnoreMagnetometer::Request &request, SetIgnoreMagnetometer::Response &response)
  {
    ignoreMagnetometer = request.ignore;
    return true;
  }

  tf::TransformListener tf_listener;
  ros::Subscriber imu_sub;
  message_filters::Subscriber<sensor_msgs::MagneticField> mag_sub;
  tf::MessageFilter<sensor_msgs::MagneticField> mag_filter;
  message_filters::Subscriber<mil_msgs::VelocityMeasurements> dvl_sub;
  tf::MessageFilter<mil_msgs::VelocityMeasurements> dvl_filter;
  message_filters::Subscriber<mil_msgs::DepthStamped> depth_sub;
  tf::MessageFilter<mil_msgs::DepthStamped> depth_filter;
  ros::Publisher odom_pub;
  ros::Publisher absodom_pub;
  ros::Publisher info_pub;

  boost::optional<Vec<3>> last_mag;
  boost::optional<ros::Time> last_good_dvl;
  boost::optional<GaussianDistribution<State>> state;
  boost::optional<Vec<3>> last_gyro;
  std::string local_frame_id;
};

class Nodelet : public nodelet::Nodelet
{
public:
  Nodelet()
  {
  }

  virtual void onInit()
  {
    nodeimpl = boost::in_place(boost::bind(&Nodelet::getName, this), &getNodeHandle(), &getPrivateNodeHandle());
  }

private:
  boost::optional<NodeImpl> nodeimpl;
};
PLUGINLIB_EXPORT_CLASS(odom_estimator::Nodelet, nodelet::Nodelet);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017-2020.
// Modifications copyright (c) 2017-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_COMPARE_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_COMPARE_HPP

#include <type_traits>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/compare.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace compare
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t I, typename P>
static inline typename geometry::coordinate_type<P>::type
get(P const& p, std::true_type /*same units*/)
{
    return geometry::get<I>(p);
}

template <std::size_t I, typename P>
static inline typename geometry::coordinate_type<P>::type
get(P const& p, std::false_type /*different units*/)
{
    return geometry::get_as_radian<I>(p);
}

template
<
    typename ComparePolicy,
    typename Point1,
    typename Point2,
    std::size_t DimensionCount
>
struct spherical_latitude
{
    typedef typename geometry::coordinate_type<Point1>::type coordinate1_type;
    typedef typename geometry::detail::cs_angular_units<Point1>::type units1_type;
    typedef typename geometry::coordinate_type<Point2>::type coordinate2_type;
    typedef typename geometry::detail::cs_angular_units<Point2>::type units2_type;
    typedef std::is_same<units1_type, units2_type> same_units_type;

    template <typename T1, typename T2>
    static inline bool apply(Point1 const& left, Point2 const& right,
                             T1 const& l1, T2 const& r1)
    {
        // latitudes equal
        if (math::equals(l1, r1))
        {
            return compare::detail::compare_loop
                <
                    ComparePolicy, 2, DimensionCount
                >::apply(left, right);
        }
        else
        {
            return ComparePolicy::apply(l1, r1);
        }
    }

    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        coordinate1_type const& l1 = compare::detail::get<1>(left, same_units_type());
        coordinate2_type const& r1 = compare::detail::get<1>(right, same_units_type());

        return apply(left, right, l1, r1);
    }
};

template
<
    typename ComparePolicy,
    typename Point1,
    typename Point2
>
struct spherical_latitude<ComparePolicy, Point1, Point2, 1>
{
    template <typename T1, typename T2>
    static inline bool apply(Point1 const& left, Point2 const& right,
                             T1 const& , T2 const& )
    {
        return apply(left, right);
    }

    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        return compare::detail::compare_loop
            <
                ComparePolicy, 1, 1
            >::apply(left, right);
    }
};

template
<
    typename ComparePolicy,
    typename Point1,
    typename Point2,
    std::size_t DimensionCount
>
struct spherical_longitude
{
    typedef typename geometry::coordinate_type<Point1>::type coordinate1_type;
    typedef typename geometry::detail::cs_angular_units<Point1>::type units1_type;
    typedef typename geometry::coordinate_type<Point2>::type coordinate2_type;
    typedef typename geometry::detail::cs_angular_units<Point2>::type units2_type;
    typedef std::is_same<units1_type, units2_type> same_units_type;
    typedef std::conditional_t<same_units_type::value, units1_type, geometry::radian> units_type;

    static const bool is_equatorial = ! std::is_same
                                        <
                                            typename geometry::cs_tag<Point1>::type,
                                            geometry::spherical_polar_tag
                                        >::value;

    static inline bool are_both_at_antimeridian(coordinate1_type const& l0,
                                                coordinate2_type const& r0,
                                                bool & is_left_at,
                                                bool & is_right_at)
    {
        is_left_at = math::is_longitude_antimeridian<units_type>(l0);
        is_right_at = math::is_longitude_antimeridian<units_type>(r0);
        return is_left_at && is_right_at;
    }

    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        // if units are different the coordinates are in radians
        coordinate1_type const& l0 = compare::detail::get<0>(left, same_units_type());
        coordinate2_type const& r0 = compare::detail::get<0>(right, same_units_type());
        coordinate1_type const& l1 = compare::detail::get<1>(left, same_units_type());
        coordinate2_type const& r1 = compare::detail::get<1>(right, same_units_type());

        bool is_left_at_antimeridian = false;
        bool is_right_at_antimeridian = false;

            // longitudes equal
        if (math::equals(l0, r0)
               // both at antimeridian
            || are_both_at_antimeridian(l0, r0, is_left_at_antimeridian, is_right_at_antimeridian)
               // both at pole
            || (math::equals(l1, r1)
                && math::is_latitude_pole<units_type, is_equatorial>(l1)))
        {
            return spherical_latitude
                <
                    ComparePolicy, Point1, Point2, DimensionCount
                >::apply(left, right, l1, r1);
        }
        // if left is at antimeridian and right is not at antimeridian
        // then left is greater than right
        else if (is_left_at_antimeridian)
        {
            // less/equal_to -> false, greater -> true
            return ComparePolicy::apply(1, 0);
        }
        // if right is at antimeridian and left is not at antimeridian
        // then left is lesser than right
        else if (is_right_at_antimeridian)
        {
            // less -> true, equal_to/greater -> false
            return ComparePolicy::apply(0, 1);
        }
        else
        {
            return ComparePolicy::apply(l0, r0);
        }
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Compare strategy for spherical coordinates
\ingroup strategies
\tparam Point point-type
\tparam Dimension dimension
*/
template
<
    typename ComparePolicy,
    int Dimension = -1
>
struct spherical
    : cartesian<ComparePolicy, Dimension>
{};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
// all dimensions starting from longitude
template <typename ComparePolicy>
struct spherical<ComparePolicy, -1>
{
    template <typename Point1, typename Point2>
    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        return compare::detail::spherical_longitude
            <
                ComparePolicy,
                Point1,
                Point2,
                std::conditional_t
                    <
                        (dimension<Point1>::value < dimension<Point2>::value),
                        std::integral_constant<std::size_t, dimension<Point1>::value>,
                        std::integral_constant<std::size_t, dimension<Point2>::value>
                    >::value
            >::apply(left, right);
    }
};

// only longitudes (and latitudes to check poles)
template <typename ComparePolicy>
struct spherical<ComparePolicy, 0>
{
    template <typename Point1, typename Point2>
    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        return compare::detail::spherical_longitude
            <
                ComparePolicy, Point1, Point2, 1
            >::apply(left, right);
    }
};

// only latitudes
template <typename ComparePolicy>
struct spherical<ComparePolicy, 1>
{
    template <typename Point1, typename Point2>
    static inline bool apply(Point1 const& left, Point2 const& right)
    {
        return compare::detail::spherical_latitude
            <
                ComparePolicy, Point1, Point2, 2
            >::apply(left, right);
    }
};

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename ComparePolicy, typename Point1, typename Point2, int Dimension>
struct default_strategy
    <
        ComparePolicy, Point1, Point2, Dimension,
        spherical_tag, spherical_tag
    >
{
    typedef compare::spherical<ComparePolicy, Dimension> type;
};

template <typename ComparePolicy, typename Point1, typename Point2, int Dimension>
struct default_strategy
    <
        ComparePolicy, Point1, Point2, Dimension,
        spherical_polar_tag, spherical_polar_tag
    >
{
    typedef compare::spherical<ComparePolicy, Dimension> type;
};

template <typename ComparePolicy, typename Point1, typename Point2, int Dimension>
struct default_strategy
    <
        ComparePolicy, Point1, Point2, Dimension,
        spherical_equatorial_tag, spherical_equatorial_tag
    >
{
    typedef compare::spherical<ComparePolicy, Dimension> type;
};

template <typename ComparePolicy, typename Point1, typename Point2, int Dimension>
struct default_strategy
    <
        ComparePolicy, Point1, Point2, Dimension,
        geographic_tag, geographic_tag
    >
{
    typedef compare::spherical<ComparePolicy, Dimension> type;
};

} // namespace services

}} // namespace strategy::compare

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_COMPARE_HPP


#include "ocean_model_interfaces/fvcom/FVCOMStructure.h"
#include "ocean_model_interfaces/fvcom/FVCOM.h"

#include <netcdf>
#include <memory>
#include <cmath>
#include <limits>
#include <algorithm>
#include <iterator>

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

using namespace ocean_model_interfaces;

FVCOMStructure::FVCOMStructure() {}

FVCOMStructure::FVCOMStructure(const std::string filename, int xChunkSize, int yChunkSize, int siglayChunkSize, int timeChunkSize) :
    xChunkSize(xChunkSize),
    yChunkSize(yChunkSize),
    siglayChunkSize(siglayChunkSize),
    timeChunkSize(timeChunkSize),
    lastContainingTriangle(0)
{
    loadStructureData(filename);

    splitIntoChunks();
}

FVCOMStructure::Plane::Plane() {}

FVCOMStructure::Plane::Plane(FVCOMStructure::Point& p0, FVCOMStructure::Point& p1, FVCOMStructure::Point& p2)
{
    double ab[3];
    double ac[3];

    ab[0] = p1.x - p0.x;
    ab[1] = p1.y - p0.y;
    ab[2] = p1.z - p0.z;

    ac[0] = p2.x - p0.x;
    ac[1] = p2.y - p0.y;
    ac[2] = p2.z - p0.z;

    a = (ab[1] * ac[2]) - (ab[2] * ac[1]);
    b = (ab[2] * ac[0]) - (ab[0] * ac[2]);
    c = (ab[0] * ac[1]) - (ab[1] * ac[0]);

    double magnitude = sqrt(a * a +  b * b +  c * c);
    a /= magnitude;
    b /= magnitude;
    c /= magnitude;

    d = -(p0.x * a + p0.y * b + p0.z * c);
}

double FVCOMStructure::Plane::getHeight(Point& interpolatePoint)
{
    return (-d - a * interpolatePoint.x - b * interpolatePoint.y) / c;
}

std::vector<std::string> FVCOMStructure::traverseDataFiles(const std::string filename)
{
    std::vector<std::string> filenames;
    fs::path p1 = filename;
    if(fs::is_directory(p1))
    {
        for(auto& p: fs::directory_iterator(p1))
        {
            if(!fs::is_directory(p))
            {
                filenames.push_back(p.path().string());
            }
        }
    }
    else
    {
        filenames.push_back(filename);
    }
    return filenames;
}

void FVCOMStructure::loadStructureData(const std::string directory)
{
    unsigned int timeDim = 0;
    std::vector<std::string> filenames = traverseDataFiles(directory);

    //Set start times and time dimensions from files
    for(auto &filename : filenames)
    {
        netCDF::NcFile dataFile(filename, netCDF::NcFile::read);
        timeDim += dataFile.getDim("time").getSize();

        std::vector<float> tempTimes;
        tempTimes.resize(dataFile.getDim("time").getSize());
        netCDF::NcVar timeVar = dataFile.getVar("time");
        timeVar.getVar(tempTimes.data());
        ModelFile modelFile;
        modelFile.filename = filename;
        modelFile.startTime = tempTimes[0];
        modelFile.timeDim = dataFile.getDim("time").getSize();
        modelFiles.push_back(modelFile);
    }

    //sort filenames based on start ties
    std::sort(modelFiles.begin(), modelFiles.end());

    //load time variables into one vector
    times.resize(timeDim);
    unsigned int currentIndex = 0;
    for(auto &modelFile : modelFiles)
    {
        netCDF::NcFile dataFile(modelFile.filename, netCDF::NcFile::read);
        netCDF::NcVar timeVar = dataFile.getVar("time");

        //Set the start time index for this file
        modelFile.startTimeIndex = currentIndex;

        //Load times from this file
        timeVar.getVar(times.data() + currentIndex);

        //move current index for next files
        currentIndex += dataFile.getDim("time").getSize();
    }
    netCDF::NcFile dataFile(modelFiles[0].filename, netCDF::NcFile::read);

    //Get dimensions of structure elements
    unsigned int nodeDim = dataFile.getDim("node").getSize();
    unsigned int neleDim = dataFile.getDim("nele").getSize();
    siglayDim = dataFile.getDim("siglay").getSize();

    //Load all variables for the structure of the model
    netCDF::NcVar xVar = dataFile.getVar("x");
    netCDF::NcVar yVar = dataFile.getVar("y");
    netCDF::NcVar xcVar = dataFile.getVar("xc");
    netCDF::NcVar ycVar = dataFile.getVar("yc");
    netCDF::NcVar nvVar = dataFile.getVar("nv");
    netCDF::NcVar hVar = dataFile.getVar("h");
    netCDF::NcVar siglayVar = dataFile.getVar("siglay");
    netCDF::NcVar centerSiglayVar = dataFile.getVar("siglay_center");

    //Load in as floats as that is what they are in the files
    //but they will be converted to doubles to maintain precision during calculations
    std::vector<float> nodeX;
    std::vector<float> nodeY;
    std::vector<float> nodeH;

    std::vector<float> triangleX;
    std::vector<float> triangleY;

    nodeX.resize(nodeDim);
    nodeY.resize(nodeDim);
    triangleX.resize(neleDim);
    triangleY.resize(neleDim);
    nodeH.resize(nodeDim);

    //Resize siglay 2d vectors
    nodeSiglay.resize(nodeDim);

    for(unsigned int i = 0; i < nodeSiglay.size(); i++)
    {
        nodeSiglay[i].resize(siglayDim);
    }

    //resize for multidimensional array
    triangleToNodes.resize(neleDim);

    for(unsigned int i = 0; i < neleDim; i++)
    {
        triangleToNodes[i].resize(3);
    }

    //Assign all arrays for the structure variables
    xVar.getVar(nodeX.data());
    yVar.getVar(nodeY.data());
    xcVar.getVar(triangleX.data());
    ycVar.getVar(triangleY.data());
    hVar.getVar(nodeH.data());

    //load nvVar into a multidimensional vector
    for(unsigned int i = 0; i < neleDim; i++)
    {
        std::vector<size_t> start = {0, i};
        std::vector<size_t> count = {3, 1};
        nvVar.getVar(start, count, triangleToNodes[i].data());
    }

    //load node siglay into a multidimensional vector
    for(unsigned int i = 0; i < nodeSiglay.size(); i++)
    {
        std::vector<size_t> start = {0, i};
        std::vector<size_t> count = {siglayDim, 1};
        siglayVar.getVar(start, count, nodeSiglay[i].data());
    }

    //Convert to use point struct
    nodes.resize(nodeDim);
    triangles.resize(neleDim);
    for(unsigned int i = 0; i < nodeDim; i++)
    {
        nodes[i].x = nodeX[i];
        nodes[i].y = nodeY[i];
        nodes[i].z = nodeH[i];
    }

    for(unsigned int i = 0; i < neleDim; i++)
    {
        triangles[i].x = triangleX[i];
        triangles[i].y = triangleY[i];
        triangles[i].z = std::numeric_limits<double>::quiet_NaN();
        //Height data is not loaded for triangles so we set it to NaN.
    }

    //The nv variable from the netCDF indexes starting at 1
    //Convert this to 0 by subtracting 1 from every value
    for(unsigned int i = 0; i < neleDim; i++)
    {
        for(unsigned int j = 0; j < 3; j++)
        {
            triangleToNodes[i][j]--;
        }
    }

    //Pre Processes model to get node to triangle conversion
    nodeToTriangles.resize(nodeDim);

    for(unsigned int i = 0; i < neleDim; i++)
    {
        for(unsigned int j = 0; j < 3; j++)
        {
            int triangle = i;
            int node = triangleToNodes[i][j];

            nodeToTriangles[node].push_back(triangle);
        }
    }

}

void FVCOMStructure::splitIntoChunks()
{
    getModelExtent();

    siglayDimChunks = std::ceil(siglayDim / (double)siglayChunkSize);
    timeDimChunks = std::ceil(times.size() / (double)timeChunkSize);
    yDimChunks = std::ceil((maxY - minY) / (double)yChunkSize);
    xDimChunks = std::ceil((maxX - minX) / (double)xChunkSize);

    nodesInChunk.resize(xDimChunks * yDimChunks);
    trianglesInChunk.resize(xDimChunks * yDimChunks);
    for(unsigned int i = 0; i < nodes.size(); i++)
    {
        FVCOMStructure::ChunkInfo chunk = getChunkForNode(i, 0, 0);

        //This does not contain the siglay or time dimensions as the
        //node locations do not change with depth or time.
        int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

        nodesInChunk[chunkId].push_back(i);
    }

    for(unsigned int i = 0; i < triangles.size(); i++)
    {
        FVCOMStructure::ChunkInfo chunk = getChunkForTriangle(i, 0, 0);

        //This does not contain the siglay or time dimensions as the
        //node locations do not change with depth or time.
        int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

        trianglesInChunk[chunkId].push_back(i);
    }
}

void FVCOMStructure::getModelExtent()
{
    for(unsigned int i = 0; i < nodes.size(); i++)
    {
        Point& node = nodes[i];

        if(node.x > maxX)
        {
            maxX = node.x;
        }

        if(node.x < minX)
        {
            minX = node.x;
        }

        if(node.y > maxY)
        {
            maxY = node.y;
        }

        if(node.y < minY)
        {
            minY = node.y;
        }
    }
}

bool FVCOMStructure::pointInTriangle(Point testPoint, int triangle) const
{
    int p0Index = triangleToNodes[triangle][0];
    int p1Index = triangleToNodes[triangle][1];
    int p2Index = triangleToNodes[triangle][2];

    Point p0 = nodes[p0Index];
    Point p1 = nodes[p1Index];
    Point p2 = nodes[p2Index];

    //Calculate barycentric coordinates
    double alpha = ((p1.y - p2.y)*(testPoint.x - p2.x) + (p2.x - p1.x)*(testPoint.y - p2.y)) /
        ((p1.y - p2.y)*(p0.x - p2.x) + (p2.x - p1.x)*(p0.y - p2.y));

    double beta = ((p2.y - p0.y)*(testPoint.x - p2.x) + (p0.x - p2.x)*(testPoint.y - p2.y)) /
           ((p1.y - p2.y)*(p0.x - p2.x) + (p2.x - p1.x)*(p0.y - p2.y));

    double gamma = 1.0 - alpha - beta;

    //if all coordinates are none negative then the point is in the triangle
    return alpha >= 0 && beta >= 0 && gamma >= 0;
}

int FVCOMStructure::getContainingTriangle(Point testPoint, int closestNode)
{
    if(pointInTriangle(testPoint, lastContainingTriangle))
    {
        return lastContainingTriangle;
    }

    //Search all triangles that are connected to the closest node
    for(unsigned int i = 0; i < nodeToTriangles[closestNode].size(); i++)
    {
        //return the triangle for which the point is inside
        if(pointInTriangle(testPoint, nodeToTriangles[closestNode][i]))
        {
            lastContainingTriangle = nodeToTriangles[closestNode][i];
            return nodeToTriangles[closestNode][i];
        }
    }

    //if the point is not inside any of those triangles search all the triangles
    for(unsigned int i = 0; i < triangles.size(); i++)
    {
        if(pointInTriangle(testPoint, i))
        {
            lastContainingTriangle = i;
            return i;
        }
    }

    throw std::out_of_range("FVCOM request outside of model extent");
}

int FVCOMStructure::getContainingTriangle(Point testPoint)
{
    if(pointInTriangle(testPoint, lastContainingTriangle))
    {
        return lastContainingTriangle;
    }

    //Get the closest node to start the search for the containing triangle
    int closestNode = getClosestNode(testPoint);

    return getContainingTriangle(testPoint, closestNode);
}

const std::vector<int>& FVCOMStructure::getNodesInTriangle(int triangle) const
{
    return triangleToNodes[triangle];
}

const std::vector<FVCOMStructure::ModelFile> FVCOMStructure::getModelFiles() const
{
    return modelFiles;
}

int FVCOMStructure::getClosestNode(Point testPoint) const
{
    //Checks distance between testPoint and every node, this is slow and will probably need to be improved
    double closestDistance = std::numeric_limits<double>::max();
    int node = -1;
    for(unsigned int i = 0; i < nodes.size(); i++)
    {

        if(distanceSquared(testPoint, nodes[i]) < closestDistance)
        {
            closestDistance = distanceSquared(testPoint, nodes[i]);
            node = i;
        }
    }

    return node;
}

int FVCOMStructure::getClosestTime(double time) const
{
    auto lower = std::lower_bound(times.begin(), times.end(), time);

    int index1 = std::distance(times.begin(), lower);
    int index2 = index1 - 1;

    if(std::abs(times[index1] - time) > std::abs(times[index2] - time))
    {
        return index2;
    }
    else
    {
        return index1;
    }

}

int FVCOMStructure::getPreviousTimeIndex(double time) const
{
    auto lower = std::lower_bound(times.begin(), times.end(), time);
    int index = std::distance(times.begin(), lower);

    if(times[index] == time)
     {
         return index;
    }

    return index - 1;
}

float FVCOMStructure::getTime(int timeIndex) const
{
    return times[timeIndex];
}

FVCOMStructure::Plane FVCOMStructure::getTrianglePlane(int triangle) const
{
    const std::vector<int>& surroundingNodes = triangleToNodes[triangle];

    FVCOMStructure::Point p0 = getNodePointWithH(surroundingNodes[0]);
    FVCOMStructure::Point p1 = getNodePointWithH(surroundingNodes[1]);
    FVCOMStructure::Point p2 = getNodePointWithH(surroundingNodes[2]);

    FVCOMStructure::Plane plane(p0, p1, p2);

    return plane;
}

FVCOMStructure::Plane FVCOMStructure::getTriangleSiglayPlane(int triangle, unsigned int siglay) const
{
    const std::vector<int>& surroundingNodes = triangleToNodes[triangle];

    FVCOMStructure::Point p0 = getNodePointAtSiglay(surroundingNodes[0], siglay);
    FVCOMStructure::Point p1 = getNodePointAtSiglay(surroundingNodes[1], siglay);
    FVCOMStructure::Point p2 = getNodePointAtSiglay(surroundingNodes[2], siglay);

    FVCOMStructure::Plane plane(p0, p1, p2);

    return plane;
}

double FVCOMStructure::distance(Point p0, Point p1) const
{
    return std::sqrt((p0.x - p1.x)*(p0.x - p1.x) + (p0.y - p1.y)*(p0.y - p1.y) );
}

double FVCOMStructure::distanceSquared(Point p0, Point p1) const
{
    return (p0.x - p1.x)*(p0.x - p1.x) + (p0.y - p1.y)*(p0.y - p1.y);
}

const FVCOMStructure::Point FVCOMStructure::getNodePointWithH(int node) const
{
    return nodes[node];
}

const FVCOMStructure::Point FVCOMStructure::getNodePointAtSiglay(int node, int siglay) const
{
    FVCOMStructure::Point returnPoint = nodes[node];
    returnPoint.z = returnPoint.z * nodeSiglay[node][siglay];

    return returnPoint;
}

FVCOMStructure::ChunkInfo FVCOMStructure::getChunkForNode(int node, int siglay, int time) const
{
    //Chunk ids based on this ordering (x,y,sigma,time)
    FVCOMStructure::ChunkInfo chunk;

    double nodeX = nodes[node].x;
    double nodeY = nodes[node].y;

    //calculate the chunks for each individual dimension
    chunk.xChunk = (nodeX - minX) / xChunkSize;
    chunk.yChunk = (nodeY - minY) / yChunkSize;
    chunk.siglayChunk = siglay / siglayChunkSize;
    chunk.timeChunk = time / timeChunkSize;

    //Check to insure that the chunks are valid.  If not this node is in the last chunk dimension.
    //NOTE: This should only occur if the x/y extent is divisible by x/y chunk dimension.
    //In this case maxX and maxY will give a chunk# as 1 more than the last chunk index.
    //It seems like a waste to have the a chunk only be these single nodes so they are included
    //in the last chunk.
    if(chunk.xChunk >= xDimChunks)
    {
        chunk.xChunk = xDimChunks - 1;
    }

    if(chunk.yChunk >= yDimChunks)
    {
        chunk.yChunk = yDimChunks - 1;
    }

    chunk.id = chunk.timeChunk +
           (chunk.siglayChunk * timeDimChunks) +
           (chunk.yChunk * timeDimChunks * siglayDimChunks) +
           (chunk.xChunk * timeDimChunks * siglayDimChunks * yDimChunks);

    chunk.xStart = chunk.xChunk * xChunkSize - minX;
    chunk.yStart = chunk.yChunk * yChunkSize - minX;
    chunk.siglayStart = chunk.siglayChunk * siglayChunkSize;
    chunk.timeStart = chunk.timeChunk * timeChunkSize;

    chunk.xSize = xChunkSize;
    chunk.ySize = yChunkSize;

    unsigned int timeSize = times.size();
    chunk.siglaySize = std::min(siglayChunkSize, siglayDim - chunk.siglayStart);
    chunk.timeSize = std::min(timeChunkSize, timeSize - chunk.timeStart);

    return chunk;
}

FVCOMStructure::ChunkInfo FVCOMStructure::getChunkForTriangle(int triangle, int siglay, int time) const
{
    //Chunk ids based on this ordering (x,y,sigma,time)

    FVCOMStructure::ChunkInfo chunk;

    double triangleX = triangles[triangle].x;
    double triangleY = triangles[triangle].y;

    //calculate the chunks for each individual dimension

    chunk.xChunk = (triangleX - minX) / xChunkSize;
    chunk.yChunk = (triangleY - minY) / yChunkSize;
    chunk.siglayChunk = siglay / siglayChunkSize;
    chunk.timeChunk = time / timeChunkSize;

    //Check to insure that the chunks are valid.  If not this node is in the last chunk dimension.
    //NOTE: This should only occur if the x/y extent is divisible by x/y chunk dimension.
    //In this case maxX and maxY will give a chunk# as 1 more than the last chunk index.
    //It seems like a waste to have the a chunk only be these single nodes so they are included
    //in the last chunk.

    if(chunk.xChunk >= xDimChunks)
    {
        chunk.xChunk = xDimChunks - 1;
    }

    if(chunk.yChunk >= yDimChunks)
    {
        chunk.yChunk = yDimChunks - 1;
    }

    chunk.id = chunk.timeChunk +
           (chunk.siglayChunk * timeDimChunks) +
           (chunk.yChunk * timeDimChunks * siglayDimChunks) +
           (chunk.xChunk * timeDimChunks * siglayDimChunks * yDimChunks);

    chunk.xStart = chunk.xChunk * xChunkSize - minX;
    chunk.yStart = chunk.yChunk * yChunkSize - minX;
    chunk.siglayStart = chunk.siglayChunk * siglayChunkSize;
    chunk.timeStart = chunk.timeChunk * timeChunkSize;

    chunk.xSize = xChunkSize;
    chunk.ySize = yChunkSize;

    unsigned int timeSize = times.size();
    chunk.siglaySize = std::min(siglayChunkSize, siglayDim - chunk.siglayStart);
    chunk.timeSize = std::min(timeChunkSize, timeSize - chunk.timeStart);

    return chunk;
}

void FVCOMStructure::timeInterpolation(double time, int& time1Index, int& time2Index, double& time1Percent) const
{
    time1Index = getPreviousTimeIndex(time);
    double previousTime = getTime(time1Index);

    //Time is exactly on a time division, no interpolation needed.
    if(previousTime == time)
    {
        time2Index = time1Index;
        time1Percent = 1;
    }
    else //time is spilt between divisions so it needs interpolation
    {
        time2Index = time1Index + 1;
        double nextTime = getTime(time2Index);
        time1Percent = (nextTime - time) / (nextTime - previousTime);
    }
}

void FVCOMStructure::siglayInterpolation(FVCOMStructure::Point& interpolatePoint, int& siglay1Index, int& siglay2Index, double& siglay1Percent)
{
    int containingTriangle = getContainingTriangle(interpolatePoint);
    siglayInterpolation(interpolatePoint, siglay1Index, siglay2Index, siglay1Percent, containingTriangle);
}

void FVCOMStructure::siglayInterpolation(FVCOMStructure::Point& interpolatePoint, int& siglay1Index, int& siglay2Index, double& siglay1Percent, int containingTriangle)
{
    siglay1Index = siglay2Index = -1;

    double prevDot = 0;
    for(uint i = 0; i < getNumSiglays(); i++)
    {
        FVCOMStructure::Plane plane = getTriangleSiglayPlane(containingTriangle, i);

        //calculate the dot product with the plane and the point to determine which side of the plane it is on
        double dot = plane.a * interpolatePoint.x + plane.b * interpolatePoint.y + plane.c * interpolatePoint.z + plane.d;

        if(i != 0)
        {
            if(dot == 0) //point is in the plane, use current siglayIndex and no interpolation needed
            {
                siglay1Index = i;
                siglay2Index = i;
                break;
            }
            else if((dot > 0 && prevDot < 0) || //The sign of dot has changed so the siglay has been found
                    (dot < 0 && prevDot > 0))
            {
                siglay1Index = i - 1;
                siglay2Index = i;
                break;
            }
            else if((prevDot > 0 && prevDot < dot) || //The distance from the plane to the point is increasing so we have passed it.
                    (prevDot < 0 && prevDot > dot))   //If this occurs then it means the point is above the 0th siglay, use the 0th siglay
            {
                siglay1Index = 0;
                siglay2Index = 0;
                break;
            }
        }

        prevDot = dot;
    }

    //The dot product was always decreasing however it never changed sign.
    //Therefore the point is below the final siglay, so use the final siglay.
    if(siglay1Index == siglay2Index && siglay2Index == -1)
    {
        siglay1Index = getNumSiglays() -1;
        siglay2Index = getNumSiglays() -1;
    }

    if(siglay1Index == siglay2Index) //The point is above the 0th siglay so and there is no data there
    {
        siglay1Percent = 1.0;
    }
    else
    {
        FVCOMStructure::Plane upperPlane = getTriangleSiglayPlane(containingTriangle, siglay1Index);
        FVCOMStructure::Plane lowerPlane = getTriangleSiglayPlane(containingTriangle, siglay2Index);

        double upperH = (-upperPlane.d - upperPlane.a * interpolatePoint.x - upperPlane.b * interpolatePoint.y) / upperPlane.c;
        double lowerH = (-lowerPlane.d - lowerPlane.a * interpolatePoint.x - lowerPlane.b * interpolatePoint.y) / lowerPlane.c;

        siglay1Percent = (lowerH - interpolatePoint.z) / (lowerH - upperH);
    }
}

double FVCOMStructure::getDepthAtPoint(FVCOMStructure::Point& interpolatePoint, int containingTriangle)
{
    const std::vector<int>& surroundingNodes = getNodesInTriangle(containingTriangle);

    FVCOMStructure::Point p1 = getNodePointWithH(surroundingNodes[0]);
    FVCOMStructure::Point p2 = getNodePointWithH(surroundingNodes[1]);
    FVCOMStructure::Point p3 = getNodePointWithH(surroundingNodes[2]);

    FVCOMStructure::Plane groundPlane(p1,p2,p3);

    return groundPlane.getHeight(interpolatePoint);
}

double FVCOMStructure::getDepthAtPoint(FVCOMStructure::Point& interpolatePoint)
{
    int containingTriangle = getContainingTriangle(interpolatePoint);

    const std::vector<int>& surroundingNodes = getNodesInTriangle(containingTriangle);

    FVCOMStructure::Point p1 = getNodePointWithH(surroundingNodes[0]);
    FVCOMStructure::Point p2 = getNodePointWithH(surroundingNodes[1]);
    FVCOMStructure::Point p3 = getNodePointWithH(surroundingNodes[2]);

    FVCOMStructure::Plane groundPlane(p1,p2,p3);

    return groundPlane.getHeight(interpolatePoint);
}

const bool FVCOMStructure::pointInModel(Point p, double time)
{
    int containingTriangle = 0;
    try
    {
        containingTriangle = getContainingTriangle(p);
    }
    catch(const std::out_of_range& e)
    {
        return false;
    }

    Plane plane = getTrianglePlane(containingTriangle);
    double depth = plane.getHeight(p);
    return p.x >= minX && p.x <= maxX &&
           p.y >= minY && p.y <= maxY &&
           time >= times[0] && time <= times[times.size() - 1] &&
           p.z <= 0 && p.z >= -depth;
}

const bool FVCOMStructure::timeInModel(double time) const
{
    return time >= times[0] && time <= times[times.size() - 1];
}

const bool FVCOMStructure::depthInModel(Point p)
{
    int containingTriangle = 0;
    try
    {
        containingTriangle = getContainingTriangle(p);
    }
    catch(const std::out_of_range& e)
    {
        return false;
    }

    Plane plane = getTrianglePlane(containingTriangle);
    double depth = plane.getHeight(p);

    return p.z <= 0 && p.z >= -depth;
}

const bool FVCOMStructure::xyInModel(Point p) const
{
    return p.x >= minX && p.x <= maxX &&
           p.y >= minY && p.y <= maxY;
}

const std::vector<unsigned int>& FVCOMStructure::getNodesInChunk(FVCOMStructure::ChunkInfo chunk) const
{
    int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

    return nodesInChunk[chunkId];
}

const std::vector<unsigned int>& FVCOMStructure::getTrianglesInChunk(FVCOMStructure::ChunkInfo chunk) const
{
    int chunkId = chunk.yChunk + (chunk.xChunk * yDimChunks);

    return trianglesInChunk[chunkId];
}

const unsigned int FVCOMStructure::getNumSiglays() const
{
    return siglayDim;
}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_SMOOTHER_HPP
#define MFMG_DEALII_SMOOTHER_HPP

#include <mfmg/common/operator.hpp>
#include <mfmg/common/smoother.hpp>

#include <deal.II/lac/trilinos_precondition.h>

#include <boost/property_tree/ptree.hpp>

#include <memory>

namespace mfmg
{
template <typename VectorType>
class DealIISmoother : public Smoother<VectorType>
{
public:
  using vector_type = VectorType;

  DealIISmoother(std::shared_ptr<Operator<vector_type> const> op,
                 std::shared_ptr<boost::property_tree::ptree const> params);

  virtual ~DealIISmoother() override = default;

  void apply(vector_type const &b, vector_type &x) const override final;

private:
  std::unique_ptr<dealii::TrilinosWrappers::PreconditionBase> _smoother;
};
} // namespace mfmg

#endif


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#pragma once

#include <functional>
#include <boost/cstdint.hpp>

#include "NOpenMeshType.hpp"
#include "NOpenMeshEnum.hpp"

template <typename T>
struct NPY_API  NOpenMeshBisectFunc
{
    typedef typename std::function<float(float,float,float)> SDF ;
    typedef typename T::Point              P ;

    NOpenMeshBisectFunc( const SDF& sdf, const P& a, const P& b ) ;

    void position(P& tp, const float t) const ;
    float operator()(const float t) const ;

    const SDF& sdf ;
    const P&     a ;
    const P&     b ;
};

struct NPY_API NOpenMeshBisectTol
{
    NOpenMeshBisectTol( float tolerance ) : tolerance(tolerance) {} ;
    bool operator()(const float& min, const float& max )
    {
        return max - min < tolerance ;
    }
    float tolerance ;
};

template <typename T>
struct NPY_API  NOpenMeshBisect
{
    typedef typename std::function<float(float,float,float)> SDF ;
    typedef typename T::Point              P ;

     NOpenMeshBisect( const SDF& sdf, const P& a, const P& b , float tolerance ) ;

     void bisect(P& frontier, float& t );

     const NOpenMeshBisectFunc<T> func ;
     const NOpenMeshBisectTol     tol ;
     const bool                 invalid ;
     const bool                 degenerate ;
     boost::uintmax_t           iterations ;

};


// 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_MULT_RESULT_INCLUDE
#define MTL_MULT_RESULT_INCLUDE

#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>

#if 0
#include <boost/numeric/mtl/matrix/map_view.hpp>
#include <boost/numeric/mtl/matrix/mat_mat_times_expr.hpp>
#include <boost/numeric/mtl/matrix/implicit_dense.hpp>
#include <boost/numeric/mtl/vector/map_view.hpp>
#include <boost/numeric/mtl/operation/mat_cvec_times_expr.hpp>
#include <boost/numeric/mtl/vector/rvec_mat_times_expr.hpp>
#endif

namespace mtl { namespace traits {

template <typename Op1, typename Op2, typename MultOp> struct mult_result_aux;
template <typename Op1, typename Op2, typename MultOp> struct vec_mult_result_aux;
    //template <typename Op1, typename Op2, typename MultOp1, typename MultOp2> struct mult_result_if_equal_aux;

/// Result type for multiplying arguments of types Op1 and Op2
/** Can be used in enable-if-style as type is only defined when appropriate.
    This one is used if at least one argument is a matrix.
**/
template <typename Op1, typename Op2>
struct mult_result
  : public mult_result_aux<Op1, Op2, typename ashape::mult_op<typename ashape::ashape<Op1>::type,
							      typename ashape::ashape<Op2>::type >::type>
{};

/// Result type for multiplying arguments of types Op1 and Op2
/** Can be used in enable-if-style as type is only defined when appropriate.
    This one is used if at least one argument is a vector and none is a matrix.
**/
template <typename Op1, typename Op2>
struct vec_mult_result
  : public vec_mult_result_aux<Op1, Op2, typename ashape::mult_op<typename ashape::ashape<Op1>::type,
								  typename ashape::ashape<Op2>::type >::type>
{};

/// Result type for multiplying arguments of types Op1 and Op2
/** MultOp according to the algebraic shapes **/
template <typename Op1, typename Op2, typename MultOp>
struct mult_result_aux {};

/// Scale matrix from left
template <typename Op1, typename Op2>
struct mult_result_aux<Op1, Op2, ::mtl::ashape::scal_mat_mult>
{
    typedef matrix::scaled_view<Op1, Op2> type;
};

/// Scale matrix from right needs functor for scaling from right
template <typename Op1, typename Op2>
struct mult_result_aux<Op1, Op2, ::mtl::ashape::mat_scal_mult>
{
    typedef matrix::rscaled_view<Op1, Op2> type;
};

/// Multiply matrices
template <typename Op1, typename Op2>
struct mult_result_aux<Op1, Op2, ::mtl::ashape::mat_mat_mult>
{
    typedef matrix::mat_mat_times_expr<Op1, Op2> type;
};

/// Multiply matrix with column vector
template <typename Op1, typename Op2>
struct mult_result_aux<Op1, Op2, ::mtl::ashape::mat_cvec_mult>
{
    typedef mat_cvec_times_expr<Op1, Op2> type;
};

/// Multiply column with row vector and return implicit matrix
template <typename Op1, typename Op2>
struct vec_mult_result_aux<Op1, Op2, ::mtl::ashape::cvec_rvec_mult>
{
    typedef mtl::matrix::outer_product_matrix<Op1, Op2> type;
};

/// Result type for multiplying arguments of types Op1 and Op2
/** MultOp according to the algebraic shapes **/
template <typename Op1, typename Op2, typename MultOp>
struct vec_mult_result_aux {};

/// Scale row vector from left
template <typename Op1, typename Op2>
struct vec_mult_result_aux<Op1, Op2, ::mtl::ashape::scal_rvec_mult>
{
    typedef vector::scaled_view<Op1, Op2> type;
};

/// Scale column vector from left
template <typename Op1, typename Op2>
struct vec_mult_result_aux<Op1, Op2, ::mtl::ashape::scal_cvec_mult>
{
    typedef vector::scaled_view<Op1, Op2> type;
};

/// Multiply row vector with matrix
// added by Cornelius and Peter
template <typename Op1, typename Op2>
struct vec_mult_result_aux<Op1, Op2, ::mtl::ashape::rvec_mat_mult>
{
    typedef vector::rvec_mat_times_expr<Op1, Op2> type;
};

/// Scale row vector from right
// added by Hui Li
template <typename Op1, typename Op2>
struct vec_mult_result_aux<Op1, Op2, ::mtl::ashape::rvec_scal_mult>
{
    typedef vector::rscaled_view<Op1, Op2> type;
};

/// Scale column vector from right
// added by Hui Li
template <typename Op1, typename Op2>
struct vec_mult_result_aux<Op1, Op2, ::mtl::ashape::cvec_scal_mult>
{
    typedef vector::rscaled_view<Op1, Op2> type;
};

/// Enabler if operation is rvec_cvec_mult
template <typename Op1, typename Op2, typename Result>
struct lazy_enable_if_rvec_cvec_mult
  : boost::lazy_enable_if<boost::is_same<typename ashape::mult_op<typename ashape::ashape<Op1>::type,
								  typename ashape::ashape<Op2>::type >::type,
					 ashape::rvec_cvec_mult>,
			  Result>
{};

}} // namespace mtl::traits

#endif // MTL_MULT_RESULT_INCLUDE


/*=========================================================================
 *
 *  Copyright David Doria 2012 daviddoria@gmail.com
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#ifndef VarianceFunctor_HPP
#define VarianceFunctor_HPP

#include <boost/graph/graph_traits.hpp>

// Parent class
#include "Visitors/AcceptanceVisitors/AcceptanceVisitorParent.h"

// Custom
#include "Helpers/Statistics.h"
#include "ITKHelpers/ITKHelpers.h"

// ITK
#include "itkImage.h"
#include "itkImageRegion.h"

/**

 */
struct VarianceFunctor
{
  template <typename TPixel>
  typename TypeTraits<TPixel>::LargerType operator()(const std::vector<TPixel>& pixels) const
  {
    assert(pixels.size() > 0);
//     if(pixels.size() <= 0)
//     {
//       throw std::runtime_error("Must have more than 0 items to use VarianceFunctor!");
//     }
    typename TypeTraits<TPixel>::LargerType allChannelsVariance = Statistics::Variance(pixels);
    return allChannelsVariance;
  }
};

#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, 6> test_int_2_6(const Eigen::Matrix<int, 2, 6> & M)
{
	return M;
}
void export_int_2_6()
{
	boost::python::def("test_int_2_6",test_int_2_6);
}