/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_ALGORITHMS_LISCHINSKI_MINIMIZATION_HPP
#define PIC_ALGORITHMS_LISCHINSKI_MINIMIZATION_HPP

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Sparse"
    #include "../externals/Eigen/src/SparseCore/SparseMatrix.h"
#else
    #include <Eigen/Sparse>
    #include <Eigen/src/SparseCore/SparseMatrix.h>
#endif

#endif

#include "../base.hpp"
#include "../image.hpp"

namespace pic {
/**
*
*	param[0] --> alpha
*	param[1] --> lambda
*
**/

/**
 * @brief LischinskiFunction
 * @param Lcur
 * @param Lref
 * @param param
 * @param LISCHINSKI_EPSILON
 * @return
 */
inline float LischinskiFunction(float Lcur, float Lref, float param[2],
                                float LISCHINSKI_EPSILON = 0.0001f)
{
    return -param[1] / (powf(fabsf(Lcur - Lref), param[0]) + LISCHINSKI_EPSILON);
}

/**
 * @brief LischinskiFunctionGauss
 * @param Lcur
 * @param Lref
 * @param param
 * @return
 */
inline float LischinskiFunctionGauss(float Lcur, float Lref, float param[2])
{
    return expf(-powf(Lcur - Lref, 2.0f) * 10.0f);
}

/**
 * @brief LischinskiMinimization
 * @param L
 * @param g
 * @param omega
 * @param omega_global
 * @param gOut
 * @param alpha
 * @param lambda
 * @param LISCHINSKI_EPSILON
 * @return
 */
PIC_INLINE Image *LischinskiMinimization(Image *L,
                              Image *g,
                              Image *omega = NULL,
                              float omega_global = 1.0f,
                              Image *gOut = NULL,
                              float alpha = 1.0f,
                              float lambda = 0.4f,
                              float LISCHINSKI_EPSILON = 1e-4f)
{
    if(L == NULL || g == NULL) {
        return gOut;
    }

#ifndef PIC_DISABLE_EIGEN
    bool bOmega = (omega == NULL);

    int width = L->width;
    int height = L->height;
    int tot = height * width;

    float param[2];
    param[0] = alpha;
    param[1] = lambda;

    Eigen::VectorXd b, x;
    b = Eigen::VectorXd::Zero(tot);

    #ifdef PIC_DEBUG
        printf("Init matrix...");
    #endif

    std::vector< Eigen::Triplet< double > > tL;

    for(int i = 0; i < height; i++) {
        int tmpInd = i * width;

        for(int j = 0; j < width; j++) {

            float sum = 0.0f;
            float tmp;
            int indJ;
            int indI = tmpInd + j;
            float Lref = L->data[indI];

            float omega_val;
            if(bOmega) {
                omega_val = omega_global;
            } else {
                omega_val = omega->data[indI];
            }

            b[indI] = omega_val * g->data[indI];

            if((i - 1) >= 0) {
                indJ = indI - width;
                tmp = LischinskiFunction(L->data[indJ], Lref, param, LISCHINSKI_EPSILON);
                tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                sum += tmp;
            }

            if((i + 1) < height) {
                indJ = indI + width;
                tmp = LischinskiFunction(L->data[indJ], Lref, param, LISCHINSKI_EPSILON);
                tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                sum += tmp;
            }

            if((j - 1) >= 0) {
                indJ = indI - 1;
                tmp = LischinskiFunction(L->data[indJ], Lref, param, LISCHINSKI_EPSILON);
                tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                sum += tmp;
            }

            if((j + 1) < width) {
                indJ = indI + 1;
                tmp = LischinskiFunction(L->data[indJ], Lref, param, LISCHINSKI_EPSILON);
                tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                sum += tmp;
            }

            tL.push_back(Eigen::Triplet< double > (indI, indI, omega_val - sum));
        }
    }

    #ifdef PIC_DEBUG
        printf("Ok\n");
    #endif

    Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);
    A.setFromTriplets(tL.begin(), tL.end());

    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver(A);
    x = solver.solve(b);

    if(solver.info() != Eigen::Success) {
        #ifdef PIC_DEBUG
            printf("SOLVER FAILED!\n");
        #endif
        return NULL;
    }

    #ifdef PIC_DEBUG
        printf("SOLVER SUCCESS!\n");
    #endif

    if(gOut == NULL) {
        gOut = g->allocateSimilarOne();
    } else {
        if(!gOut->isSimilarType(g)) {
            gOut = g->allocateSimilarOne();
        }
    }

    for(int i = 0; i < height; i++) {
        int counter = i * width;

        for(int j = 0; j < width; j++) {
            (*gOut)(j, i)[0] = float(x(counter + j));
        }
    }

    return gOut;
#else
    return gOut;
#endif
}

} // end namespace pic

#endif /* PIC_ALGORITHMS_LISCHINSKI_MINIMIZATION_HPP */


#include <math.h>
#include <limits.h>
#include "defines.h"
#include <flann/mpi/matrix.h>
#include <algorithm>
#include <vector>
#include <map>
#include <matio.h>
#include "ros/ros.h"
#include <nav_msgs/GridCells.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#define INF 1000000.0

using namespace std;
using namespace __gnu_cxx;
using namespace flann;

typedef boost::geometry::model::d2::point_xy<double> point_type;
typedef boost::geometry::model::polygon<point_type> polygon_type;

int xf, yf, fif;

extern const float mapResolution;
extern const int node_dist;

void publishVisitedNodes(nav_msgs::GridCells gCells);

class Vector : public vector<int>
{
public:
    //operator size_t() const { return (*this).size(); };
    operator size_t() const { return (*this)[0]*1600 + (*this)[1]*16 + (*this)[2]; };
};

// zamjena mjesta elemenata na indexima i,j u vektoru
void swap(int i, int j, vector<pair<Vector, double> > *v, map<Vector, int> *h)
{
    pair<Vector, double> tmp;
    tmp = (*v)[i];
    (*v)[i] = (*v)[j];
    (*v)[j] = tmp;

    (*h)[(*v)[i].first] = i;
    (*h)[(*v)[j].first] = j;
}

// podizanje elementa na odgovarajući nivo u binarnom stablu
void bubble_up(int index, vector<pair<Vector, double> > *v, map<Vector, int> *h)
{
    int new_ind;

    while (index > 0)
    {
        new_ind = (index - 1)/2;

        if ((*v)[index].second < (*v)[new_ind].second)
        {
            swap(index, new_ind, v, h);
            index = new_ind;
        }
        else
        {
            break;
        }
    }
}

// spuštanje elementa na odgovarajući nivo u binarnom stablu
void bubble_down(int index, vector<pair<Vector, double> > *v, map<Vector, int> *h)
{
    int size = v->size();
    int new_ind;

    while (true)
    {
        if (2 * index + 2 >= size)
        {
            if (2 * index + 1 >= size)
            {
                break;
            }
            else
            {
                new_ind = 2 * index + 1;
            }
        }
        else
        {
            if ((*v)[2 * index + 1].second < (*v)[2 * index + 2].second)
            {
                new_ind = 2 * index + 1;
            }
            else
            {
                new_ind = 2 * index + 2;
            }
        }

        if ((*v)[index].second > (*v)[new_ind].second)
        {
            swap(index, new_ind, v, h);
            index = new_ind;
        }
        else
        {
            break;
        }
    }
}

// provjera izvedivosti i određivanje cijene segmenta
// ako je segment izvediv vraća se njegova cijena, u suprotnom se vraća vrijednost INF
double cost(int x, int y, int ei, int ej, vector<vector<pair<int, int> > > *cm, const vector<vector<short> >& map, vector<pair<vector<int>, double> > *g)
{
    double rez = 0.0;
    int m, n, mapWidth, mapHeight;
    double h = 0;

    mapWidth = map.size();

    if(mapWidth > 0)
        mapHeight = map[0].size();
    else return INF;

    // ***********************************************************************
    // ************* PROVJERA KOLIZIJE IZMEĐU VOZILA I PREPREKA  *************
    // ***********************************************************************

    for (unsigned int i = 0; i < cm[ei][ej].size(); i++)
    {
      m = node_dist * x + cm[ei][ej][i].first;
      n = node_dist * y + cm[ei][ej][i].second;

      // ako se bilo koja od čelija koju vozilo okupira tijekom izvođenja segmenta putanje nalazi
      // izvan granica karte, vraća se vrijednost 100000
      if (m < 0 || n < 0 || m > (mapWidth - 1) || n > (mapHeight - 1))
      {
        return INF;
      }
      else
      {
        if (map[m][n] < 0)  // ako čelija zahvaća prepreku vraća se vrijednost 100000
          return INF;
        else
          rez += map[m][n];
      }
    }
    // **********************************************************************

    int vx = x + g[ei][ej].first[0];
    int vy = y + g[ei][ej].first[1];

    int dx = xf - vx;
    int dy = yf - vy;

    h = sqrt(dx * dx + dy * dy);    // heuristična komponenta cijene
    //fprintf(stderr, "xf: %d yf: %d vx: %d vy: %d h: %f g: %f\n", xf, yf, vx, vy, h, g[ei][ej].second);

    return (g[ei][ej].second + h);  // ako sve čelije segmenta putanje leže u slobodnom prostoru karte, vraća se cijena segmenta
}

// graph search algoritm (Dijkstra + A*)
void graphSearch(matvar_t * varVars, matvar_t * varCost, const vector<vector<short> >& vMap, int start[], int finish[], vector<int> wrX, vector<int> wrY, double** M, int sizeM[], int** veh_orientations)
{
    // varVars -> sadrži informacije o latici; za svaku početnu orijentaciju sadrži koord. susjednih čvorova, konačne orijentacije i cijene pripadajućih segmenta
    // varCost -> koordinate svih čelija kroz koje vozilo prolazi prilikom gibanja pojedinim segmentom u latici

    vector<int> wrongx;
    vector<int> wrongy;

    // početne koordinate i orijentacija vozila
    int xs =  start[0];
    int ys =  start[1];
    int fis = start[2];

    xf = finish[0];
    yf = finish[1];
    fif = finish[2];

    //publishanje posjećenih čvorova (za prikaz u RViz-u)
    geometry_msgs::Point a;
    nav_msgs::GridCells gCells;
    gCells.header.frame_id = "/Alfa/map";
    gCells.cell_width = mapResolution;
    gCells.cell_height = mapResolution;
    gCells.cells.clear();

    map<Vector, int> hm;
    map<Vector, pair<int, int> > hm_sol;

    int m, n, mc, nc;
    double *data_in, *data_out, *cell_in, *map_in;

    vector<pair<vector<int>, double> > graph[16];
    vector<vector<pair<int, int> > > cost_map[16];

    vector<int> tmp;
    vector<pair<int, int> > vptmp;
    double sf;

    m = varVars->dims[0];
    n = varVars->dims[1];

    data_in = (double*)varVars->data;

    // petlja po svim segmentima u latici, počevši od segmenata s početnom orijentacijom 0
    for (int i = 0; i < m; i++)
    {
        tmp.clear();
        tmp.push_back((int)data_in[m + i]);     // x - koordinata susjednog čvora
        tmp.push_back((int)data_in[2*m + i]);   // y - koordinata susjednog čvora
        tmp.push_back((int)data_in[3*m + i]);   // konačna orijentacija u susjednom čvoru
        sf = data_in[4*m + i];                  // cijena (duljina pripadajućeg segmenta)

        // indexi elemenata vektora "graph" predstavljaju početne orijentacije (0-15)
        // svaki element vektora "graph" sadrži informacije o svim
        // susjednim čvorovima u koje se može doći s dotičnom početnom orijentacijom
        // informacije o susjednim čvorovima sadržane su u parovima oblika:
        // (x-y koordinate susjednog čvora + konačna orijentacija, cijena pripadajućeg segmenta)
        graph[(int)data_in[i]].push_back(make_pair(tmp, sf));

        matvar_t* matvar = Mat_VarGetCell(varCost, i);
        mc = matvar->dims[0];
        nc = matvar->dims[1];
        cell_in = (double*)matvar->data;

        vptmp.clear();
        for (int j = 0; j < nc; j++)
        {
            vptmp.push_back(make_pair((int)cell_in[2*j], (int)cell_in[2*j+1]));
        }

        // indexi elemenata vektora "cost_map" predstavljaju početne orijentacije (0-15)
        // svaki element vektora "cost_map" sadrži vektore parova s
        // x-y koordinatama svih čelija kroz koje vozilo prolazi prilikom
        // izvođenja pojedinog segmenta s dotičnom početnom orijentacijom
        cost_map[(int)data_in[i]].push_back(vptmp);
    }

    // definicija vektora za pohranu posjećenih čvorova prilikom pretraživanja grafa
    // koristi se kao sortirano binarno stablo (min-heap)
    // korijen stabla (prvi element vektora) je čvor s najmanjom cijenom
    vector<pair<Vector, double> > heap;

    double c;
    double new_c;
    //double pen = 0.0; // penalizacija promjena kuta
    bool path_exist = true;

    Vector v;
    Vector new_v;

    // spremanje početnih koordinata i orijentacije vozila u vektor "v"
    v.push_back(xs);
    v.push_back(ys);
    v.push_back(fis);

    heap.push_back(make_pair(v, 0.0));

    bool found;

    while(true)
    {
        if (heap.size() == 0)
        {
            path_exist = false;
            break;
        }
        swap(0, heap.size() - 1, &heap, &hm);   // zamjena mjesta prvog i posljednjeg čvora u stablu -> čvor s najmanjom cijenom dolazi na dno stabla

        // pomicanje u sljedeći čvor s najmanjom cijenom iz kojega će se dalje provjeravati ukupne cijene dolaska u njegove susjedne čvorove
        v = heap.back().first;  // koordinate trenutnog čvora (x,y,fi)
        c = heap.back().second; // ukupna minimalna cijena dolaska u trenutni čvor iz početnog čvora
        hm[v] = -1;

        heap.pop_back();    // uklanjanje čvora s najmanjom cijenom iz binarnog stabla (to je trenutno zadnji čvor u heap-u)
        bubble_down(0, &heap, &hm); // spuštanje elementa koji se privremeno našao na root mjestu binarnog stabla (nakon prethodne swap operacije)

        // publishanje posjećenih čvorova (za prikaz u RViz-u)
        /*a.x = v[0] * node_dist * mapResolution;
        a.y = v[1] * node_dist * mapResolution;
        a.z = 0.0;
        gCells.cells.push_back(a);
        gCells.header.stamp = ros::Time::now();
        publishVisitedNodes(gCells);*/

        // određivanje ciljnih koordinata kod removinga
        if (v[0] == xf  &&  v[1] == yf  &&  v[2] == fif) // ako su koord. tenutnog čvora jednake koordinatama ciljnog čvora..
        {
            //fprintf(stderr, "Pronađen ciljni čvor!!\n");
            break;  // prekida se proces planiranja putanje..
        }

        // određivanje najkraćih putanja do svih susjednih čvorova pod pretpostavkom da se vozilo kreće unaprijed
        for (unsigned int i = 0; i < graph[v[2]].size(); i++)   // graph[v[2]] sadrži podatke o svim susjednim čvorovima u koje se može doći iz trenutnog čvora "v" uz početnu orijentaciju v[2]
        {
            // u "new_v" se upisuju x-y koordinate i konačne orijentacije i-tog susjednog čvora koji je povezan s čvorom "v",
            // a u koji se iz čvora "v" može doći uz početnu orijentaciju v[2]
            new_v.clear();
            new_v.push_back(v[0] + graph[v[2]][i].first[0]);    // x - koordinata i-tog susjednog čvora
            new_v.push_back(v[1] + graph[v[2]][i].first[1]);    // y - koordinata i-tog susjednog čvora
            new_v.push_back(graph[v[2]][i].first[2]);           // konačna orijentacija u i-tom susjednom čvoru

            if (hm.find(new_v) == hm.end()) // ako "new_v" još nije dodan u mapu "hm"..
            {
                //heap.push_back(make_pair(new_v, c + graph[v[2]][i].second + pen * abs(new_v[2] - v[2])));
                new_c = cost(v[0], v[1], v[2], i, cost_map, vMap, graph);   // provjera izvedivosti i određivanje cijene segmenta

                if (new_c < INF)    // ako je segment izvediv
                {
                    heap.push_back(make_pair(new_v, c + new_c));    // čvor se dodaje na kraj stabla zajedno s pripadajućom ukupnom cijenom
                    hm[new_v] = heap.size() - 1;        // pamti se index čvora u stablu
                    hm_sol[new_v] = make_pair(v[2], i); // pamti se orijentacija u prethodnom čvoru i index segmenta do čvora "new_v"
                    bubble_up(heap.size() - 1, &heap, &hm); // čvor se podiže na odgovarajuće mjesto u stablu tako da stablo bude sortirano
                }
            }
            else
            {
                if (hm[new_v] != -1)
                {
                    // ako je cijena dolaska u susjedni čvor "new_v" manja od prehodne cijene za taj čvor..
                    if (c + cost(v[0], v[1], v[2], i, cost_map, vMap, graph) < heap[hm[new_v]].second)
                    {
                        if(heap[hm[new_v]].first[0] != new_v[0] || heap[hm[new_v]].first[1] != new_v[1] || heap[hm[new_v]].first[2] != new_v[2])
                            fprintf(stderr, "Error in graph search!\n");

                        //heap[hm[new_v]].second = c + graph[v[2]][i].second + pen * (abs(new_v[2] - v[2]));
                        heap[hm[new_v]].second = c + cost(v[0], v[1], v[2], i, cost_map, vMap, graph); // upisuje se nova cijena do čvora "new_v"
                        hm_sol[new_v] = make_pair(v[2], i); // pamti se orijentacija u prethodnom čvoru i index segmenta do čvora "new_v"
                        bubble_up(hm[new_v], &heap, &hm);   // modificira se mjesto "new_v" čvora u stablu tako da stablo ponovno bude sortirano
                    }
                }
            }
        }

        // određivanje najkraćih putanja do svih čvorova pod pretpostavkom da se vozilo kreće unatrag
        for (unsigned int i = 0; i < graph[(v[2] + 8) % 16].size(); i++)    // graph[(v[2] + 8) % 16] sadrži podatke o svim susjednim čvorovima u koje se može doći iz trenutnog čvora "v" kretanjem unatrag
        {
            // u "new_v" se upisuju x-y koordinate i konačne orijentacije i-tog susjednog čvora koji je povezan s čvorom "v",
            // a u koji se iz čvora "v" može doći uz početnu orijentaciju (v[2] + 8) % 16 (vožnjom unatrag)
            new_v.clear();
            new_v.push_back(v[0] + graph[(v[2] + 8) % 16][i].first[0]);     // x - koordinata i-tog susjednog čvora
            new_v.push_back(v[1] + graph[(v[2] + 8) % 16][i].first[1]);     // y - koordinata i-tog susjednog čvora
            new_v.push_back((graph[(v[2] + 8) % 16][i].first[2] + 8) % 16); // konačna orijentacija u i-tom susjednom čvoru

            if (hm.find(new_v) == hm.end()) // ako "new_v" još nije dodan u mapu "hm"..
            {
                //heap.push_back(make_pair(new_v, c + graph[v[2]][i].second + pen * abs(new_v[2] - v[2])));
                new_c = 2 * cost(v[0], v[1], (v[2] + 8) % 16, i, cost_map, vMap, graph);    // provjera izvedivosti i određivanje cijene segmenta
                if (new_c < INF)    // ako je segment izvediv
                {
                    heap.push_back(make_pair(new_v, c + new_c));    // čvor se dodaje na kraj stabla zajedno s pripadajućom ukupnom cijenom
                    hm[new_v] = heap.size() - 1;    // pamti se index čvora u stablu
                    hm_sol[new_v] = make_pair((v[2] + 8) % 16 + 16, i); // pamti se orijentacija u prethodnom čvoru i index segmenta do čvora "new_v"
                    bubble_up(heap.size() - 1, &heap, &hm); // čvor se podiže na odgovarajuće mjesto u stablu tako da stablo bude sortirano
                }
            }
            else
            {
                if (hm[new_v] != -1)
                {
                    // ako je cijena dolaska u susjedni čvor "new_v" manja od prehodne cijene za taj čvor..
                    if (c + 2*cost(v[0], v[1], (v[2] + 8) % 16, i, cost_map, vMap, graph) < heap[hm[new_v]].second)
                    {
                        if(heap[hm[new_v]].first[0] != new_v[0] ||  heap[hm[new_v]].first[1] != new_v[1] || heap[hm[new_v]].first[2] != new_v[2])
                            fprintf(stderr, "Error in graph search!\n");

                        //heap[hm[new_v]].second = c + graph[v[2]][i].second + pen * (abs(new_v[2] - v[2]));
                        heap[hm[new_v]].second = c + 2*cost(v[0], v[1], (v[2] + 8)%16, i, cost_map, vMap, graph); // upisuje se nova cijena do čvora "new_v"
                        hm_sol[new_v] = make_pair((v[2] + 8) % 16 + 16, i); // pamti se orijentacija u prethodnom čvoru i index segmenta do čvora "new_v"
                        bubble_up(hm[new_v], &heap, &hm);   // modificira se mjesto "new_v" čvora u stablu tako da stablo ponovno bude sortirano
                    }
                }
            }
        }
    }

    if (path_exist) // ako je pronađena izvediva putanja
    {
        vector<pair<int, int> > sol;
        pair<int, int> p;
        vector<int> orient;

        v[0] = xf; v[1] = yf; v[2] = fif;

        // kretanje unatrag od ciljnog čvora do početnog čvora
        while (true)
        {
            // kada se dođe do početnog čvora prekida se while petlja
            if (v[0] == xs  &&  v[1] == ys  &&  v[2] == fis)
            {
                break;
            }

            p = hm_sol[v];
            sol.push_back(p);   // pohrana početne orijentacije u prethodnom čvoru i indexa segmenta putanje koji povezuje prethodni čvor s trenutnim čvorom "v"

            // pomicanje u prethodni čvor (tj. u jedan čvor bliže početnom čvoru)
            v[0] = v[0] - graph[p.first % 16][p.second].first[0];
            v[1] = v[1] - graph[p.first % 16][p.second].first[1];

            if (p.first > 15)
            {
                v[2] = (p.first + 8) % 16;
            }
            else
                v[2] = p.first % 16;

            orient.push_back(v[2]);
        }

        // vektor "veh_orientations" sadrži stvarne orijentacije vozila na početku svakog segmenta, počevši od zadnjeg prema prvom
        *veh_orientations = new int[orient.size() * sizeof(int)];
        for (int i = orient.size() - 1; i >= 0; i--)
        {
            (*veh_orientations)[i] = orient[i];
        }

        *M = new double [2 * sol.size() * sizeof(double)];
        //*M = new double [2 * 336 * sizeof(double)];
        data_out = *M;
        sizeM[0] = 2;
        sizeM[1] = sol.size();  // ukupan broj segmenata od kojih se sastoji izračunata putanja
        //sizeM[1] = 336;   // ukupan broj segmenata od kojih se sastoji izračunata putanja

        int k = 0;
        for (int i = sol.size() - 1; i >= 0; i--)
        {
            data_out[k] = sol[i].first % 16;    // početna orijentacija u i-tom čvoru
            data_out[k+1] = sol[i].second;      // index segmenta koji uz danu početnu orijentaciju vodi do sljedećeg čvora na putanji
            k += 2;
        }

        // ZA ISCRTAVANJE CIJELE LATICE:
        /*for(int j = 0; j < 16; j++)
        {
            for (unsigned int i = 0; i < graph[j].size(); i++)
            {
                data_out[k] = j;        // početna orijentacija u i-tom čvoru
                data_out[k+1] = i;      // index segmenta koji uz danu početnu orijentaciju vodi do sljedećeg čvora na putanji
                k += 2;
            }
        }*/
    }
    else
    {
        *M = NULL;
        data_out = *M;
        sizeM[0] = 0;
        sizeM[1] = 0;
    }
}

// **************************************************************************************


#include <smtbx/refinement/constraints/geometrical_hydrogens.h>
#include <scitbx/sparse/io.h>

#include <boost/lambda/lambda.hpp>
#include <scitbx/array_family/tiny_reductions.h>

namespace smtbx { namespace refinement { namespace constraints {

  //*** Base class ***

  template <int n_hydrogens>
  index_range
  geometrical_hydrogen_sites<n_hydrogens>
  ::component_indices_for(scatterer_type const *scatterer) const
  {
    using boost::lambda::_1;
    boost::optional<std::size_t>
    i_sc = af::first_index(hydrogen, _1 == scatterer);
    return i_sc ? index_range(index() + 3*(*i_sc), 3)
                : index_range();
  }

  template <int n_hydrogens>
  void geometrical_hydrogen_sites<n_hydrogens>
  ::write_component_annotations_for(scatterer_type const *scatterer,
                                    std::ostream &output) const
  {
    using boost::lambda::_1;
    boost::optional<std::size_t>
    i_sc = af::first_index(hydrogen, _1 == scatterer);
    if (i_sc) {
      scatterer_type const *h = hydrogen[*i_sc];
      output << h->label << ".x,";
      output << h->label << ".y,";
      output << h->label << ".z,";
    }
  }

  //*** CH3, NH2, OH ***

  template <int n_hydrogens, bool staggered>
  void
  terminal_tetrahedral_xhn_sites<n_hydrogens, staggered>
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter
    *pivot           = dynamic_cast<site_parameter *>(this->argument(0)),
    *pivot_neighbour = dynamic_cast<site_parameter *>(this->argument(1));
    scalar_parameter *azimuth, *length;
    site_parameter *stagger;
    if (staggered) stagger = dynamic_cast<site_parameter *>(this->argument(2));
    else           azimuth = dynamic_cast<scalar_parameter *>(this->argument(2));
    length  = dynamic_cast<scalar_parameter *>(this->argument(3));

    // Local frame
    cart_t x_p = unit_cell.orthogonalize(pivot->value),
           x_pn = unit_cell.orthogonalize(pivot_neighbour->value);
    if (staggered) {
      cart_t x_s = unit_cell.orthogonalize(stagger->value);
      e_zero_azimuth = x_s - x_pn;
    }
    af::tiny<cart_t, 3>
    e = scitbx::math::orthonormal_basis(x_p - x_pn, e_zero_azimuth);

    double l = length->value;
    double cos_phi, sin_phi;
    if (!staggered) {
      double phi = azimuth->value;
      cos_phi = std::cos(phi);
      sin_phi = std::sin(phi);
    }
    else {
      cos_phi = -1.;
      sin_phi = 0.;
    }

    // Loop over the Hydrogen atoms
    for (int k=0; k < n_hydrogens; ++k) {

      // Cosine and Sine of the azimutal angle of the k-th Hydrogen
      /* Mathematica:
       Table[TrigExpand[Cos[\[Phi] + n Pi/3]], {n, {2, 4}}]
       Table[TrigExpand[Sin[\[Phi] + n Pi/3]], {n, {2, 4}}]
       */
      double c, s;
      switch (k) {
        case 0:
          // 1st Hydrogen: azimuthal angle = phi
          c = cos_phi;
          s = sin_phi;
          break;
        case 1:
          // 2nd Hydrogen: azimuthal angle = phi + 2pi/3
          c = -0.5        *cos_phi - half_sqrt_3*sin_phi;
          s =  half_sqrt_3*cos_phi -         0.5*sin_phi;
          break;
        case 2:
          // 3rd Hydrogen: azimuthal angle = phi + 4pi/3
          c = -0.5        *cos_phi + half_sqrt_3*sin_phi;
          s = -half_sqrt_3*cos_phi -         0.5*sin_phi;
        default:
          break;
      }

      // Site of k-th Hydrogen
      cart_t u = sin_tetrahedral_angle*(c*e[1] + s*e[2]) + e[0]/3.;
      this->x_h[k] = x_p + l*u;

      // Derivatives
      if (!jacobian_transpose) continue;
      sparse_matrix_type &jt = *jacobian_transpose;
      std::size_t const j_h = this->index() + 3*k;

      // Riding
      for (int i=0; i<3; ++i) {
        jt.col(j_h + i) = jt.col(pivot->index() + i);
      }

      /** We take advantage of the fact that azimuth and length are
          independent variables. So jt.col(azimuth->index()) is either
          zero or is a column of the identity matrix.
       */

      // Rotation
      if (!staggered && azimuth->is_variable()) {
        cart_t grad_c = l*sin_tetrahedral_angle*(-s*e[1] + c*e[2]);
        frac_t grad_f = unit_cell.fractionalize(grad_c);
        for (int i=0; i<3; ++i) jt(azimuth->index(), j_h + i) = grad_f[i];
      }

      // Bond stretching
      if (length->is_variable()) {
        frac_t grad_f = unit_cell.fractionalize(u);
        for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
      }
    }
  }

  template class terminal_tetrahedral_xhn_sites<1, /*staggered=*/false>;
  template class terminal_tetrahedral_xhn_sites<2, /*staggered=*/false>;
  template class terminal_tetrahedral_xhn_sites<3, /*staggered=*/false>;

  template class terminal_tetrahedral_xhn_sites<1, /*staggered=*/true>;
  template class terminal_tetrahedral_xhn_sites<2, /*staggered=*/true>;
  template class terminal_tetrahedral_xhn_sites<3, /*staggered=*/true>;

  // angle parameter

  void angle_parameter::linearise(uctbx::unit_cell const &unit_cell,
                         sparse_matrix_type *jacobian_transpose)
  {
    site_parameter *sites[] = {
      dynamic_cast<site_parameter *>(argument(0)),
      dynamic_cast<site_parameter *>(argument(1)),
      dynamic_cast<site_parameter *>(argument(2))
    };
    cart_t crds[] = {
      unit_cell.orthogonalize(sites[0]->value),
      unit_cell.orthogonalize(sites[1]->value),
      unit_cell.orthogonalize(sites[2]->value)
    };
    value = (crds[0]-crds[1]).angle(crds[2]-crds[1]);
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    // http://salilab.org/modeller/8v0/manual/node248.html
    cart_t ij = (crds[0] - crds[1]);
    cart_t kj = (crds[2] - crds[1]);
    const double ij_l = ij.length(), kj_l = kj.length();
    ij = ij.normalize();
    kj = kj.normalize();
    const double ca = ij*kj;
    //if( std::abs(ca) >= 1.0-1e-16 )
    const double oos = 1./std::sqrt(1-ca*ca);
    cart_t grad[3];
    grad[0] = (ij*ca - kj)*oos/ij_l;  // d(angle)/d(left)
    grad[2] = (kj*ca - ij)*oos/kj_l;  // d(angle)/d(right)
    grad[1] = -(grad[0] + grad[2]);   // d(angle)/d(center)
    for (int i=0; i < 3; i++)  {
      frac_t grad_f = unit_cell.fractionalize(grad[i]);
      for (int j=0; j < 3; j++) {
        jt(sites[i]->index()+j, index()) = grad_f[j];
      }
    }
  }

  // X-CH2-Y

  void secondary_xh2_sites::linearise(uctbx::unit_cell const &unit_cell,
                                      sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter
    *pivot             = dynamic_cast<site_parameter *>(argument(0)),
    *pivot_neighbour_0 = dynamic_cast<site_parameter *>(argument(1)),
    *pivot_neighbour_1 = dynamic_cast<site_parameter *>(argument(2));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(3));
    scalar_parameter *h_c_h = dynamic_cast<scalar_parameter *>(argument(4));

    bool is_angle_param = dynamic_cast<angle_parameter *>(argument(4)) != 0;
    // Local frame
    /* (C, e0, e1) is the bisecting plane of the angle X-C-Y
        with e0 bisecting X-C-Y
     */
    cart_t x_p  = unit_cell.orthogonalize(pivot->value);
    cart_t x_pn_0 = unit_cell.orthogonalize(pivot_neighbour_0->value),
           x_pn_1 = unit_cell.orthogonalize(pivot_neighbour_1->value);
    cart_t u_pn_0 = (x_p - x_pn_0).normalize(),
           u_pn_1 = (x_p - x_pn_1).normalize();
    cart_t e0 = (u_pn_1 + u_pn_0).normalize();
    cart_t e2 = (u_pn_1 - u_pn_0).normalize();
    cart_t e1 = e2.cross(e0);
    double l = length->value,
      theta = h_c_h->value,
      cos_phi = 0;
    const double theta_to_phi_const = 0.0698;
    // calculate new theta/2 = 0.9678 + 0.0698 cos phi as in shelxl
    if (is_angle_param) {
      cos_phi = u_pn_0*u_pn_1;
      theta = 2*(0.9678 + theta_to_phi_const*cos_phi);
    }
    // Hydrogen sites
    double c = std::cos(theta/2), s = std::sin(theta/2);
    af::tiny<cart_t, 2> u_h(c*e0 + s*e1, c*e0 - s*e1);
    for (int k=0; k<2; ++k) x_h[k] = x_p + l*u_h[k];

    // Derivatives
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    af::tiny<std::size_t, 2> j_h(index(), index() + 3);

    // Riding
    for (int k=0; k<2; ++k) {
      for (int i=0; i<3; ++i)
        jt.col(j_h[k] + i) = jt.col(pivot->index() + i);
    }

    // Bond stretching
    if (length->is_variable()) {
      for (int k=0; k<2; ++k) {
        frac_t grad_f = unit_cell.fractionalize(u_h[k]);
        for (int i=0; i<3; ++i) jt(length->index(), j_h[k] + i) = grad_f[i];
      }
    }

    // H-C-H flapping
    if (h_c_h->is_variable() && !is_angle_param) {
      af::tiny<cart_t, 2> grad_c(l/2*(-s*e0 + c*e1), l/2*(-s*e0 - c*e1));
      for (int k=0; k<2; ++k) {
        frac_t grad_f = unit_cell.fractionalize(grad_c[k]);
        for (int i=0; i<3; ++i) jt(h_c_h->index(), j_h[k] + i) = grad_f[i];
      }
    }
    else if (is_angle_param) {
      /*
      d(cos(theta/2))/d(phi) = d(cos(theta/2)/d(theta)*d(theta)/d(phi) =
        sin(theta/2)*theta_to_phi_const*sin(phi)
      d(sin(theta/2))/d(phi) = d(sin(theta/2)/d(theta)*d(theta)/d(phi) =
        cos(theta/2)*theta_to_phi_const*cos(phi)
      */
      double sin_phi = std::sqrt(1-cos_phi*cos_phi),
        k = l*theta_to_phi_const/2;
      af::tiny<cart_t, 2> grad_c(
        k*(s*sin_phi*e0 + c*cos_phi*e1),
        k*(s*sin_phi*e0 - c*cos_phi*e1));
      for (int i=0; i < 2; i++) {
        frac_t grad_f = unit_cell.fractionalize(grad_c[i]);
        for (int j=0; j < 3; j++)  {
          jt(j_h[i]+j, h_c_h->index()) = grad_f[j];
        }
      }
    }
  }

  /***    H
          |
       X0-C-X1
          |
          X2
   */
  void tertiary_xh_site::linearise(uctbx::unit_cell const &unit_cell,
                                   sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    af::tiny<site_parameter *, 3> pivot_neighbour;
    for (int k=0; k<3; ++k) {
      pivot_neighbour[k] = dynamic_cast<site_parameter *>(argument(k+1));
    }
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(4));

    // Local frame
    cart_t x_p = unit_cell.orthogonalize(pivot->value);
    af::tiny<cart_t, 3> u_cn; // Directions C->Xi
    for (int k=0; k<3; ++k) {
      cart_t x = unit_cell.orthogonalize(pivot_neighbour[k]->value);
      u_cn[k] = (x_p - x).normalize();
    }
    cart_t u = u_cn[0] - u_cn[1], v = u_cn[1] - u_cn[2];
    cart_t e0 = u.cross(v).normalize();
    if (e0*(u_cn[0] + u_cn[1] + u_cn[2]) < 0) e0 = -e0;
    double l = length->value;

    // Hydrogen site
    x_h[0] = x_p + l*e0;

    // Derivatives
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; ++i) {
      jt.col(j_h + i) = jt.col(pivot->index() + i);
    }

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(e0);
      for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  /// aromatic or amide Y-XH-Z
  void secondary_planar_xh_site::linearise(uctbx::unit_cell const &unit_cell,
                                           sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    site_parameter
    *pivot_neighbour_0 = dynamic_cast<site_parameter *>(argument(1)),
    *pivot_neighbour_1 = dynamic_cast<site_parameter *>(argument(2));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(3));

    // Local frame
    cart_t x_p = unit_cell.orthogonalize(pivot->value);
    cart_t
    u_yx = (x_p - unit_cell.orthogonalize(pivot_neighbour_0->value)).normalize(),
    u_zx = (x_p - unit_cell.orthogonalize(pivot_neighbour_1->value)).normalize();
    cart_t e0 = (u_yx + u_zx).normalize();
    double l = length->value;

    // Hydrogen site
    x_h[0] = x_p + l*e0;

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; ++i) jt.col(j_h + i) = jt.col(pivot->index() + i);

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(e0);
      for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  // Terminal Z-Y=XH2
  void terminal_planar_xh2_sites::
  linearise(uctbx::unit_cell const &unit_cell,
            sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    site_parameter
    *pivot_neighbour = dynamic_cast<site_parameter *>(argument(1));
    site_parameter
    *pivot_neighbour_substituent = dynamic_cast<site_parameter *>(argument(2));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(3));

    // Local frame
    cart_t p = unit_cell.orthogonalize(pivot->value);
    cart_t y = unit_cell.orthogonalize(pivot_neighbour->value);
    cart_t z = unit_cell.orthogonalize(pivot_neighbour_substituent->value);
    cart_t e0 = (p - y).normalize();
    cart_t u_yz = z - y;
    cart_t e1 = (e0 - 1/(e0*u_yz) * u_yz).normalize();
    double l = length->value;

    // Hydrogen sites
    af::tiny<cart_t, 2> u_h(0.5*e0 + half_sqrt_3*e1,
                            0.5*e0 - half_sqrt_3*e1);
    for (int k=0; k<2; ++k) x_h[k] = p + l*u_h[k];

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int k=0; k<2; ++k) for (int i=0; i<3; ++i) {
      jt.col(j_h + 3*k + i) = jt.col(pivot->index() + i);
    }

    // Bond stretching
    if (length->is_variable()) {
      for (int k=0; k<2; ++k) {
        frac_t grad_f = unit_cell.fractionalize(u_h[k]);
        for (int i=0; i<3; ++i) jt(length->index(), j_h + 3*k + i) = grad_f[i];
      }
    }
  }

  // Acetylenic X-CH
  void terminal_linear_ch_site
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    site_parameter *pivot_neighbour = dynamic_cast<site_parameter *>(argument(1));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(2));

    // Local frame
    cart_t p = unit_cell.orthogonalize(pivot->value);
    cart_t x = unit_cell.orthogonalize(pivot_neighbour->value);
    cart_t e0 = (p - x).normalize();
    double l = length->value;

    // Hydrogen site
    x_h[0] = p + l*e0;

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; ++i) jt.col(j_h + i) = jt.col(pivot->index() + i);

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(e0);
      for (int i=0; i<3; ++i) jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  // boron cage B(n)-H
  void polyhedral_bh_site
  ::linearise(uctbx::unit_cell const &unit_cell,
              sparse_matrix_type *jacobian_transpose)
  {
    using namespace constants;
    site_parameter *pivot = dynamic_cast<site_parameter *>(argument(0));
    scalar_parameter *length = dynamic_cast<scalar_parameter *>(argument(1));
    const cart_t c = unit_cell.orthogonalize(pivot->value);
    cart_t p(0,0,0);
    for (int i=2; i<n_arguments(); i++)  {
      p = p + (unit_cell.orthogonalize(
        dynamic_cast<site_parameter *>(argument(i))->value) - c).normalize();
    }
    p = -p.normalize();
    double l = length->value;
    // Hydrogen site
    x_h[0] = c + p*l;

    // Jacobian
    if (!jacobian_transpose) return;
    sparse_matrix_type &jt = *jacobian_transpose;
    std::size_t j_h = index();

    // Riding
    for (int i=0; i<3; i++)
      jt.col(j_h + i) = jt.col(pivot->index() + i);

    // Bond stretching
    if (length->is_variable()) {
      frac_t grad_f = unit_cell.fractionalize(p);
      for (int i=0; i<3; i++)
        jt(length->index(), j_h + i) = grad_f[i];
    }
  }

  // Explicitly instantiate template classes used in other libraries
  template class geometrical_hydrogen_sites<1>;
  template class geometrical_hydrogen_sites<2>;
  template class geometrical_hydrogen_sites<3>;
}}}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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_TRANSFORM_INVERSE_TRANSFORMER_HPP
#define BOOST_GEOMETRY_STRATEGIES_TRANSFORM_INVERSE_TRANSFORMER_HPP

// Remove the ublas checking, otherwise the inverse might fail
// (while nothing seems to be wrong)
#define BOOST_UBLAS_TYPE_CHECK 0

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/geometry/strategies/transform/matrix_transformers.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace transform
{

/*!
\brief Transformation strategy to do an inverse ransformation in Cartesian system
\ingroup strategies
\tparam P1 first point type
\tparam P2 second point type
 */
template <typename P1, typename P2>
class inverse_transformer
    : public ublas_transformer<P1, P2, dimension<P1>::type::value, dimension<P2>::type::value>
{
    typedef typename select_coordinate_type<P1, P2>::type T;

public :
    template <typename Transformer>
    inline inverse_transformer(Transformer const& input)
    {
        typedef boost::numeric::ublas::matrix<T> matrix_type;

        // create a working copy of the input
        matrix_type copy(input.matrix());

        // create a permutation matrix for the LU-factorization
        typedef boost::numeric::ublas::permutation_matrix<> permutation_matrix;
        permutation_matrix pm(copy.size1());

        // perform LU-factorization
        int res = boost::numeric::ublas::lu_factorize<matrix_type>(copy, pm);
        if( res == 0 )
        {
            // create identity matrix
            this->m_matrix.assign(boost::numeric::ublas::identity_matrix<T>(copy.size1()));

            // backsubstitute to get the inverse
            boost::numeric::ublas::lu_substitute(copy, pm, this->m_matrix);
        }
    }

};

}} // namespace strategy::transform

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_TRANSFORM_INVERSE_TRANSFORMER_HPP


#if defined(_USE_EIGEN_)
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <cassert>
#include <vector>
#include <list>
#include "zeigenutils.h"

using namespace Eigen;
using namespace std;

ZMesh::ZMesh()
{
}

bool ZMesh::convertAllenAtlasMeshToWavefrontObj(const char* iFile, const char* oFile)
{
  Matrix<float, Dynamic, Dynamic, RowMajor> normals;
  Matrix<float, Dynamic, Dynamic, RowMajor> vertices;
  Matrix<int, Dynamic, Dynamic, RowMajor> triangles;
  if (!readAllenAtlasMesh(iFile, normals, vertices, triangles))
    return false;
  if (!writeWavefrontObj(oFile, normals, vertices, triangles))
    return false;
  return true;
}

bool ZMesh::convertAllenAtlasMeshToHxSurface(const char *iFile, const char *oFile)
{
  Matrix<float, Dynamic, Dynamic, RowMajor> normals;
  Matrix<float, Dynamic, Dynamic, RowMajor> vertices;
  Matrix<int, Dynamic, Dynamic, RowMajor> triangles;
  if (!readAllenAtlasMesh(iFile, normals, vertices, triangles))
    return false;
  if (!writeHxSurface(oFile, vertices, triangles))
    return false;
  return true;
}

bool ZMesh::convertAllenAtlasMeshToHxSurface(const QString &iFile, const char *oFile)
{
  return convertAllenAtlasMeshToHxSurface(iFile.toLocal8Bit().constData(), oFile);
}

bool ZMesh::convertWavefrontObjToHxSurface(const char *iFile, const char *oFile)
{
  Matrix<float, Dynamic, Dynamic, RowMajor> normals;
  Matrix<float, Dynamic, Dynamic, RowMajor> vertices;
  Matrix<int, Dynamic, Dynamic, RowMajor> triangles;
  if (!readWavefrontObj(iFile, normals, vertices, triangles))
    return false;
  if (!writeHxSurface(oFile, vertices, triangles))
    return false;
  return true;
}

bool ZMesh::convertWavefrontObjToHxSurface(const QString &iFile, const char *oFile)
{
  return convertWavefrontObjToHxSurface(iFile.toLocal8Bit().constData(), oFile);
}

bool ZMesh::readAllenAtlasMesh(const char *iFile, Matrix<float, Dynamic, Dynamic, RowMajor> &normals,
                               Matrix<float, Dynamic, Dynamic, RowMajor> &vertices,
                               Matrix<int, Dynamic, Dynamic, RowMajor> &triangles)
{
  // sizeof int and sizeof float should be 4, otherwise fail
  assert(sizeof(int)==4);
  assert(sizeof(float)==4);
  if (sizeof(int) != 4 || sizeof(float) != 4)
    return false;

  std::ifstream inputFileStream;
  inputFileStream.open(iFile, std::ios::in | std::ios::binary);
  if (!inputFileStream.is_open()) {
    std::perror((std::string("readAllenAtlasMesh: error while opening file ") + iFile).c_str());
    return false;
  }

  int numPoints;
  inputFileStream.read((char*)&numPoints, 4);

  normals.resize(numPoints, 3);
  vertices.resize(numPoints, 3);

  //read points
  // 3 floats for normals followed by 3 floats for coordinates per point
  float normal[3];
  float vertice[3];
  for (int i=0; i<numPoints; i++) {
    inputFileStream.read((char*)normal, 3*4);
    normals.row(i) << normal[0], normal[1], normal[2];
    inputFileStream.read((char*)vertice, 3*4);
    vertices.row(i) << vertice[0], vertice[1], vertice[2];
  }

  int numTriangleStrips = 0;
  inputFileStream.read((char*)&numTriangleStrips, 4);
  std::vector<std::vector<int> > allStrips;

  // read triangle strips
  for (int i=0; i<numTriangleStrips; i++) {
    //read number of points in the strip
    int numPointsInStrip = 0;
    inputFileStream.read((char*)&numPointsInStrip, 2);

    if (numPointsInStrip > 0) {
      std::vector<int> strip(numPointsInStrip);
      inputFileStream.read((char*)strip.data(), numPointsInStrip*4);
      allStrips.push_back(strip);
    }
  }
  inputFileStream.close();

  int numTriangles = 0;
  for (int i=0; i<numTriangleStrips; i++) {
    numTriangles += allStrips[i].size() - 2;
  }
  triangles.resize(numTriangles, 3);

  int triIdx = 0;
  for (int i=0; i<numTriangleStrips; i++) {
    for (size_t j = 0; j < allStrips[i].size()-2; j++) {
      //Indicies in the triStripIndices are like: ABCDEFG
      //We need to change them to be ABC CBD CDE EDF EFG(note swapping of ordering)
      if (j%2 == 0) {
        triangles.row(triIdx++) << allStrips[i][j]+1, allStrips[i][j+1]+1, allStrips[i][j+2]+1;
      } else {
        triangles.row(triIdx++) << allStrips[i][j+1]+1, allStrips[i][j]+1, allStrips[i][j+2]+1;
      }
    }
  }
  return true;
}

bool ZMesh::readAllenAtlasMesh(const QStringList &iFiles, Matrix<float, Dynamic, Dynamic, RowMajor> &normals,
                               Matrix<float, Dynamic, Dynamic, RowMajor> &vertices,
                               Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &triangles, QList<int> &pointSizes, QList<int> &triangleSizes)
{
  if (iFiles.size() == 0)
    return false;
  pointSizes.clear();
  triangleSizes.clear();
  bool ok;
  for (int i=0; i<iFiles.size(); i++) {
    if (i==0) {
      ok = readAllenAtlasMesh(iFiles[i].toLocal8Bit().constData(), normals, vertices, triangles);
      if (!ok)
        return false;
      pointSizes.push_back(vertices.rows());
      triangleSizes.push_back(triangles.rows());
    } else {
      Matrix<float, Dynamic, Dynamic, RowMajor> currentNormals;
      Matrix<float, Dynamic, Dynamic, RowMajor> currentVertices;
      Matrix<int, Dynamic, Dynamic, RowMajor> currentTriangles;
      ok = readAllenAtlasMesh(iFiles[i].toLocal8Bit().constData(), currentNormals, currentVertices, currentTriangles);
      if (!ok)
        return false;
      pointSizes.push_back(vertices.rows());
      triangleSizes.push_back(currentTriangles.rows());
      int pointStartIdx = vertices.rows();
      currentTriangles.array() += pointStartIdx;
      normals.conservativeResize(normals.rows()+currentNormals.rows(), NoChange);
      vertices.conservativeResize(vertices.rows()+currentVertices.rows(), NoChange);
      triangles.conservativeResize(triangles.rows()+currentTriangles.rows(), NoChange);
      normals.bottomRows(currentNormals.rows()) = currentNormals;
      vertices.bottomRows(currentVertices.rows()) = currentVertices;
      triangles.bottomRows(currentTriangles.rows()) = currentTriangles;
    }
  }
  return true;
}

bool ZMesh::readWavefrontObj(const char *filename, Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &normals,
                             Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &vertices,
                             Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &triangles)
{
  ifstream inputFileStream;
  inputFileStream.open(filename, ios::in);
  if (!inputFileStream.is_open()) {
    perror((string("readWavefrontObj: error while opening file ") + filename).c_str());
    return false;
  }

  string line;
  int numPoints = 0;
  int numNormals = 0;
  int numTriangles = 0;

  while (getline(inputFileStream, line)) {
    //skip empty lines
    if (line.find_first_not_of(" \t\r") == string::npos) {
      continue;
    }
    line  = line.substr(line.find_first_not_of(" \t\r"));
    if (line[0] == '#')
      continue;
    else if (line[0] == 'v' && line[1] == 'n') {
      numNormals++;
    } else if (line[0] == 'v') {
      numPoints++;
    } else if (line[0] == 'f') {
      numTriangles++;
    }
  }
  if (inputFileStream.bad()) {
    std::perror((string("readWavefrontObj: error while reading file ") + filename).c_str());
    inputFileStream.close();
    return false;
  }
  if (numNormals > 0 && numPoints != numNormals) {
    cerr << "readWavefrontObj: can not read this file (normal number not equal to vertices number)" << endl;
    inputFileStream.close();;
    return false;
  }

  inputFileStream.clear();
  inputFileStream.seekg(0, std::ios::beg);
  normals.resize(numNormals, 3);
  vertices.resize(numPoints, 3);
  triangles.resize(numTriangles, 3);

  int normalIdx = 0;
  int verticeIdx = 0;
  int triangleIdx = 0;

  while (getline(inputFileStream, line)) {
    //skip empty lines
    if (line.find_first_not_of(" \t\r") == string::npos) {
      continue;
    }
    line  = line.substr(line.find_first_not_of(" \t\r"));
    if (line[0] == '#')
      continue;
    else if (line[0] == 'v' && line[1] == 'n') {
      int nData;
      RowVectorXd rowVector = ZEigenUtils::readRowVector(line, "vn", &nData, 3);
      if (nData != 3) {
        cerr << "readWavefrontObj: wrong file content (vn)" << endl;
        inputFileStream.close();;
        return false;
      } else {
        normals.row(normalIdx++) = rowVector.cast<float>();
      }
    } else if (line[0] == 'v') {
      int nData;
      RowVectorXd rowVector = ZEigenUtils::readRowVector(line, "v", &nData, 3);
      if (nData != 3) {
        cerr << "readWavefrontObj: wrong file content (v)" << endl;
        inputFileStream.close();;
        return false;
      } else {
        vertices.row(verticeIdx++) = rowVector.cast<float>();
      }
    } else if (line[0] == 'f') {
      int nData;
      RowVectorXd rowVector = ZEigenUtils::readRowVector(line, "f/", &nData, 6);
      if (nData != 6) {
        cerr << "readWavefrontObj: wrong file content (f)" << endl;
        inputFileStream.close();;
        return false;
      } else {
        triangles.row(triangleIdx++) << (int)rowVector(0), (int)rowVector(2), (int)rowVector(4);
      }
    }
  }
  if (inputFileStream.bad()) {
    std::perror((std::string("readWavefrontObj: error while reading file ") + filename).c_str());
    inputFileStream.close();
    return false;
  }

  inputFileStream.close();
  return true;
}

bool ZMesh::readWavefrontObj(const QStringList &iFiles, Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &normals,
                             Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &vertices,
                             Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &triangles,
                             QList<int> &pointSizes, QList<int> &triangleSizes)
{
  if (iFiles.size() == 0)
    return false;
  pointSizes.clear();
  triangleSizes.clear();
  bool ok;
  for (int i=0; i<iFiles.size(); i++) {
    if (i==0) {
      ok = readWavefrontObj(iFiles[i].toLocal8Bit().constData(), normals, vertices, triangles);
      if (!ok)
        return false;
      pointSizes.push_back(vertices.rows());
      triangleSizes.push_back(triangles.rows());
    } else {
      Matrix<float, Dynamic, Dynamic, RowMajor> currentNormals;
      Matrix<float, Dynamic, Dynamic, RowMajor> currentVertices;
      Matrix<int, Dynamic, Dynamic, RowMajor> currentTriangles;
      ok = readWavefrontObj(iFiles[i].toLocal8Bit().constData(), currentNormals, currentVertices, currentTriangles);
      if (!ok)
        return false;
      pointSizes.push_back(vertices.rows());
      triangleSizes.push_back(currentTriangles.rows());
      int pointStartIdx = vertices.rows();
      currentTriangles.array() += pointStartIdx;
      normals.conservativeResize(normals.rows()+currentNormals.rows(), NoChange);
      vertices.conservativeResize(vertices.rows()+currentVertices.rows(), NoChange);
      triangles.conservativeResize(triangles.rows()+currentTriangles.rows(), NoChange);
      normals.bottomRows(currentNormals.rows()) = currentNormals;
      vertices.bottomRows(currentVertices.rows()) = currentVertices;
      triangles.bottomRows(currentTriangles.rows()) = currentTriangles;
    }
  }
  return true;
}

bool ZMesh::writeWavefrontObj(const char *oFile, const Matrix<float, Dynamic, Dynamic, RowMajor> &normals,
                              const Matrix<float, Dynamic, Dynamic, RowMajor> &vertices,
                              const Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &triangles)
{
  std::ofstream outputFileStream(oFile);
  if (!outputFileStream.is_open()) {
    std::perror((std::string("writeWavefrontObj: error while opening file ") + oFile).c_str());
    return false;
  }

  // write vertices
  for (int i=0; i<vertices.rows(); i++) {
    outputFileStream << "v " << vertices.row(i) << std::endl;
  }
  // write normals
  for (int i=0; i<normals.rows(); i++) {
    outputFileStream << "vn " << normals.row(i) << std::endl;
  }
  // write triangles
  for (int i=0; i<triangles.rows(); i++) {
    outputFileStream << "f " << triangles(i,0) << "//" << triangles(i,0) << " "
                     << triangles(i,1) << "//" << triangles(i,1) << " "
                     << triangles(i,2) << "//" << triangles(i,2) << std::endl;
  }
  outputFileStream.close();
  return true;
}

bool ZMesh::writeHxSurface(const char *oFile,
                           const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &vertices,
                           const Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &triangles,
                           const QList<int> &triangleSizes)
{
  std::ofstream outputFileStream(oFile);
  if (!outputFileStream.is_open()) {
    std::perror((std::string("writeHxSurface: error while opening file ") + oFile).c_str());
    return false;
  }
  outputFileStream << "# HyperSurface ASCII" << endl << endl;
  outputFileStream << "Parameters {\n    Info \"3 colors\"\n}" << endl << endl;
  outputFileStream << "Materials { {\n    color color 0.83562 0.78 0.06,\n    Name \"Yellow\" }\n{\n    color 0.21622 0.8 0.16,\n    name \"Green\" }\n{\n    color 0.8 0.16 0.596115,\n    name \"Magenta\" }\n}" << endl << endl;

  outputFileStream << "Vertices " << vertices.rows() << endl;
  for (int i=0; i<vertices.rows(); i++) {
    outputFileStream << "    " << vertices.row(i) << endl;
  }
  outputFileStream << endl;
  outputFileStream << "Patches " << triangleSizes.size() << endl;
  int idx = 0;
  for (int i=0; i<triangleSizes.size(); i++) {
    if (i%3 == 0) {
      outputFileStream << "{   InnerRegion Green\n    OuterRegion Yellow\n    Triangles " << triangleSizes[i] << endl;
    } else if (i%3 == 1) {
      outputFileStream << "{   InnerRegion Magenta\n    OuterRegion Yellow\n    Triangles " << triangleSizes[i] << endl;
    } else if (i%3 == 2) {
      outputFileStream << "{   InnerRegion Magenta\n    OuterRegion Green\n    Triangles " << triangleSizes[i] << endl;
    }

    for (int j=0; j<triangleSizes[i]; j++) {
      outputFileStream << "       " << triangles.row(idx++) << endl;
    }

    outputFileStream << "}" << endl;
  }
  return true;
}

bool ZMesh::writeHxSurface(const char *oFile, const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &vertices,
                           const Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &triangles)
{
  QList<int> triangleSizes;
  triangleSizes.push_back(triangles.rows());
  return writeHxSurface(oFile, vertices, triangles, triangleSizes);
}

bool ZMesh::convertAllenAtlasMeshToWavefrontObj(const QStringList &iFiles, const QString &oFile)
{
  Matrix<float, Dynamic, Dynamic, RowMajor> normals;
  Matrix<float, Dynamic, Dynamic, RowMajor> vertices;
  Matrix<int, Dynamic, Dynamic, RowMajor> triangles;
  QList<int> pointSizes;
  QList<int> triangleSizes;
  if (!readAllenAtlasMesh(iFiles, normals, vertices, triangles, pointSizes, triangleSizes))
    return false;
  if (!writeWavefrontObj(oFile.toLocal8Bit().constData(), normals, vertices, triangles))
    return false;
  return true;
}

bool ZMesh::convertAllenAtlasMeshToHxSurface(const QStringList &iFiles, const QString &oFile)
{
  Matrix<float, Dynamic, Dynamic, RowMajor> normals;
  Matrix<float, Dynamic, Dynamic, RowMajor> vertices;
  Matrix<int, Dynamic, Dynamic, RowMajor> triangles;
  QList<int> pointSizes;
  QList<int> triangleSizes;
  if (!readAllenAtlasMesh(iFiles, normals, vertices, triangles, pointSizes, triangleSizes))
    return false;
  if (!writeHxSurface(oFile.toLocal8Bit().constData(), vertices, triangles, triangleSizes))
    return false;
  return true;
}
#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_DIVCEIL_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_DIVCEIL_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd {
  namespace tag
  {
    /*!
      @brief  divceil generic tag

      Represents the divceil function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct divceil_ : ext::elementwise_<divceil_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<divceil_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_divceil_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::divceil_, Site> dispatching_divceil_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::divceil_, Site>();
    }
    template<class... Args>
    struct impl_divceil_;
  }
  /*!
    Computes the ceil of the division of its parameters.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divceil(x, y);
    @endcode

    For floating point values the code is equivalent to:

    @code
    T r = ceil(x/y);
    @endcode

    for integral types, if y is null, it returns Valmax (resp. Valmin)
    if x is positive (resp. negative), and 0 if x is null.
    Take care also that dividing Valmin by -1 for signed integral types has
    undefined behaviour.

    @see  @funcref{divides}, @funcref{fast_divides}, @funcref{rec}, @funcref{fast_rec}, @funcref{divs}, @funcref{divfloor},
    @funcref{divround}, @funcref{divround2even}, @funcref{divfix}
    @param  a0
    @param  a1

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::divceil_, divceil, 2)
} }

#endif


#include <iostream>

#include <armadillo>

#include <glm/models/binomial.hpp>
#include <glm/models/normal.hpp>
#include <cpp-argparse/OptionParser.h>

#include <besiq/method/glm_method.hpp>
#include <besiq/method/method.hpp>

#include "common_options.hpp"

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-glm [OPTIONS] pairs genotype_plink_prefix";
const std::string DESCRIPTION = "Generalized linear models for genetic interactions.";

int
main(int argc, char *argv[])
{
    OptionParser parser = create_common_options( USAGE, DESCRIPTION, true );

    char const* const model_choices[] = { "binomial", "normal" };
    char const* const link_choices[] = { "logit", "logc", "odds", "identity", "log" };
    char const* const factor_choices[] = { "factor", "additive", "tukey", "noia" };

    OptionGroup group = OptionGroup( parser, "Options for glm", "These options will change the behavior of the glm model." );
    group.add_option( "-m", "--model" ).choices( &model_choices[ 0 ], &model_choices[ 2 ] ).metavar( "model" ).help( "The model to use for the phenotype, 'binomial' or 'normal', default = 'binomial'." ).set_default( "binomial" );
    group.add_option( "-l", "--link-function" ).choices( &link_choices[ 0 ], &link_choices[ 5 ] ).metavar( "link" ).help( "The link function, or scale, that is used for the penetrance: 'logit' log(p/(1-p)), 'logc' log(1 - p), 'odds' p/(1-p), 'identity' p, 'log' log(p)." );
    group.add_option( "-f", "--factor" ).choices( &factor_choices[ 0 ], &factor_choices[ 4 ] ).help( "Determines how to code the SNPs, in 'factor' no order of the alleles is assumed, in 'additive' the SNPs are coded as the number of minor alleles, in 'tukey' the coding is the same as factor except that a single parameter for the interaction is used, 'noia' the model is divided into additive and dominance interactions." ).set_default( "factor" );
    group.add_option( "--fast" ).help( "Faster but less robust matrix inversion, generally a speedup of 2 can be expected." ).action( "store_true" );
    parser.add_option_group( group );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 2 )
    {
        parser.print_help( );
        exit( 1 );
    }
    shared_ptr<common_options> parsed_data = parse_common_options( options, parser.args( ) );
    parsed_data->data->phenotype.elem( arma::find_nonfinite( parsed_data->data->phenotype ) ).zeros( );
    parsed_data->data->fast_inversion = options.is_set( "fast" );

    model_matrix *model_matrix = make_model_matrix( options[ "factor" ], parsed_data->data->covariate_matrix, parsed_data->data->phenotype.n_elem );

    method_type *m = NULL;
    if( options[ "model" ] == "binomial" )
    {
        std::string link = "logit";
        if( options.is_set( "link_function" ) )
        {
            link = options[ "link_function" ];
        }

        binomial *model = new binomial( link );
        m = new glm_method( parsed_data->data, *model, *model_matrix );
    }
    else if( options[ "model" ] == "normal" )
    {
        std::string link = "identity";
        if( options.is_set( "link_function" ) )
        {
            link = options[ "link_function" ];
        }

        normal *model = new normal( link );
        m = new glm_method( parsed_data->data, *model, *model_matrix );
    }

    run_method( *m, parsed_data->genotypes, *parsed_data->pairs, *parsed_data->result_file );

    delete m;
    delete model_matrix;

    return 0;
}


/// test
#include <vector>
#include <cassert>

#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

int main(){
Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(100, 0, 1);
std::cout << x << std::endl;
std::cout <<1e4<< std::endl;
}


#include <ros/ros.h>
#include <robot_node/RelRotate.h>
#include <robot_node/Brake.h>
#include <robot_node/RelTranslate.h>
#include <robot_node/Follow_Wall.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <iostream>
#include <limits>
#include <utility>
#include <cmath>
#include <boost/bind.hpp>
#include "robotSim.h"

robotSim::robotSim(std::string& name, int id) {
  this->id = id;

  double dy_kp; double dy_ki; double dy_kd; double dy_signal;
  double ang_vel_kp; double ang_vel_ki; double ang_vel_kd; double ang_vel_signal;

  nh.getParam("/" + name + "/dy_kp", dy_kp);
  nh.getParam("/" + name + "/dy_ki", dy_ki);
  nh.getParam("/" + name + "/dy_kd", dy_kd);
  nh.getParam("/" + name + "/dy_signal", dy_signal);

  nh.getParam("/" + name + "/ang_vel_kp", ang_vel_kp);
  nh.getParam("/" + name + "/ang_vel_ki", ang_vel_ki);
  nh.getParam("/" + name + "/ang_vel_kd", ang_vel_kd);
  nh.getParam("/" + name + "/ang_vel_signal", ang_vel_signal);

  dy_control = new PIDController(dy_signal, dy_kp, dy_kd, dy_ki);
  ang_vel_control = new PIDController(ang_vel_signal, ang_vel_kp, ang_vel_kd, ang_vel_ki);

  this->relTranSrv = nh.advertiseService("/indoor/relative_translate", &robotSim::relTranslate, this);
  this->relRotSrv = nh.advertiseService("/indoor/relative_rotate", &robotSim::relRotate, this);
  this->brakeSrv = nh.advertiseService("/indoor/brake", &robotSim::stop, this);

  this->odomSub = nh.subscribe("/indoor/gvg/odom_combined", 1, &robotSim::handleOdometry, this);
  this->wfSub   = nh.subscribe("/indoor/follow_wall", 1, &robotSim::follow_wall, this);
  this->movePub = nh.advertise<geometry_msgs::Twist>("/indoor/cmd_vel", 1);
}

robotSim::~robotSim() {
  this->relTranSrv.shutdown();
  this->relRotSrv.shutdown();
  this->brakeSrv.shutdown();
}

/* Motion command: tVel, the translational velocity is in m/sec
   and aVel, the angular velocity is in rad/sec */
void robotSim::move(double tVel, double aVel) {
  geometry_msgs::Twist msg;
  msg.linear.x = tVel;
  msg.linear.y = 0;
  msg.linear.z = 0;
  msg.angular.x = 0;
  msg.angular.y = 0;
  msg.angular.z = aVel;
  movePub.publish(msg);
}

/* Accepts dy, dtheta as errors and drives the robot to a direction
   that will minimize those errors */
void robotSim::follow_wall(const robot_node::Follow_Wall::ConstPtr& msg) {

  static int innerLoop = 0;
  int N = 2;
  if (innerLoop % N == 0) { // execute the outer loop in the cascading control
    dy_control->update(msg->dy_in_m);
  } else {                  // execute the inner loop
    ang_vel_control->update((dy_control->signal * M_PI/180.0) -(-msg->dtheta_in_rad));
  }
  this->move(msg->lin_vel, ang_vel_control->signal);
  innerLoop = (innerLoop + 1) % N;

}

/* Move forward by dx_in_m meters */
bool robotSim::relTranslate(robot_node::RelTranslate::Request& req,
			      robot_node::RelTranslate::Response& res) {

  geometry_msgs::Point before = odom.pose.pose.position;
  geometry_msgs::Point after = odom.pose.pose.position;
   ros::Rate loop_rate(10);
  while (distance(before, after) < req.dx_in_m) {
    move(req.lin_speed, 0.0);
    ros::spinOnce();
    loop_rate.sleep();
    after = odom.pose.pose.position;
  }

  res.success = true;
  return true;
}

/* TODO: bug fix, relrotate can only handle < 90 degree turns. Also ang velocity should be recalculated depending on the angle */
bool robotSim::relRotate(robot_node::RelRotate::Request& req,
			   robot_node::RelRotate::Response& res) {

  double ang_vel = req.dtheta_rad >= 0 ? std::abs(req.ang_speed) : (-std::abs(req.ang_speed));
  ros::Duration dt(std::abs(req.dtheta_rad)/std::abs(req.ang_speed));
  ros::Time start = ros::Time::now();
  ros::Rate loop_rate(10);
  while (ros::Time::now() < start + dt) {
    move(0.0, ang_vel);
    ros::spinOnce();
    loop_rate.sleep();
  }

  res.success = true;
  return true;
}

/* Stop the robot */
bool robotSim::stop(robot_node::Brake::Request& req,
		      robot_node::Brake::Response& res) {
  this->move(0, 0);
  res.success = true;
  return true;
}

/* Reads up the odometry of the robot from Stage  */
void robotSim::handleOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
  this->odom = *msg;
}

double robotSim::distance(geometry_msgs::Point& a, geometry_msgs::Point& b) {
  double distSqr = (a.x - b.x)*(a.x - b.x) + (a.y - b.y)*(a.y - b.y);
  return sqrt(distSqr);
}


/*
Pharmit
Copyright (c) David Ryan Koes, University of Pittsburgh and contributors.
All rights reserved.

Pharmit is licensed under both the BSD 3-clause license and the GNU
Public License version 2. Any use of the code that retains its reliance
on the GPL-licensed OpenBabel library is subject to the terms of the GPL2.

Use of the Pharmit code independently of OpenBabel (or any other
GPL2 licensed software) may choose between the BSD or GPL licenses.

See the LICENSE file provided with the distribution for more information.

*/
/*
 * MappableOctTree.cpp
 *
 *  Created on: Oct 13, 2011
 *      Author: dkoes
 */

#include "MappableOctTree.h"
#include "MiraObject.h"
#include <cstring>
#include <cassert>
#include <boost/multi_array.hpp>

using namespace boost;

MappableOctTree* MappableOctTree::clone() const
{
	unsigned sz = bytes();
	void* mem = malloc(sz);
	memcpy(mem, this, sz);
	return (MappableOctTree*) mem;
}

//return volume of this node given that the full volume of the cube is dim3
//to save space while keeping most performance, volume is cached in octnodes
float MChildNode::volume(const MOctNode* tree, float dim3) const
		{
	if (isLeaf)
	{
		return leaf.numbits * dim3 / 8;
	}
	else
	{
		return tree[node.index].vol;
	}
}

//volume version for tree under construction
float MChildNode::volume(const vector<MOctNode>& tree, float dim3) const
		{
	if (isLeaf)
	{
		return leaf.numbits * dim3 / 8;
	}
	else
	{
		return tree[node.index].vol;
	}
}

MChildNode MappableOctTree::createFrom_r(unsigned N, MChildNode* nodes,
		const MappableOctTree** trees, vector<MOctNode>& newtree, bool isUnion,
		float cubeVol)
{
	float bitvol = cubeVol / 8;
	unsigned numFilled = 0;
	unsigned andpat = 0xff, orpat = 0;
	for (unsigned i = 0; i < N; i++)
	{
		if (nodes[i].isLeaf)
		{
			numFilled++;
			andpat &= nodes[i].leaf.pattern;
			orpat |= nodes[i].leaf.pattern;
		}
	}

	if (isUnion && (numFilled == N || orpat == 0xff)) //just return a patterned node
	{
		unsigned nb = __builtin_popcount(orpat);
		MChildNode ret(orpat, nb);
		return ret;
	}
	else if (!isUnion && (numFilled == N || andpat == 0))
	{
		unsigned nb = __builtin_popcount(andpat);
		MChildNode ret(andpat, nb);
		return ret;
	}
	else //need to look at non-leaf children
	{
		unsigned pos = newtree.size();
		MChildNode ret(pos);
		newtree.push_back(MOctNode());

		unsigned numFullEmpty = 0;
		unsigned newPattern = 0;
		MChildNode nextnodes[N + 1];
		const MappableOctTree *nexttrees[N + 1];

		//setup nonleaf trees
		unsigned n = 0;
		for (unsigned i = 0; i < N; i++)
		{
			if (!nodes[i].isLeaf)
			{
				nexttrees[n] = trees[i];
				n++;
			}
		}
		if (numFilled > 0)
			nexttrees[n++] = NULL; //dummy for merge of leaves
		for (unsigned i = 0; i < 8; i++)
		{
			//create array of sub-octants from non-leafs
			unsigned cnt = 0;
			for (unsigned j = 0; j < N; j++)
			{
				if (!nodes[j].isLeaf)
				{
					nextnodes[cnt] =
							trees[j]->tree[nodes[j].node.index].children[i];
					cnt++;
				}
			}
			//summerize filled nodes
			if (numFilled > 0)
			{
				unsigned pat = isUnion ? orpat : andpat;
				if (pat & (1 << i))
					pat = 0xff;
				else
					pat = 0;
				nextnodes[cnt] = MChildNode(pat, pat ? 8 : 0);
				cnt++;
			}
			assert(cnt == n);

			MChildNode nchild = createFrom_r(n, nextnodes, nexttrees, newtree,
					isUnion, bitvol);
			newtree[pos].children[i] = nchild;

			if (nchild.isLeaf
					&& (nchild.leaf.pattern == 0 || nchild.leaf.pattern == 0xff))
			{
				numFullEmpty++;
				if (nchild.leaf.pattern == 0xff)
					newPattern |= 1 << i;
			}

			newtree[pos].vol += nchild.volume(newtree, bitvol);
		}

		if (numFullEmpty == 8)
		{
			newtree.pop_back();
			ret.isLeaf = true;
			ret.leaf.pattern = newPattern;
			ret.leaf.numbits = __builtin_popcount(newPattern);
			//volume is correct
		}
		return ret;
	}
}

//invert tree, this can be done inplace
void MappableOctTree::invert()
{
	float cubeVol = dimension * dimension * dimension;
	float expectedV = cubeVol - root.volume(tree, cubeVol);
	root.invert(tree, cubeVol);
	assert(root.volume(tree, cubeVol) == expectedV);
}

MappableOctTree* MappableOctTree::newFromVector(const vector<MOctNode>& newtree,
		const MChildNode& newroot, float dim)
{
	unsigned sz = sizeof(MappableOctTree) + newtree.size() * sizeof(MOctNode);
	void *mem = malloc(sz);
	return new (mem) MappableOctTree(dim, newroot, newtree);
}

//union
MappableOctTree* MappableOctTree::createFromUnion(unsigned N,
		const MappableOctTree** in)
{
	MChildNode roots[N];
	float dim = 0;
	for (unsigned i = 0; i < N; i++)
	{
		roots[i] = in[i]->root;
		if (i == 0)
			dim = in[i]->dimension;
		else
			assert(dim == in[i]->dimension);
	}
	vector<MOctNode> newtree;
	MChildNode newroot = createFrom_r(N, roots, in, newtree, true,
			dim * dim * dim);

	return newFromVector(newtree, newroot, dim);
}

//mutually recurse the N trees, round up/down nodes iff doing so will not
//change the local equality relationship between the N trees
bool MappableOctTree::createRoundedSet_r(unsigned N, MChildNode* nodes,
		const MappableOctTree** trees, bool roundUp,
		vector<vector<MOctNode> >& newtrees, vector<MChildNode>& newroots,
		float cubeVol)
{
	unsigned numFilled = 0;
	unsigned numFull = 0;
	unsigned numEmpty = 0;
	int lastPat = -1;
	bool differentPats = false;
	float bitvol = cubeVol / 8;
	for (unsigned i = 0; i < N; i++)
	{
		if (nodes[i].isLeaf)
		{
			numFilled++;

			if (nodes[i].leaf.pattern == 0xff)
				numFull++;
			else if (nodes[i].leaf.pattern == 0)
				numEmpty++;
			else if (lastPat == -1)
			{
				lastPat = nodes[i].leaf.pattern;
			}
			else if (nodes[i].leaf.pattern != lastPat)
				differentPats = true;
		}
	}

	if (numFilled == N) //all leaves
	{
		if (roundUp)
		{
			//can only round up equivalent leaves if remaining leaves are empty
			if (!differentPats && lastPat > 0 && numFull == 0)
			{
				for (unsigned i = 0; i < N; i++)
				{
					if (nodes[i].leaf.pattern == 0)
						newroots[i] = nodes[i];
					else
						newroots[i] = MChildNode(0xff, 8);
				}
				return true;
			}
			else //just copy
			{
				for (unsigned i = 0; i < N; i++)
				{
					newroots[i] = nodes[i];
				}
				return false;
			}
		}
		else //round down
		{
			//leaves must be equivalent or full
			if (!differentPats && lastPat > 0 && numEmpty == 0)
			{
				for (unsigned i = 0; i < N; i++)
				{
					if (nodes[i].leaf.pattern == 0)
						newroots[i] = nodes[i];
					else
						newroots[i] = MChildNode(0, 0);
				}
				return true;
			}
			else //just duplicate
			{
				for (unsigned i = 0; i < N; i++)
				{
					newroots[i] = nodes[i];
				}
				return false;
			}
		}
	}
	else //must descend non-leaf tree parts
	{
		//create new octnodes for all non-leaf trees
		unsigned positions[N];
		for (unsigned i = 0; i < N; i++)
		{
			if (nodes[i].isLeaf)
			{
				//stays a leaf
				newroots[i] = nodes[i];
			}
			else
			{
				unsigned pos = newtrees[i].size();
				positions[i] = pos;
				newroots[i] = MChildNode(pos);
				newtrees[i].push_back(MOctNode());
			}
		}

		MChildNode subnodes[N];
		bool changed = false;
		unsigned leafCnts[N];
		unsigned leafPatterns[N];
		memset(leafCnts, 0, sizeof(leafCnts));
		memset(leafPatterns, 0, sizeof(leafPatterns));

		for (unsigned o = 0; o < 8; o++)
		{
			//setup subnodes, divide leaf nodes
			for (unsigned i = 0; i < N; i++)
			{
				if (nodes[i].isLeaf)
				{
					if (nodes[i].leaf.pattern & (1 << o))
						subnodes[i] = MChildNode(0xff, 8);
					else
						subnodes[i] = MChildNode(0, 0);
				}
				else
				{
					subnodes[i] =
							trees[i]->tree[nodes[i].node.index].children[o];
				}
			}

			vector<MChildNode> newnodes(N);
			changed |= createRoundedSet_r(N, subnodes, trees, roundUp, newtrees,
					newnodes, bitvol);

			for (unsigned i = 0; i < N; i++)
			{
				if (!nodes[i].isLeaf)
				{
					newtrees[i][positions[i]].children[o] = newnodes[i];
					if (newnodes[i].isLeaf)
					{
						if (newnodes[i].leaf.pattern == 0xff)
						{
							leafCnts[i]++;
							leafPatterns[i] |= (1 << o);
						}
						else if (newnodes[i].leaf.pattern == 0)
							leafCnts[i]++;
					}
					newtrees[i][positions[i]].vol += newnodes[i].volume(
							newtrees[i], bitvol);
				}
			}
		}

		//coalesce any all leaves
		for (unsigned i = 0; i < N; i++)
		{
			if (leafCnts[i] == 8)
			{
				newtrees[i].pop_back();
				newroots[i].isLeaf = true;
				newroots[i].leaf.pattern = leafPatterns[i];
				newroots[i].leaf.numbits = __builtin_popcount(leafPatterns[i]);
				//volume is correct
			}
		}

		return changed;
	}
}

//round trees in in as much as possible while maintaining local distiguishability
//and put the result in out
bool MappableOctTree::createRoundedSet(unsigned N, const MappableOctTree**in,
		bool roundUp, MappableOctTree** out)
{
	MChildNode roots[N];
	float dim = 0;
	for (unsigned i = 0; i < N; i++)
	{
		roots[i] = in[i]->root;
		if (i == 0)
			dim = in[i]->dimension;
		else
			assert(dim == in[i]->dimension);
	}
	vector<vector<MOctNode> > newtrees(N);
	vector<MChildNode> newroots(N);
	bool changed = createRoundedSet_r(N, roots, in, roundUp, newtrees, newroots,
			dim * dim * dim);

	for (unsigned i = 0; i < N; i++)
	{
		out[i] = newFromVector(newtrees[i], newroots[i], dim);
	}

	return changed;
}

MappableOctTree* MappableOctTree::createFromIntersection(unsigned N,
		const MappableOctTree** in)
{
	MChildNode roots[N];
	float dim = 0;
	for (unsigned i = 0; i < N; i++)
	{
		roots[i] = in[i]->root;
		if (i == 0)
			dim = in[i]->dimension;
		else
			assert(dim == in[i]->dimension);
	}
	vector<MOctNode> newtree;
	MChildNode newroot = createFrom_r(N, roots, in, newtree, false,
			dim * dim * dim);

	return newFromVector(newtree, newroot, dim);
}

MChildNode MappableOctTree::createTruncated_r(const MChildNode& node, float res,
		bool fillIn, vector<MOctNode>& newtree, float cubeVol) const
		{
	if (cubeVol <= res * res * res) //at level that needs to be truncated to a leaf
	{
		if (fillIn && node.volume(tree, cubeVol) != 0)
		{
			return MChildNode(0xff, 8);
		}
		else
		{
			return MChildNode(0, 0);
		}
	}
	else if (node.isLeaf)
	{
		return node;
	}
	else //recursively descend
	{
		unsigned pos = newtree.size();
		MChildNode ret(pos);
		newtree.push_back(MOctNode());

		unsigned numFullEmpty = 0;
		unsigned newPattern = 0;
		for (unsigned i = 0; i < 8; i++)
		{
			MChildNode nchild = createTruncated_r(
					tree[node.node.index].children[i], res, fillIn, newtree,
					cubeVol / 8);
			newtree[pos].children[i] = nchild;
			if (nchild.isLeaf
					&& (nchild.leaf.pattern == 0 || nchild.leaf.pattern == 0xff))
			{
				numFullEmpty++;
				if (nchild.leaf.pattern == 0xff)
					newPattern |= 1 << i;
			}

			newtree[pos].vol += nchild.volume(newtree, cubeVol / 8);
		}

		if (numFullEmpty == 8)
		{
			newtree.pop_back();
			ret.isLeaf = true;
			ret.leaf.pattern = newPattern;
			ret.leaf.numbits = __builtin_popcount(newPattern);
		}
		return ret;
	}
}

MappableOctTree* MappableOctTree::createTruncated(float res, bool fillIn) const
		{
	vector<MOctNode> newtree;
	MChildNode newroot = createTruncated_r(root, res, fillIn, newtree,
			dimension * dimension * dimension);

	return newFromVector(newtree, newroot, dimension);
}

MChildNode MappableOctTree::createRounded_r(const MChildNode& node,
		float volcutoff, bool fillIn, vector<MOctNode>& newtree,
		float maxvol) const
		{
	float vol = node.volume(tree, maxvol);
	float empty = maxvol - vol;

	if (fillIn && empty < volcutoff)
	{
		return MChildNode(0xff, 8);
	}
	else if (!fillIn && vol < volcutoff)
	{
		return MChildNode(0, 0);
	}
	else if (node.isLeaf)
	{
		return node;
	}
	else //recursively descend
	{
		unsigned pos = newtree.size();
		MChildNode ret(pos);
		newtree.push_back(MOctNode());

		unsigned numFullEmpty = 0;
		unsigned newPattern = 0;
		for (unsigned i = 0; i < 8; i++)
		{
			MChildNode nchild = createRounded_r(
					tree[node.node.index].children[i], volcutoff, fillIn,
					newtree, maxvol / 8);
			newtree[pos].children[i] = nchild;
			if (nchild.isLeaf
					&& (nchild.leaf.pattern == 0 || nchild.leaf.pattern == 0xff))
			{
				numFullEmpty++;
				if (nchild.leaf.pattern == 0xff)
					newPattern |= 1 << i;
			}

			newtree[pos].vol += nchild.volume(newtree, maxvol / 8);
		}

		if (numFullEmpty == 8)
		{
			newtree.pop_back();
			ret.isLeaf = true;
			ret.leaf.pattern = newPattern;
			ret.leaf.numbits = __builtin_popcount(newPattern);
			//volume is correct
		}
		return ret;
	}
}

MappableOctTree* MappableOctTree::createRounded(float vol, bool fillIn) const
		{
	vector<MOctNode> newtree;
	MChildNode newroot = createRounded_r(root, vol, fillIn, newtree,
			dimension * dimension * dimension);

	return newFromVector(newtree, newroot, dimension);
}

//volume calculations that don't require creating a tmp tree
float MappableOctTree::intersectVolume(const MappableOctTree * rhs) const
		{
	float ival = 0, uval = 0;
	assert(dimension == rhs->dimension);
	root.intersectUnionVolume(tree, rhs->root, rhs->tree,
			dimension * dimension * dimension, ival, uval);
	return ival;
}

float MappableOctTree::unionVolume(const MappableOctTree *rhs) const
		{
	float ival = 0, uval = 0;
	assert(dimension == rhs->dimension);
	root.intersectUnionVolume(tree, rhs->root, rhs->tree,
			dimension * dimension * dimension, ival, uval);
	return uval;
}

void MappableOctTree::intersectUnionVolume(const MappableOctTree *rhs,
		float& ival, float& uval) const
		{
	ival = 0;
	uval = 0;
	assert(dimension == rhs->dimension);

	root.intersectUnionVolume(tree, rhs->root, rhs->tree,
			dimension * dimension * dimension, ival, uval);
}

bool MappableOctTree::containedIn(const MappableOctTree *rhs) const
		{
	return root.containedIn(tree, rhs->root, rhs->tree);
}

//return total volume contained in octtree
float MappableOctTree::volume() const
{
	return root.volume(tree, dimension * dimension * dimension);
}

//return number of leaves
unsigned MappableOctTree::leaves() const
{
	unsigned cnt = 0;
	for (unsigned i = 0; i < N; i++)
	{
		for (unsigned j = 0; j < 8; j++)
		{
			if (tree[i].children[j].isLeaf)
				cnt++;
		}
	}
	return cnt;
}

void MappableOctTree::write(ostream& out) const
		{
	unsigned sz = sizeof(MappableOctTree) + N * sizeof(MOctNode);
	out.write((char*) this, sz);
}

float MappableOctTree::relativeVolumeDistance(const MappableOctTree * rhs) const
{
	float ival = 0, uval = 0;
	assert(dimension == rhs->dimension);

	root.intersectUnionVolume(tree, rhs->root, rhs->tree,
			dimension * dimension * dimension, ival, uval);
	if (uval == 0) //possible when don't have anchored shapes
		return 0.0; //two empty shapes..
	return 1 - ival / uval;
}

float MappableOctTree::absoluteVolumeDistance(const MappableOctTree * rhs) const
		{
	float ival = 0, uval = 0;
	assert(dimension == rhs->dimension);

	root.intersectUnionVolume(tree, rhs->root, rhs->tree,
			dimension * dimension * dimension, ival, uval);

	return uval - ival;
}

void MChildNode::invert(MOctNode* tree, float maxvol)
{
	if (isLeaf)
	{
		leaf.pattern = ~leaf.pattern;
		leaf.numbits = 8 - leaf.numbits;
	}
	else
	{
		tree[node.index].vol = 0;
		for (unsigned i = 0; i < 8; i++)
		{
			tree[node.index].children[i].invert(tree, maxvol / 8);
			tree[node.index].vol += tree[node.index].children[i].volume(tree,
					maxvol / 8);
		}
	}
}

//compute both the intersection and union volume at once
void MChildNode::intersectUnionVolume(const MOctNode* tree,
		const MChildNode& rhs, const MOctNode* rtree, float cubeVol,
		float& intersectval, float& unionval) const
		{
	float bitv = cubeVol / 8;
	if (rhs.isLeaf && isLeaf)
	{
		if (leaf.pattern == 0 || rhs.leaf.pattern == 0)
			intersectval += 0; //no intersect val
		else
		{
			unsigned intpat = leaf.pattern & rhs.leaf.pattern;
			intersectval += __builtin_popcount(intpat) * bitv;
		}
		if (leaf.pattern == 0)
			unionval += rhs.volume(rtree, cubeVol);
		else if (rhs.leaf.pattern == 0)
			unionval += volume(tree, cubeVol);
		else
		{
			unsigned orpat = leaf.pattern | rhs.leaf.pattern;
			unionval += __builtin_popcount(orpat) * bitv;
		}
	}
	else if (isLeaf && leaf.pattern == 0xff)
	{
		unionval += volume(tree, cubeVol);
		intersectval += rhs.volume(rtree, cubeVol);
	}
	else if (rhs.isLeaf && rhs.leaf.pattern == 0xff)
	{
		unionval += rhs.volume(rtree, cubeVol);
		intersectval += volume(tree, cubeVol);
	}
	else if (rhs.isLeaf && rhs.leaf.pattern == 0)
	{
		//vol of this tree
		unionval += volume(tree, cubeVol);
	}
	else if (isLeaf && leaf.pattern == 0)
	{
		// from rhs
		unionval += rhs.volume(rtree, cubeVol);
	}
	else if (isLeaf)
	{
		assert(!rhs.isLeaf);
		assert(leaf.numbits > 0);
		//rhs has children, have to compare bit by bit
		for (unsigned i = 0; i < 8; i++)
		{
			MChildNode rchild = rtree[rhs.node.index].children[i];
			if (leaf.pattern & (1 << i))
			{
				unionval += bitv;
				intersectval += rchild.volume(rtree, bitv);
			}
			else
			{
				unionval += rchild.volume(rtree, bitv);
			}
		}
	}
	else if (rhs.isLeaf)
	{
		assert(!isLeaf);
		assert(rhs.leaf.numbits > 0);
		//rhs has children, have to compare bit by bit
		for (unsigned i = 0; i < 8; i++)
		{
			MChildNode child = tree[node.index].children[i];
			if (rhs.leaf.pattern & (1 << i))
			{
				unionval += bitv;
				intersectval += child.volume(tree, bitv);
			}
			else
			{
				unionval += child.volume(tree, bitv);
			}
		}
	}
	else //both have children
	{
		for (unsigned i = 0; i < 8; i++)
		{
			tree[node.index].children[i].intersectUnionVolume(tree,
					rtree[rhs.node.index].children[i], rtree, bitv,
					intersectval, unionval);
		}
	}
}

//return true if this is contained in rhs - short circuit eval
bool MChildNode::containedIn(const MOctNode* tree, const MChildNode& rhs,
		const MOctNode* rtree) const
		{
	if (rhs.isLeaf && isLeaf)
	{
		return (leaf.pattern & rhs.leaf.pattern) == leaf.pattern;
	}
	else if (isLeaf && leaf.pattern == 0)
	{
		return true;
	}
	else if (rhs.isLeaf && rhs.leaf.pattern == 0)
	{
		return false; //somthing not in nothing (nothing handled above)
	}
	else if (rhs.isLeaf && rhs.leaf.pattern == 0xff)
	{
		return true;
	}
	else if (isLeaf && leaf.pattern == 0xff)
	{
		return false;
	}
	else if (isLeaf)
	{
		assert(!rhs.isLeaf);
		assert(leaf.numbits > 0);
		//rhs has children, have to compare bit by bit
		for (unsigned i = 0; i < 8; i++)
		{
			if (leaf.pattern & (1 << i))
			{
				MChildNode rchild = rtree[rhs.node.index].children[i];
				//rchild must be a leaf and full
				if (!rchild.isLeaf || rchild.leaf.pattern != 0xff)
					return false;
			}
		}
		return true;
	}
	else if (rhs.isLeaf)
	{
		assert(!isLeaf);
		assert(rhs.leaf.numbits > 0);
		//rhs has children, have to compare bit by bit
		for (unsigned i = 0; i < 8; i++)
		{
			if (!(rhs.leaf.pattern & (1 << i)))
			{
				MChildNode child = tree[node.index].children[i];
				//if rhs's bit is empty, then our child must be empty
				if (!child.isLeaf || child.leaf.pattern != 0)
					return false;
			}
		}
		return true;
	}
	else //both have children
	{
		for (unsigned i = 0; i < 8; i++)
		{
			if (!tree[node.index].children[i].containedIn(tree,
					rtree[rhs.node.index].children[i], rtree))
				return false;
		}
		return true;;
	}
	return false;
}

static void addVertices(vector<Vertex>& vertices, Cube box)
{
	for (unsigned i = 0; i < 8; i++)
	{
		float x = box.x;
		float y = box.y;
		float z = box.z;

		if (i & 1)
			x += box.dim;
		if (i & 2)
			y += box.dim;
		if (i & 4)
			z += box.dim;

		vertices.push_back(Vertex(x, y, z));
	}
}

//append vertices of this child to vertices, this child's box is provided
void MChildNode::collectVertices(vector<Vertex>& vertices, Cube box,
		const MOctNode* tree) const
		{
	if (isLeaf)
	{
		if (leaf.pattern == 0)
			return; //empty, no vertices
		else if (leaf.pattern == 0xff) //full, add this box's vertices
		{
			addVertices(vertices, box);
		}
		else
		{
			for (unsigned i = 0; i < 8; i++)
			{
				if (leaf.pattern & (1 << i))
				{
					addVertices(vertices, box.getOctant(i));
				}
			}
		}
	}
	else
	{
		for (unsigned i = 0; i < 8; i++)
		{
			tree[node.index].children[i].collectVertices(vertices,
					box.getOctant(i), tree);
		}
	}
}

//sort, then remove duplicates and remove vertices that appear 8 times altogether
static void uniqueRemove8ify(vector<Vertex>& V)
{
	sort(V.begin(), V.end());

	unsigned i = 1;
	unsigned n = V.size();
	unsigned cnt = 1;
	unsigned last = 0;
	while (i < n)
	{
		if (V[i] == V[last])
		{
			cnt++;
			i++;
		}
		else
		{
			if (cnt < 8)
				last++; //keep what we copied
			V[last] = V[i];
			cnt = 1;
		}
	}

	if (cnt < 8)
		V.resize(last + 1);
	else
		V.resize(last);
}

//the Hausdorff distance is the maximum minimum distance between the
//two shapes; it is not symmetric, but this computes the symmetric
//(max H(A,B),H(B,A)) distance
//
//TODO: make this efficient if it turns out to be worthwhile, right now
//do the easiest implimentation: collect all leaf corner coordinates, sort,
//remove completely buried (8 versions), compute all pairs distances
float MappableOctTree::hausdorffDistance(const MappableOctTree* B) const
{
	vector<Vertex> Av, Bv;
	float dim = dimension;

	Cube box(dim, -dim / 2, -dim / 2, -dim / 2);
	root.collectVertices(Av, box, tree);
	B->root.collectVertices(Bv, box, B->tree);

	uniqueRemove8ify(Av);
	uniqueRemove8ify(Bv);

	//compute distances once
	vector<float> Bmin(Bv.size(), HUGE_VAL);
	float Amax = 0;
	for (unsigned i = 0, n = Av.size(); i < n; i++)
	{
		float Amin = HUGE_VAL;
		for (unsigned j = 0, m = Bv.size(); j < m; j++)
		{
			float dist = Av[i].distanceSq(Bv[j]);
			if (dist < Amin)
				Amin = dist;

			if (dist < Bmin[j])
				Bmin[j] = dist;
		}
		if (Amin > Amax)
			Amax = Amin;
	}

	float Bmax = *std::max_element(Bmin.begin(), Bmin.end());
//cout << Av.size() << " " << oldA << " " << Bv.size() << " " << oldB << " " << Amax << " " << Bmax << "\n";
	return sqrt(std::max(Amax, Bmax));
}

bool MChildNode::equals(const MOctNode* tree, const MChildNode& rhs,
		const MOctNode* rtree) const
		{
	if (isLeaf)
	{
		if (rhs.isLeaf)
		{
			if (rhs.leaf.pattern == leaf.pattern)
				return true;
			else
				return false;
		}
		else
			return false;
	}
	else if (rhs.isLeaf)
	{
		return false;
	}
	else
	{
		for (unsigned i = 0; i < 8; i++)
		{
			if (!tree[node.index].children[i].equals(tree,
					rtree[rhs.node.index].children[i], rtree))
				return false;
		}
		return true;
	}
	return true;
}

bool MappableOctTree::equals(const MappableOctTree* rhs) const
		{
	return root.equals(tree, rhs->root, rhs->tree);
}

//recurse down and see if i,j,k is set
//max is the integral dimension of this node
bool MChildNode::checkCoord(const MOctNode *tree, unsigned i, unsigned j,
		unsigned k, unsigned max) const
		{
	//first compute the octant identified by i,j,k
	unsigned half = max / 2;
	unsigned oct = 0;
	if (i >= half)
	{
		oct |= 1;
		i -= half;
	}
	if (j >= half)
	{
		oct |= 2;
		j -= half;
	}
	if (k >= half)
	{
		oct |= 4;
		k -= half;
	}

	if (isLeaf)
	{
		return leaf.pattern & (1 << oct);
	}
	else
	{
		return tree[node.index].children[oct].checkCoord(tree, i, j, k, half);
	}
}

//dump an mmp formated grid
void MappableOctTree::dumpGrid(ostream& out, float res) const
		{
	//size
	float dim = dimension;
	out << "header\n";
	unsigned max = ceil(dim / res);
	out << max << " " << max << " " << max << "\n";
	//origin
	out << -dim / 2 << " " << -dim / 2 << " " << -dim / 2 << "\n";
	//resolution
	out << res << " " << res << " " << res << "\n";

	//now coordinates
	//definitely not the most efficient way
	for (unsigned i = 0; i < max; i++)
	{
		for (unsigned j = 0; j < max; j++)
		{
			for (unsigned k = 0; k < max; k++)
			{
				if (root.checkCoord(tree, i, j, k, max))
					out << "1 ";
				else
					out << "0 ";
			}
			out << "\n";
		}
		out << "\n";
	}
}

//no header, just binary output
void MappableOctTree::dumpRawGrid(ostream& out, float res) const
		{
	char one = 0xff;
	char zero = 0;
	unsigned max = ceil(dimension / res);

	//now coordinates
	//definitely not the most efficient way
	for (unsigned i = 0; i < max; i++)
	{
		for (unsigned j = 0; j < max; j++)
		{
			for (unsigned k = 0; k < max; k++)
			{
				if (root.checkCoord(tree, i, j, k, max))
					out.write(&one, 1);
				else
					out.write(&zero, 1);
			}
		}
	}
}

//autodock 4 grid
void MappableOctTree::dumpAD4Grid(ostream& out, float res) const
{

	unsigned max = ceil(dimension / res);

	out << "GRID_PARAMETER_FILE\nGRID_DATA_FILE\nMACROMOLECULE\n";
	out << "SPACING " << res << "\n";
	out << "NELEMENTS " << max-1 << " " << max-1 << " " << max-1 << "\n";
	out << "CENTER 0 0 0\n";
	//now coordinates - z,y,x
	//definitely not the most efficient way
	for (unsigned k = 0; k < max; k++)
	{
		for (unsigned j = 0; j < max; j++)
		{
			for (unsigned i = 0; i < max; i++)
			{
				if (root.checkCoord(tree, i, j, k, max))
					out << "1\n";
				else
					out << "0\n";
			}
		}
	}
}

#include <arpa/inet.h>

//sproxel csv format, voxelValue is the string to specify voxels occupied by the object
void MappableOctTree::dumpSproxelGrid(ostream& out, float res,
		const string& voxelValue) const
		{
	unsigned short max = ceil(dimension / res);
	out << max << "," << max << "," << max << "\n";

	string blank = "#00000000";
	for (unsigned j = 0; j < max; j++)
	{ //sproxel csv is in xz slices
		for (unsigned i = 0; i < max; i++)
		{
			for (unsigned k = 0; k < max; k++)
			{
				if (k != 0)
					out << ",";
				if (root.checkCoord(tree, i, j, k, max))
					out << voxelValue;
				else
					out << blank;
			}
			out << "\n";
		}
		out << "\n";
	}
}

//mira grid format
void MappableOctTree::dumpMiraGrid(ostream& out, float res) const
		{
	unsigned short max = ceil(dimension / res);

	MiraHeader fileheader =
			{
					{ 'V', 'O', 'X', 'E', 'L' }, 26, 1, htons(max), htons(max), htons(
							max), 0, 256, (unsigned)(256 + 3 * max * sizeof(double)) };

	//we store resolution information in unused part of header
	stringstream str;
	str << "resolution = " << res;
	strncpy(fileheader.unused, str.str().c_str(), sizeof(fileheader.unused)-1);

	char one = 0xff;
	char zero = 0;

	out.write((char*) &fileheader, sizeof(fileheader));
	for (unsigned i = 0; i < 3; i++)
	{
		double pos = 0;
		for (unsigned j = 0; j < max; j++)
		{
			out.write((char*) &pos, sizeof(double));
			pos += res;
		}
	}

	//now coordinates
	//definitely not the most efficient way
	for (unsigned i = 0; i < max; i++)
	{
		for (unsigned j = 0; j < max; j++)
		{
			for (unsigned k = 0; k < max; k++)
			{
				if (root.checkCoord(tree, i, j, k, max))
					out.write(&one, 1);
				else
					out.write(&zero, 1);
			}
		}
	}
}

void MChildNode::countLeavesAtDepths(const MOctNode* tree, unsigned depth,
		vector<unsigned>& counts) const
		{
	if (counts.size() <= depth)
		counts.resize(depth + 1, 0);

	if (isLeaf)
	{
		counts[depth]++;
	}
	else
	{
		for (unsigned i = 0; i < 8; i++)
		{
			tree[node.index].children[i].countLeavesAtDepths(tree, depth + 1,
					counts);
		}
	}
}

void MappableOctTree::countLeavesAtDepths(vector<unsigned>& counts) const
		{
	counts.clear();
	root.countLeavesAtDepths(tree, 0, counts);
}

void MappableOctTree::makeGrid(MGrid& grid, float res) const
		{
	//size
	float dim = dimension;
	unsigned max = ceil(dim / res);
	grid = MGrid(dim, res);
	float start = -dim / 2;

	//now coordinates
	//definitely not the most efficient way
	for (unsigned i = 0; i < max; i++)
	{
		for (unsigned j = 0; j < max; j++)
		{
			for (unsigned k = 0; k < max; k++)
			{
				if (root.checkCoord(tree, i, j, k, max))
				{
					float x = start + i * res;
					float y = start + j * res;
					float z = start + k * res;
					grid.setPoint(x, y, z);
				}
			}
		}
	}
}