#include "b18TrafficOD.h"

#include "../roadGraphB2018Loader.h"
#include "../misctools/misctools.h"

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions/non_central_t.hpp>

#define ROUTE_DEBUG 0
#define PERSON_DEBUG 0

namespace LC {

B18TrafficOD::B18TrafficOD(const parameters & inputSimParameters) : simParameters(inputSimParameters) {
}//
B18TrafficOD::~B18TrafficOD() {
}//

#ifdef B18_RUN_WITH_GUI
void B18TrafficOD::sampleDistribution(int numberToSample,
                                      PeopleJobOneLayer &dist, std::vector<QVector2D> &samples, QString &name) {

  QMap<int, int> posInArrayIndexInSamples;
  int posInArray = 0;
  std::vector<float> accProbability;

  for (int mP = 0; mP < dist.amplitudes.size(); mP++) {
    float ampl = dist.amplitudes[mP];

    if (ampl > 0) {
      float accProb = ampl;

      if (accProbability.size() > 0) {
        accProb += accProbability[accProbability.size() - 1];
      }

      accProbability.push_back(accProb);
      posInArrayIndexInSamples.insert(posInArray, mP);
      posInArray++;
    }
  }

  int maxAccValue = accProbability[accProbability.size() - 1];

  samples.resize(numberToSample);
  std::random_device rd;
  std::normal_distribution<float> distSam(0, dist.stdev);
  std::mt19937 rand_engine(rd());

  for (int nSam = 0; nSam < numberToSample; nSam++) {
    // find amplitude
    float randAmpl = (((float)qrand()) / RAND_MAX) *
                     maxAccValue; //sample in the acc
    int aN;
    bool found = true;

    for (aN = 0; aN < accProbability.size(); aN++) {
      if (accProbability[aN] > randAmpl) {
        found = true;
        break;
      }
    }

    //printf("aN %d randAmpl %f accPro[aN] %f\n",aN,randAmpl,accProbability[aN]);
    if (found == false) {
      printf("ERROR randAmpl not found\n");
    }

    // sample gaussian nad create sample
    samples[nSam].setX(distSam(rand_engine) +
                       dist.samplePosition[posInArrayIndexInSamples[aN]].x());
    samples[nSam].setY(distSam(rand_engine) +
                       dist.samplePosition[posInArrayIndexInSamples[aN]].y());
  }

  if (name.length() > 0) {
    if (PERSON_DEBUG) {
      printf(">> >> >> 2.1. normalize image to check and save maxAccValue %d numSample %d\n",
             maxAccValue, numberToSample);
    }

    cv::Mat peopleSelecF = cv::Mat::zeros(1024, 1024, CV_32FC1);
    cv::Mat peopleSelecU = cv::Mat::zeros(1024, 1024, CV_8UC1);
    float maxDesity = 0;
    int r, c;

    for (int nSam = 0; nSam < numberToSample; nSam++) {
      r = ((samples[nSam].y() / G::global().getFloat("worldWidth")) + 0.5f) *
          peopleSelecF.rows;
      c = ((samples[nSam].x() / G::global().getFloat("worldWidth")) + 0.5f) *
          peopleSelecF.cols;

      if (r < 0) {
        r = 0;
      }

      if (r >= peopleSelecF.rows) {
        r = peopleSelecF.rows - 1;
      }

      if (c < 0) {
        c = 0;
      }

      if (c >= peopleSelecF.rows) {
        c = peopleSelecF.cols - 1;
      }

      peopleSelecF.at<float>(r, c) += 1.0f;

      if (maxDesity < peopleSelecF.at<float>(r, c)) {
        maxDesity = peopleSelecF.at<float>(r, c);
      }
    }

    if (PERSON_DEBUG) {
      printf("maxDensity %f\n", maxDesity);
    }

    for (int c = 0; c < peopleSelecU.cols; c++) {
      for (int r = 0; r < peopleSelecU.rows; r++) {
        peopleSelecU.at<uchar>(r, c) = (uchar)(255.0f * peopleSelecF.at<float>(r,
                                               c) / maxDesity);
      }
    }

    cv::imwrite(name.toLatin1().constData(), peopleSelecU);
  }

  if (PERSON_DEBUG) {
    printf("<<sampleDistribution\n");
  }
}
#endif
void B18TrafficOD::randomPerson(int p, B18TrafficPerson &person,
                                uint srcvertex,
                                uint tgtvertex, float startTimeH) {

  // Data
  person.init_intersection = srcvertex;
  person.end_intersection = tgtvertex;
  person.time_departure = startTimeH;// * 3600.0f; //seconds
  // Status
  qsrand(p);

  person.a = simParameters.a;
  person.b = simParameters.b;
  person.T = simParameters.T;
  person.v = 0;
  person.num_steps = 0;
  person.co = 0;
  person.active = 0;
  person.numOfLaneInEdge = 0;
  person.color = p << 8;
  person.LC_stateofLaneChanging = 0;
  person.indexPathInit = 0; // 0 should point to -1.

}

void B18TrafficOD::randomPerson(int p, B18TrafficPerson &person,
                                QVector3D housePos3D, QVector3D jobPos3D,
                                float startTimeH,
                                LC::RoadGraph::roadBGLGraph_BI &roadGraph) {
  LC::RoadGraph::roadGraphVertexDesc srcvertex, tgtvertex;
  RoadGraph::roadGraphVertexIter vi, viEnd;

  // 3.3 find closest nodes
  QVector2D hP = housePos3D.toVector2D();
  QVector2D jP = jobPos3D.toVector2D();
  float minHouseDis = FLT_MAX;
  float minJobDis = FLT_MAX;

  for (boost::tie(vi, viEnd) = boost::vertices(roadGraph);
       vi != viEnd; ++vi) {
    QVector2D roadVert(roadGraph[*vi].pt);
    float newHDist = (hP - roadVert).lengthSquared();

    if (newHDist < minHouseDis) {
      srcvertex = *vi;
      minHouseDis = newHDist;
    }

    float newJDist = (jP - roadVert).lengthSquared();

    if (newJDist < minJobDis) {
      tgtvertex = *vi;
      minJobDis = newJDist;
    }
  }

  randomPerson(p, person, srcvertex, tgtvertex, startTimeH);
}

void B18TrafficOD::resetTrafficPersonJob(std::vector<B18TrafficPerson>
    &trafficPersonVec) {
  for (int p = 0; p < trafficPersonVec.size(); p++) {
    trafficPersonVec[p].active = 0;
  }
}//

#ifdef B18_RUN_WITH_GUI
void B18TrafficOD::createRandomPeople(
  int numberPerGen,
  float startTimeH, float endTimeH,
  std::vector<B18TrafficPerson> &trafficPersonVec,
  PeopleJobInfoLayers &simPeopleJobInfoLayers,
  LC::RoadGraph::roadBGLGraph_BI &roadGraph) {
  if (PERSON_DEBUG) {
    printf(">> generateTrafficPersonJob\n");
  }

  QTime timer;
  //////////////////////////////////
  // 1. Generate PEOPLE
  timer.start();
  std::vector<QVector2D> peopleDis;
  QString people_dist_file("test/people_dist.png");
  sampleDistribution(numberPerGen, simPeopleJobInfoLayers.layers[1], peopleDis,
                     people_dist_file);

  if (PERSON_DEBUG) {
    printf(">> >> People %d sampled in %d ms\n", numberPerGen, timer.elapsed());
  }

  //////////////////////////////////
  // 2. Generate JOBS
  timer.restart();
  std::vector<QVector2D> jobDis;
  QString job_dist_file("test/job_dist.png");
  sampleDistribution(numberPerGen, simPeopleJobInfoLayers.layers[0], jobDis,
                     job_dist_file);

  if (PERSON_DEBUG) {
    printf(">> >> Job %d sampled in %d ms\n", numberPerGen, timer.elapsed());
  }

  // 3. Generate PEOPLE TRAFFIC
  timer.restart();
  trafficPersonVec.resize(numberPerGen);
  qsrand(6541799621);

  float midTime = (startTimeH + endTimeH) / 2.0f;

  for (int p = 0; p < numberPerGen; p++) {
    float goToWorkH = midTime + LC::misctools::genRand(startTimeH - midTime,
                      endTimeH - startTimeH);
    randomPerson(p, trafficPersonVec[p], peopleDis[p], jobDis[p], goToWorkH,
                 roadGraph);
  }

  if (PERSON_DEBUG) {
    printf(">> >> PeopleTraffic %d generated in %d ms\n", numberPerGen,
           timer.elapsed());
  }

  if (PERSON_DEBUG) {
    printf("<< generateTrafficPersonJob\n");
  }
}//
#endif

bool fileDistributionInitialized = false;
const float startSamples = 2.5f;
const float endSamples = 14.5f;
const int numBucketsPerHour = 6;
std::vector<float> hToWDistribution;
const bool gaussianDistribution = false; // true = file; false = gaussian.
float sampleFileDistribution() {
  // Initialized.
  if (fileDistributionInitialized == false) {
    QFile inputFile("berkeley_2018/HtoW_trips.csv");

    if (!inputFile.open(QIODevice::ReadOnly)) {
      printf("for fileDistributionInitialized must exist file berkeley_2018/HtoW_trips.csv\n");
      exit(-1);
    }

    int numBuckets = ceil((endSamples - startSamples) * numBucketsPerHour);
    hToWDistribution = std::vector<float>(numBuckets, 0);
    int accPeople = 0;

    QTextStream in(&inputFile);

    while (!in.atEnd()) {
      QString line = in.readLine();
      QStringList fields = line.split(",");

      if (fields.size() >= 3) {
        int numPeople = fields[2].toInt();
        float time = fields[1].toFloat();
        int targetBucket = (time - startSamples) * numBucketsPerHour;

        if (targetBucket < 0 || targetBucket >= hToWDistribution.size()) {
          continue;
        }

        hToWDistribution[targetBucket] += numPeople;
        accPeople += numPeople;
        //printf("time %.2f people %d -> bucket %d of %d\n", time, hToWDistribution[targetBucket], targetBucket, numBuckets);
      }
    }

    inputFile.close();

    // Print and normalize.
    for (int b = 0; b < hToWDistribution.size(); b++) {
      float bucketStartTime = startSamples + b * (1.0f / numBucketsPerHour);
      //printf("htoW,%.2f,%.0f\n", bucketStartTime, hToWDistribution[b]);
      hToWDistribution[b] /= float(accPeople + 1); // distribution
    }

    // Accumulate distribution.
    for (int b = 1; b < hToWDistribution.size(); b++) { // note starts with 1.
      hToWDistribution[b] += hToWDistribution[b - 1];
      float bucketStartTime = startSamples + b * (1.0f / numBucketsPerHour);
      //printf("Acc,%.2f,%f\n", bucketStartTime, hToWDistribution[b]); // acumulate
    }

    fileDistributionInitialized = true;
  }

  // Sample
  // Select bucket
  float randNumBucket = misctools::genRand();
  int b = 0;

  for (; b < hToWDistribution.size(); b++) {
    //printf("Compare %.2f vs %.2f", randNumBucket, hToWDistribution[b]);
    if (hToWDistribution[b] > randNumBucket) {
      break;
    }
  }

  float bucketStartTime = startSamples + b * (1.0f / numBucketsPerHour);
  // Random withinbucket
  float randTimeWithinBucket = misctools::genRand() / numBucketsPerHour;
  //printf("randBNum %.2f --> b %d\n", randNumBucket, b);
  return randTimeWithinBucket + bucketStartTime;
}

void B18TrafficOD::loadB18TrafficPeople(
    float startTimeH, float endTimeH,
    std::vector<B18TrafficPerson> &trafficPersonVec, // out
    RoadGraph::roadBGLGraph_BI &roadGraph, const int limitNumPeople, const bool addRandomPeople) {

  trafficPersonVec.clear();
  QTime timer;
  timer.start();

  if (RoadGraphB2018::demandB2018.size() == 0) {
    printf("ERROR at loadB18TrafficPeople: Imposible to generate b2018 without loading b2018 demmand first\n");
    return;
  }

  const int totalNumPeople = [&limitNumPeople, &addRandomPeople] {
    if (limitNumPeople > 0 && addRandomPeople)
      return limitNumPeople;
    else
      return RoadGraphB2018::totalNumPeople;
  }();
  trafficPersonVec.resize(totalNumPeople);

  boost::mt19937 rng;
  srand(45321654);
  //boost::math::non_central_t_distribution<> td(/*v=*/2.09,/*delta=*/7.51);
  //boost::variate_generator<boost::mt19937&, boost::math::non_central_t_distribution<> > var(rng, td);
  boost::normal_distribution<> nd(7.5, 0.75);
  boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > var(
    rng, nd);

  int numPeople = 0;
  printf("demandB2018 = %d\n", RoadGraphB2018::demandB2018.size());
  for (int d = 0; (d < RoadGraphB2018::demandB2018.size()) &&
       (numPeople < totalNumPeople); d++) {
    int odNumPeople = std::min<int>(totalNumPeople - numPeople,
                                    RoadGraphB2018::demandB2018[d].num_people);
    //printf("odNumPeople = %d\n", odNumPeople);
    uint src_vertex = RoadGraphB2018::demandB2018[d].src_vertex;
    uint tgt_vertex = RoadGraphB2018::demandB2018[d].tgt_vertex;

    for (int p = 0; p < odNumPeople; p++) {
      float goToWorkH;

      if (gaussianDistribution) {
        goToWorkH  = var();
      } else {
        goToWorkH = sampleFileDistribution();
      }

      randomPerson(numPeople, trafficPersonVec[numPeople], src_vertex, tgt_vertex,
                   goToWorkH);
      // printf("go to work %.2f --> %.2f\n", goToWork, (trafficPersonVec[p].time_departure / 3600.0f));
      numPeople++;
    }
  }
  //printf("trafficPersonVec size = %d", trafficPersonVec.size());

  if (totalNumPeople > numPeople) {
    std::cerr << "Current amount: " << numPeople << std::endl;
    std::cerr << "Total amount: " << totalNumPeople << std::endl;
    printf("No enough on file --> Add random people %d\n",
           (totalNumPeople - numPeople));
    // If this happens, the user ask to generate random people.
    QList<int>  allVertexInd = RoadGraphB2018::indToNodeIndex.keys();

    for (; numPeople < totalNumPeople; numPeople++) {
      uint src_vertex = allVertexInd[rand() % allVertexInd.size()];
      uint tgt_vertex = allVertexInd[rand() % allVertexInd.size()];

      float goToWorkH;

      if (gaussianDistribution) {
        goToWorkH = var();
      } else {
        goToWorkH = sampleFileDistribution();
      }

      randomPerson(numPeople, trafficPersonVec[numPeople], src_vertex, tgt_vertex,
                   goToWorkH);
    }
  }

  if (totalNumPeople != numPeople) {
    printf("ERROR: generateB2018TrafficPeople totalNumPeople != numPeople, this should not happen.");
    exit(-1);
  }

  if (gaussianDistribution) {
    //print histogram
    float binLength = 0.166f;//10min
    float numBins = ceil((endTimeH - startTimeH) / binLength);
    printf("End time %.2f  Start time %.2f --> numBins %f\n", endTimeH, startTimeH,
           numBins);
    std::vector<int> bins(numBins);
    std::fill(bins.begin(), bins.end(), 0);

    for (int p = 0; p < trafficPersonVec.size(); p++) {
      // printf("depart %.2f\n", (trafficPersonVec[p].time_departure / 3600.0f));
      float t = (trafficPersonVec[p].time_departure / 3600.0f) - startTimeH;

      int binN = t / binLength;

      if (binN < 0 || binN >= numBins) {
        printf("ERROR: Bin out of range %d of %f\n", binN, numBins);
        continue;
      }

      bins[binN]++;
    }

    printf("\n");

    for (int binN = 0; binN < bins.size(); binN++) {
      printf("%f %d\n", startTimeH + binN * binLength, bins[binN]);
    }
  } else {
    // Plot histogram of file distribution.
    int numBuckets = ceil((endSamples - startSamples) * numBucketsPerHour);
    std::vector<int> bins(numBuckets, 0);

    for (int p = 0; p < trafficPersonVec.size(); p++) {
      // printf("depart %.2f\n", (trafficPersonVec[p].time_departure / 3600.0f));
      float time = (trafficPersonVec[p].time_departure / 3600.0f);
      int targetBucket = (time - startSamples) * numBucketsPerHour;

      if (targetBucket < 0 || targetBucket >= hToWDistribution.size()) {
        continue;
      }

      bins[targetBucket]++;
    }

    for (int b = 0; b < bins.size(); b++) {
      float bucketStartTime = startSamples + b * (1.0f / numBucketsPerHour);
      //printf("PeopleDist,%.2f,%d\n", bucketStartTime, bins[b]); // acumulate
    }
  }

  printf("loadB18TrafficPeople: People %d\n", numPeople);
}

void B18TrafficOD::loadB18TrafficPeopleSP(
    float startTimeH, float endTimeH,
    std::vector<B18TrafficPerson> &trafficPersonVec, // out
    const std::shared_ptr<abm::Graph>& graph_,
    const int limitNumPeople, const bool addRandomPeople, const std::vector<float> dep_times) {

  trafficPersonVec.clear();
  QTime timer;
  timer.start();

  //printf("demandB2018 size = %d\n", RoadGraphB2018::demandB2018.size());
  if (RoadGraphB2018::demandB2018.size() == 0) {
    printf("ERROR at loadB18TrafficPeopleSP: Imposible to generate b2018 without loading b2018 demmand first\n");
    return;
  }

  const int totalNumPeople = [&limitNumPeople, &addRandomPeople] {
    if (limitNumPeople > 0 && addRandomPeople)
      return limitNumPeople;
    else
      return RoadGraphB2018::totalNumPeople;
  }();
  trafficPersonVec.resize(totalNumPeople);

  /*
  boost::mt19937 rng;
  srand(45321654);
  //boost::math::non_central_t_distribution<> td(v=2.09,delta=7.51);
  //boost::variate_generator<boost::mt19937&, boost::math::non_central_t_distribution<> > var(rng, td);
  boost::normal_distribution<> nd(7.5, 0.75);
  boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > var(
    rng, nd);
  */
  int numPeople = 0;
  for (int d = 0; (d < RoadGraphB2018::demandB2018.size()) &&
       (numPeople < totalNumPeople); d++) {
    int odNumPeople = std::min<int>(totalNumPeople - numPeople,
                                    RoadGraphB2018::demandB2018[d].num_people);
    //printf("odNumPeople = %d\n", odNumPeople);
    uint src_vertex = RoadGraphB2018::demandB2018[d].src_vertex;
    uint tgt_vertex = RoadGraphB2018::demandB2018[d].tgt_vertex;
    float goToWorkH = dep_times[d];
    //printf("dep time %f\n", goToWorkH);

    for (int p = 0; p < odNumPeople; p++) {

      /*
      if (gaussianDistribution) {
        goToWorkH  = var();
      } else {
        goToWorkH = sampleFileDistribution();
      }
      */

      randomPerson(numPeople, trafficPersonVec[numPeople], src_vertex, tgt_vertex,
                   goToWorkH);
      // printf("go to work %.2f --> %.2f\n", goToWork, (trafficPersonVec[p].time_departure / 3600.0f));
      numPeople++;
    }
  }
  /*
  if (totalNumPeople > numPeople) {
    std::cerr << "Current amount: " << numPeople << std::endl;
    std::cerr << "Total amount: " << totalNumPeople << std::endl;
    printf("No enough on file --> Add random people %d\n",
           (totalNumPeople - numPeople));
    // If this happens, the user ask to generate random people.
    QList<int>  allVertexInd = RoadGraphB2018::indToNodeIndex.keys();

    for (; numPeople < totalNumPeople; numPeople++) {
      uint src_vertex = allVertexInd[rand() % allVertexInd.size()];
      uint tgt_vertex = allVertexInd[rand() % allVertexInd.size()];

      float goToWorkH;

      if (gaussianDistribution) {
        goToWorkH = var();
      } else {
        goToWorkH = sampleFileDistribution();
      }

      randomPerson(numPeople, trafficPersonVec[numPeople], src_vertex, tgt_vertex,
                   goToWorkH);
    }
  }
  */

  if (totalNumPeople != numPeople) {
    printf("ERROR: generateB2018TrafficPeople totalNumPeople != numPeople, this should not happen.");
    exit(-1);
  }

  /*
  if (gaussianDistribution) {
    //print histogram
    float binLength = 0.166f;//10min
    float numBins = ceil((endTimeH - startTimeH) / binLength);
    printf("End time %.2f  Start time %.2f --> numBins %f\n", endTimeH, startTimeH,
           numBins);
    std::vector<int> bins(numBins);
    std::fill(bins.begin(), bins.end(), 0);

    for (int p = 0; p < trafficPersonVec.size(); p++) {
      // printf("depart %.2f\n", (trafficPersonVec[p].time_departure / 3600.0f));
      float t = (trafficPersonVec[p].time_departure / 3600.0f) - startTimeH;

      int binN = t / binLength;

      if (binN < 0 || binN >= numBins) {
        printf("ERROR: Bin out of range %d of %f\n", binN, numBins);
        continue;
      }

      bins[binN]++;
    }

    printf("\n");

    for (int binN = 0; binN < bins.size(); binN++) {
      printf("%f %d\n", startTimeH + binN * binLength, bins[binN]);
    }
  } else {
    // Plot histogram of file distribution.
    int numBuckets = ceil((endSamples - startSamples) * numBucketsPerHour);
    std::vector<int> bins(numBuckets, 0);

    for (int p = 0; p < trafficPersonVec.size(); p++) {
      // printf("depart %.2f\n", (trafficPersonVec[p].time_departure / 3600.0f));
      float time = (trafficPersonVec[p].time_departure / 3600.0f);
      int targetBucket = (time - startSamples) * numBucketsPerHour;

      if (targetBucket < 0 || targetBucket >= hToWDistribution.size()) {
        continue;
      }

      bins[targetBucket]++;
    }

    for (int b = 0; b < bins.size(); b++) {
      float bucketStartTime = startSamples + b * (1.0f / numBucketsPerHour);
      //printf("PeopleDist,%.2f,%d\n", bucketStartTime, bins[b]); // acumulate
    }
  }
  */
  printf("loadB18TrafficPeople: People %d\n", numPeople);
}
}


/*
 * Copyright (c) 2020 Johannes Pankert <pankertj@ethz.ch>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of this work nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

// msgs
#include <geometry_msgs/Wrench.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <kindr/Core>
#include <mutex>
#include <string>

namespace perceptive_mpc {

class AdmittanceReferenceModule {
 public:
  AdmittanceReferenceModule();

  bool initialize();

  using eigen_pose_t = Eigen::Matrix<double, -1, 1>;
  using eigen_pose_vector_t = std::vector<eigen_pose_t, Eigen::aligned_allocator<eigen_pose_t>>;

  nav_msgs::Path adaptPath(double dt, const nav_msgs::Path& rawPath, const kindr::WrenchD measuredWrench);

  // interface for Kinematic Simulation demo implementation that does not use nav_msgs::Path types
  void adaptPath(double dt, eigen_pose_vector_t& path, const kindr::WrenchD measuredWrench);

  void setDesiredEndeffectorWrench(const geometry_msgs::Wrench::ConstPtr&);

 protected:
  kindr::WrenchD desiredEEWrench_;
  std::mutex desiredEEWrenchMutex_;

  Eigen::Vector3d forcePGains_;
  Eigen::Vector3d torquePGains_;
  Eigen::Vector3d forceIGains_;
  Eigen::Vector3d torqueIGains_;

  kindr::WrenchD wrenchIntegralMax_;
  kindr::WrenchD wrenchIntegral_;

  void antiWindup();
};

}  // namespace perceptive_mpc


#pragma once
#include <Eigen/Core>

#include "control_points_trait.hpp"
#include "internal/no_discard.hpp"

namespace ubs {

/** @brief The control points trait for Eigen::Matrix. */
template <typename OutputType_, int N_, int M_>
struct ControlPointsTrait<Eigen::Matrix<OutputType_, N_, M_>> {
    using Container = Eigen::Matrix<OutputType_, N_, M_>;

    template <std::size_t Dims>
    static void resize(Container& c, const std::array<int, Dims>& shape) {
        static_assert(Dims <= 2, "An eigen matrix can only used for up to two dimensions.");
        resize(c, shape, std::integral_constant<int, Dims>());
    }

    UBS_NO_DISCARD static int getNumElements(const Container& c) {
        return int(c.size());
    }

    UBS_NO_DISCARD static OutputType_* data(Container& c) {
        return c.data();
    }

    UBS_NO_DISCARD static const OutputType_* data(const Container& c) {
        return c.data();
    }

    UBS_NO_DISCARD static int getStride(const Container& c, int dim) {
        switch (dim) {
        case 0:
            return (M_ == 1) ? 1 : int(c.rows());
        case 1:
            assert(M_ != 1);
            return 1;
        default:
            assert(false);
            return 0;
        }
    }

    UBS_NO_DISCARD static int getSize(const Container& c, int dim) {
        if (M_ == 1) {
            return int(c.size());
        }

        switch (dim) {
        case 0:
            return int(c.cols());
        case 1:
            return int(c.rows());
        default:
            assert(false);
            return 0;
        }
    }

private:
    template <std::size_t Dims>
    static void resize(Container& c, const std::array<int, Dims>& shape, std::integral_constant<int, 1> /*dim*/) {
        c.conservativeResize(shape[0], 1);
    }

    template <std::size_t Dims>
    static void resize(Container& c, const std::array<int, Dims>& shape, std::integral_constant<int, 2> /*dim*/) {
        c.conservativeResize(shape[1], shape[0]);
    }
};
} // namespace ubs


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE staticsite_test

// Third party incldues
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/staticsite.h"

using namespace votca::xtp;

BOOST_AUTO_TEST_SUITE(staticsite_test)

BOOST_AUTO_TEST_CASE(constructors_test) { StaticSite st(1, "st1"); }

BOOST_AUTO_TEST_CASE(getters_test) {
  StaticSite st(1, "st2");
  BOOST_CHECK_EQUAL(st.getId(), 1);
  BOOST_CHECK_EQUAL(st.getElement(), "st2");
}

BOOST_AUTO_TEST_CASE(multipole_test) {
  StaticSite st(1, "st2");
  Eigen::VectorXd multipole = Eigen::VectorXd::Zero(9);
  multipole << 1, 2, 3, 4, 8, 7, 2, 3.3, -0.5;
  st.setMultipole(multipole, 2);
  bool check_mpoles = multipole.isApprox(st.Q(), 0.0001);
  BOOST_CHECK_EQUAL(check_mpoles, true);

  bool check_rank = (st.getRank() == 2);
  BOOST_CHECK_EQUAL(check_rank, true);
}

BOOST_AUTO_TEST_CASE(translate_test) {
  StaticSite st(1, "st2");
  Eigen::Vector3d shift;
  shift << 0, 0, 5;
  st.Translate(shift);
  BOOST_CHECK_EQUAL(shift.isApprox(st.getPos(), 1e-5), true);
}

BOOST_AUTO_TEST_CASE(writepolarization_test) {
  StaticSite st(1, "st2");
  Eigen::VectorXd multipole = Eigen::VectorXd::Zero(9);
  multipole << 1, 2, 3, 4, 8, 7, 2, 3.3, -0.5;
  st.setMultipole(multipole, 2);
  BOOST_CHECK_EQUAL(
      st.WriteMpsLine(),
      " st2 +0.0000000 +0.0000000 +0.0000000 Rank 2\n"
      "    +1.0000000\n"
      "    +4.0000000 +2.0000000 +3.0000000\n"
      "    +8.0000000 +7.0000000 +2.0000000 +3.3000000 -0.5000000\n"
      "     P +1.0000000\n");
}

BOOST_AUTO_TEST_CASE(rotation_test) {
  StaticSite st(1, "st2", Eigen::Vector3d::UnitY());

  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();  // Rotation around z axes
  R << 0, -1, 0, 1, 0, 0, 0, 0, 1;

  Eigen::VectorXd multipoles = Eigen::VectorXd::Zero(9);
  multipoles << 1, 1, 0, 0, 0, 1, 0, 0,
      0;  // q=1, mu_x=1 and Q_21c=1 the rest is 0
  st.setMultipole(multipoles, 2);
  st.Rotate(R, Eigen::Vector3d::Zero());
  bool equalpos = st.getPos().isApprox(Eigen::Vector3d(-1, 0, 0), 1e-5);
  if (!equalpos) {
    std::cout << "Result " << std::endl;
    std::cout << st.getPos() << std::endl;
    std::cout << "Reference" << std::endl;
    std::cout << Eigen::Vector3d(-1, 0, 0) << std::endl;
  }
  BOOST_CHECK_EQUAL(equalpos, true);

  Eigen::VectorXd rotmultipoles = Eigen::VectorXd::Zero(9);
  rotmultipoles << 1, 0, 1, 0, 0, 0, 1, 0, 0;  // q=1, mu_y=1 and Q_21s=1 is 0
  bool equalmultipoles = rotmultipoles.isApprox(st.Q(), 1e-5);
  if (!equalmultipoles) {
    std::cout << "Result " << std::endl;
    std::cout << st.Q() << std::endl;
    std::cout << "Reference" << std::endl;
    std::cout << rotmultipoles << std::endl;
  }
  BOOST_CHECK_EQUAL(equalmultipoles, true);
}

BOOST_AUTO_TEST_SUITE_END()


// -----------------------------------------------------------------------------
/**
 *  @brief Optimization algorithm interface.
 *  @author Jose M. Buenaposada
 *  @date 2012/07/10
 *  @version $revision$
 *
 *  $id$
 *
 *  Grupo de investigación en Percepción Computacional y Robótica)
 *  (Perception for Computers & Robots research Group)
 *  Facultad de Informática (Computer Science School)
 *  Universidad Politécnica de Madrid (UPM) (Madrid Technical University)
 *  http://www.dia.fi.upm.es/~pcr
 *
 */
// -----------------------------------------------------------------------------

// ------------------ RECURSION PROTECTION -------------------------------------
#ifndef OPTIMIZER_HPP
#define OPTIMIZER_HPP

// ----------------------- INCLUDES --------------------------------------------
#include <boost/shared_ptr.hpp>
#include "optimization_problem.hpp"
#include "img_align_lib.hpp"

namespace upm { namespace pcr
{
// ----------------------------------------------------------------------------
/**
 *  Definition of an smart pointer to Optimizer type
 */
// -----------------------------------------------------------------------------
class Optimizer;
typedef boost::shared_ptr<Optimizer> OptimizerPtr;

// -----------------------------------------------------------------------------
/**
 *  Definition of a container of smart pointers to optimizers
 */
// -----------------------------------------------------------------------------
typedef std::vector<Optimizer> Optimizers;

// ----------------------------------------------------------------------------
/**
 * @class Optimizer
 * @brief A class that defines the interface for an optimization algorithms
 */
// -----------------------------------------------------------------------------
class Optimizer
{
public:

  Optimizer
    (
    OptimizationProblemPtr optim_problem
    ):
    m_optim_problem(optim_problem),
    m_max_iterations(20),
    m_tol_gradient(0.001),
    m_tol_params(0.00001),
    m_show_iterations(false)
    {};

  virtual ~Optimizer
    () {};

  virtual cv::Mat
  solve
    (
    cv::Mat& frame,
    cv::Mat& former_params
    ) = 0;

  OptimizationProblemPtr
  getOptimizationProblem
    () { return m_optim_problem; };

  void
  setMaxNumIterations
    (
    size_t max_iterations
    )
  { m_max_iterations = max_iterations; };

  void
  setTolGradient
    (
    float tol
    )
  { m_tol_gradient = tol; };

  void
  setTolParams
    (
    float tol
    )
  { m_tol_params = tol; };

  std::vector<double>
  getIterationsCosts
    ()
  { return m_iteration_costs; };

  void
  setShowIterations
    (
    bool show
    )
  { m_show_iterations = show; };

  bool
  getShowIterations
    ()
  { return m_show_iterations; };

protected:
  OptimizationProblemPtr m_optim_problem;
  size_t m_max_iterations;
  float m_tol_gradient;
  float m_tol_params;
  bool m_show_iterations;

  // The 0 index is the first iteration costs and
  // the m_iterations_costs.size()-1 is the
  // last iteration cost. Every time
  std::vector<double> m_iteration_costs;
};

}; }; // namespace

#endif


/*=============================================================================
    Copyright (c) 2010 Tim Blechmann

    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)
=============================================================================*/

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <algorithm>

#include <boost/heap/fibonacci_heap.hpp>

#include "common_heap_tests.hpp"
#include "stable_heap_tests.hpp"
#include "mutable_heap_tests.hpp"
#include "merge_heap_tests.hpp"

template <bool stable, bool constant_time_size>
void run_fibonacci_heap_test(void)
{
    typedef boost::heap::fibonacci_heap<int, boost::heap::stable<stable>,
                                             boost::heap::compare<std::less<int> >,
                                             boost::heap::allocator<std::allocator<int> >,
                                             boost::heap::constant_time_size<constant_time_size>
                                       > pri_queue;

    BOOST_CONCEPT_ASSERT((boost::heap::MutablePriorityQueue<pri_queue>));
    BOOST_CONCEPT_ASSERT((boost::heap::MergablePriorityQueue<pri_queue>));

    run_common_heap_tests<pri_queue>();
    run_iterator_heap_tests<pri_queue>();
    run_copyable_heap_tests<pri_queue>();
    run_moveable_heap_tests<pri_queue>();

    run_merge_tests<pri_queue>();

    run_mutable_heap_tests<pri_queue>();
    run_ordered_iterator_tests<pri_queue>();

    if (stable) {
        typedef boost::heap::fibonacci_heap<q_tester, boost::heap::stable<stable>,
                                            boost::heap::constant_time_size<constant_time_size>
                                           > stable_pri_queue;
        run_stable_heap_tests<stable_pri_queue>();
    }
}

BOOST_AUTO_TEST_CASE( fibonacci_heap_test )
{
    run_fibonacci_heap_test<true, false>();
    run_fibonacci_heap_test<true, true>();

    run_fibonacci_heap_test<false, false>();
    run_fibonacci_heap_test<false, true>();

    RUN_EMPLACE_TEST(fibonacci_heap);
}

BOOST_AUTO_TEST_CASE( fibonacci_heap_compare_lookup_test )
{
    typedef boost::heap::fibonacci_heap<int,
                                       boost::heap::compare<less_with_T>,
                                       boost::heap::allocator<std::allocator<int> > > pri_queue;
    run_common_heap_tests<pri_queue>();
}

BOOST_AUTO_TEST_CASE( fibonacci_heap_leak_test )
{
    typedef boost::heap::fibonacci_heap<boost::shared_ptr<int> > pri_queue;
    run_leak_check_test<pri_queue>();
}


//////////////////////////////////////////////////////////////////////////////
// data::field::survival::function::elapsed_time.hpp         				//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are 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_STATISTICS_DETAIL_DATA_FIELD_SURVIVAL_FUNCTION_ELAPSED_TIME_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DATA_FIELD_SURVIVAL_FUNCTION_ELAPSED_TIME_HPP_ER_2010
#include <boost/statistics/detail/data/field/framework/function/field_value.hpp>
#include <boost/statistics/detail/data/field/survival/fields/entry_time.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace data{
namespace field{

	// Beware that this value may be <0
	template<typename T,typename T1>
	T elapsed_time(
    	const typename field::meta::entry_time<T>::type& f,
        const T1& time
    ){
    	return time - <data::keyword::entry_time>(f);
    }

}// field
}// data
}// detail
}// statistics
}// boost

#endif


#include <algorithm>
#include <assert.h>
#include <experimental/filesystem>
#include <iostream>
#include <numeric>
#include <omp.h>
#include <sstream>
#include <random>
#include <stdio.h>
#include <stdlib.h>
#include <vector>

#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include <vtkBSplineTransform.h>
#include <vtkImageData.h>
#include <vtkLandmarkTransform.h>
#include <vtkMath.h>
#include <vtkMatrixToLinearTransform.h>
#include <vtkPoints.h>

#include "../vtkOpenSURF3D/picojson.h"
#include "../tools/transformIO.h"

#include "imageGroup.h"

using namespace std;

void checkNaN( float value ) {

	if ( !isnan( value ) ) return;
	cout << endl << "Error : NaN" << endl;
	exit( 1 );

}

void ImageGroup::run() {

	// read transform if there are fixed images
	if ( this->numberOfFixedImages ) readAndApplyFixedImagesTransforms();

	this->setupStats();
	this->setupLinearTransforms();
	this->transformPoints();

	if ( this->useRANSAC && this->numberOfFixedImages ) {

		for ( int i = this->numberOfFixedImages; i < this->images.size(); i++ )
			this->RANSAC( i );

		this->transformPoints();
		this->updateStats();
		if ( this->printStats ) displayStats();
		if ( this->printLinear ) this->displayLinearTransforms();

	} else {

		// linear registration
		cout << endl << "Linear registration" << endl;
		for ( int iteration = 0; iteration < this->linearIterations; iteration++ ) {

			Measure measure;
			cout << "Linear registration, iteration " << iteration + 1
				<< "/" << this->linearIterations << endl;

			if ( !( iteration % this->statIntervalUpdate ) ) this->updateStats();
			if ( this->printStats ) displayStats();
			measure.E = this->updateLinearTransforms();
			if ( this->printLinear ) this->displayLinearTransforms();
			this->transformPoints();
			cout << "E = " << measure.E;
			checkNaN( measure.E );
			if ( !this->computeLandmarkDistances( measure ) ) cout << endl;
			this->measures.push_back( measure );

		}

	}

	this->transformPoints( true );
	this->saveDistanceHistograms( "histograms_linear.csv" );

	if ( this->deformableLevels ) {
		cout << endl << "Deformable registration" << endl;
		vector< int > nGrids;
		this->countInliers();

		for ( int level = 0; level < this->deformableLevels; level++ ) {

			cout << endl << "Level " << level + 1 << "/" << this->deformableLevels << endl;
			this->setupDeformableTransforms( level );
			this->transformPoints();
			Measure measure;
			int numberOfGrids = 1;

			for ( int iteration = 0; iteration < this->deformableIterations; iteration++ ) {

				cout << "Level " << level + 1 << "/" << this->deformableLevels
					<< ", Iteration " << iteration + 1 <<"/"
					<< this->deformableIterations << endl;

				if ( !( iteration % this->statIntervalUpdate ) ) this->updateStats();
				if ( this->printStats ) this->displayStats();
				measure.E = this->updateDeformableTransforms();
				cout << "E = " << measure.E;
				checkNaN( measure.E );
				if ( measure.E < 0 ) {

					cout << " creating new grid" << endl;
					numberOfGrids++;
					iteration--;
					this->transformPoints( true );
					this->setupDeformableTransforms( level );
					this->transformPoints();
					continue;

				}

				this->transformPoints();
				if ( !this->computeLandmarkDistances( measure ) ) cout << endl;
				this->measures.push_back( measure );

			}

			this->countInliers();
			cout << "Number of grids for this level : " << numberOfGrids << endl;
			nGrids.push_back( numberOfGrids );
			this->transformPoints( true );

		}

		int totalNumberOfGrids = 0;
		cout << "Grids per level : ";

		for ( int i = 0; i < nGrids.size(); i++ ) {

			totalNumberOfGrids +=nGrids[ i ];
			cout << nGrids[ i ] << " ";

		}

		cout << endl << "Total number of grids : " << totalNumberOfGrids << endl;
	}

	this->displayStats();
	this->saveDistanceHistograms( "histograms.csv" );
	this->saveMeasures( this->outputFileName );
	this->saveTransforms();
	this->saveBoundingBox();
	this->saveLandmarkDistances();
	this->saveTransformedLandmarks();
	if ( this->writePairs ) this->writeLinksDistances();

}

void ImageGroup::setupDeformableTransforms( int level ) {

	double size = this->initialGridSize / pow( 2, level ); // grid size
	float box[ 6 ];
	double spacing[ 3 ];
	double length[ 3 ];
	int dims[ 3 ];
	double origin[ 3 ];
	this->getBoundingBox( box );

	// add margin to bounding box
	for ( int k = 0; k < 3; k++ ) {

		float length = box[ 1 + 2 * k ] - box[ 2 * k ];
		float margin = length * this->boundingBoxMargin;
		box[ 1 + 2 * k ] += margin;
		box[ 2 * k ] -= margin;

	}

	for ( int k = 0; k < 3; k++ ) {

		length[ k ] = box[ 1 + 2 * k ] - box[ 2 * k ];
		dims[ k ] = round( length[ k ] / size );
		if ( dims[ k ] < 1 ) dims[ k ] = 1;
		spacing[ k ] = length[ k ] / dims[ k ];
		origin[ k ] = box[ 2 * k ] - spacing[ k ];
		dims[ k ] += 3;

	}

	cout << "Bounding box : ";
	print( box, 6 );
	cout << "Box length : ";
	print( length, 3 );
	cout << "Grid origin : ";
	print( origin, 3 );
	cout << "Grid spacing : ";
	print( spacing, 3 );
	cout << "Grid dimensions (control points): ";
	print( dims, 3 );

	for ( int i = this->numberOfFixedImages; i < this->images.size(); i++ ) {

		vtkImageData *coeffs = vtkImageData::New();
		coeffs->SetOrigin( origin );
		coeffs->SetSpacing( spacing );
		coeffs->SetDimensions( dims );
		coeffs->AllocateScalars( VTK_FLOAT, 3 );
		float *p = ( float *) coeffs->GetScalarPointer();

		for ( int i = 0; i < 3 * dims[ 0 ] * dims[ 1 ] * dims[ 2 ]; i++ ) {

			p[ i ] = 0;

		}

		vtkBSplineTransform *transform = vtkBSplineTransform::New();
		transform->SetCoefficientData( coeffs );
		transform->SetBorderModeToZero();
		Image *image = &this->images[ i ];
		image->allTransforms->Concatenate( transform );
		image->transform = transform;
		if ( image->gradient ) image->gradient->Delete();
		vtkImageData *gradient = vtkImageData::New();
		gradient->SetOrigin( origin );
		gradient->SetSpacing( spacing );
		gradient->SetDimensions( dims );
		gradient->AllocateScalars( VTK_FLOAT, 4 );
		image->gradient = gradient;

	}

}

// inspired from vtkBSplineTransform.cxx
inline void vtkBSplineTransformWeights( double F[ 4 ], double f ) {

  const double sixth = 1.0/6.0;
  const double half = 0.5;

  double f2 = f * f;

  F[ 3 ] = f2 * f * sixth;
  F[ 0 ] = ( f2 - f ) * half - F[ 3 ] + sixth;
  F[ 2 ] = f + F[ 0 ] - F[ 3 ] * 2;
  F[ 1 ] = 1 - F[ 0 ] - F[ 2 ] - F[ 3 ];

}

double ImageGroup::updateDeformableTransforms() {

	double sDistances = 0, sWeights = 0;

	#pragma omp parallel for reduction( +:sDistances, sWeights )
	for ( int image1 = this->numberOfFixedImages; image1 < this->images.size(); image1++ ) {

		Image &image = this->images[ image1 ];
		float *gradient = ( float* ) image.gradient->GetScalarPointer();
		int dims[ 3 ];
		vtkIdType increments[ 3 ];
		double spacing[ 3 ];
		double origin[ 3 ];
		image.gradient->GetSpacing( spacing );
		image.gradient->GetOrigin( origin );
		image.gradient->GetDimensions( dims );
		image.gradient->GetIncrements( increments );
		double weights[ 3 ][ 4 ];
		int nValues = 4 * dims[ 0 ] * dims[ 1 ] * dims[ 2 ];
		for ( int i = 0; i < nValues; i++ ) gradient[ i ] = 0;
		Stats *statsA = &image.stats;

		for ( auto point = image.points.begin(), end = image.points.end(); point != end; point++ ) {

			const float *pos = point->xyz;
			const float *pA = point->xyz2;
			float sWeight = 0;
			float sDisp[ 3 ] = { 0, 0, 0 };
			// compute point displacement

			for ( auto link = point->links.begin(), endL = point->links.end(); link != endL; link++ ) {

				Image *image2 = &this->images[ link->image ];
				Point *pointB = &image2->points[ link->point ];
				float *pB = pointB->xyz2;
				float dist = 0;

				for ( int k = 0; k < 3; k++ ) {

					float diff = pB[ k ] - pA[ k ];
					dist += diff * diff;

				}

				dist = sqrt( dist );
				float probA = statsA->getInlierProbability( dist );
				float probB = image2->stats.getInlierProbability( dist );
				float weight = min( probA, probB );

				sDistances += weight *weight * dist * dist;
				sWeights += weight * weight;

				if ( weight < this->inlierThreshold ) continue;
				weight *= weight;

				for ( int k = 0; k < 3; k++ )
					sDisp[ k ] += weight * ( pB[ k ] - pA[ k ] );

				sWeight += weight;

			}

			if ( sWeight == 0 ) continue;
			// add point displacement to gradient
			int idZ = 0;

			for ( int k = 0; k < 3; k++) {

				float coord = ( pos[ k ] - origin[ k ] ) / spacing[ k ];
				int intCoord = floor( coord );
				vtkBSplineTransformWeights( weights[ k ], coord - intCoord );
				idZ += ( intCoord - 1 ) * increments[ k ];

			}

			for ( int k = 0; k < 4; k++ ) {

				int idY = idZ;

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

					int idX = idY;

					for ( int i = 0; i < 4; i++ ) {

						double w = weights[ 0 ][ i ]* weights[ 1 ][ j ] * weights[ 2 ][ k ];

						for ( int l = 0; l < 3; l++ )
							gradient[ idX + l ] +=  w * sDisp[ l ];

						gradient[ idX + 3 ] += w * sWeight;
						idX += increments[ 0 ];

					}

					idY += increments[ 1 ];

				}

				idZ += increments[ 2 ];

			}

		}

		// apply gradient to transform
		const float alpha = this->deformableAlpha;
		float *coeffs = ( float *) ( ( vtkBSplineTransform * ) image.transform )
			->GetCoefficientData()->GetScalarPointer();

		int gradientId = 0;
		int coeffsId = 0;

		for ( int i = 0; i < dims[ 0 ] * dims[ 1 ] * dims[ 2 ]; i++ ) {

			const float coeff = gradient[ gradientId + 3 ];

			if ( coeff > 0 ) {

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

					gradient[ gradientId + j ] = coeffs[ coeffsId + j ]
						+ alpha * gradient[ gradientId + j ] / coeff;

				}

			} else {

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

					gradient[ gradientId + j ] = coeffs[ coeffsId + j ];

				}

			}

			gradientId += 4;
			coeffsId += 3;

		}

	}

	// substract average displacement on all transforms;
	int nImages = this->images.size();
	float *coeffs[ nImages ];
	int dims[ 3 ];
	double spacing[ 3 ];

	for ( int image = this->numberOfFixedImages; image < nImages; image++ ) {

		vtkImageData *gradient = this->images[ image ].gradient;
		gradient->GetDimensions( dims );
		gradient->GetSpacing( spacing );
		coeffs[ image ] = ( float * ) gradient->GetScalarPointer();

	}

	int nValues = dims[ 0 ] * dims[ 1 ] * dims[ 2 ];
	int nCoeffs = 0, nBigCoeffs = 0;
	int direction = 0;
	const float maxD = this->maxDisplacementRatio;
	const bool apply = this->numberOfFixedImages == 0;

	//#pragma omp parallel for reduction( +:nCoeffs, nBigCoeffs )
	for ( int i = 0; i < nValues; i++ ) {

		int offset = i * 4;

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

			double sum = 0;

			if ( apply ) {

				for ( int image = this->numberOfFixedImages; image < nImages; image++ ) {

					sum += coeffs[ image ][ offset + j ];

				}

				sum /= nImages;

			}

			for ( int image = this->numberOfFixedImages; image < nImages; image++ ) {

				coeffs[ image ][ offset + j ] -= sum;

				if ( fabs( coeffs[ image ][ offset + j ] ) > maxD * spacing[ j ] )
					nBigCoeffs++;

				nCoeffs++;

			}

		}

	}

	if ( this->guaranteeDiffeomorphism && ( nBigCoeffs > 0 ) ) {

		cout << endl<< "Diffeomorphism is not guaranteed : Iteration canceled" << endl;
		return -1;

	}

	for ( int image1 = this->numberOfFixedImages; image1 < nImages; image1++ ) {

		Image &image = this->images[ image1 ];
		vtkBSplineTransform *transform = ( vtkBSplineTransform * ) image.transform;
		vtkImageData *imageData = transform->GetCoefficientData();
		float *disp = ( float * ) imageData->GetScalarPointer();
		float *newDisp = coeffs[ image1 ];

		int offset = 0;
		int offset2 = 0;

		for ( int i = 0; i < nValues; i++ ) {

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

				disp[ offset + j ] = newDisp[ offset2 + j ];

			}

			offset += 3;
			offset2 += 4;

		}

		image.transform->Update();

	}

	return sqrt( sDistances / sWeights );

}

void ImageGroup::updateStats() {

	// add samples to statistics
	#pragma omp parallel for
	for ( int image1 = 0; image1 < this->images.size(); image1++) {

		Image *image = &this->images[ image1 ];
		Stats *stats = &image->stats;
		stats->reset();

		for ( auto point = image->points.begin(), end = image->points.end(); point != end; point++ ) {

			const float *pA = point->xyz2;
			for ( auto link = point->links.begin(), endL = point->links.end(); link != endL; link++ ) {

				Image *image2 = &this->images[ link->image ];
				Point *pointB = &image2->points[ link->point ];
				float *pB = pointB->xyz2;
				float dist2 = 0;

				for ( int k = 0; k < 3; k++ ) {

					float diff = pB[ k ] - pA[ k ];
					dist2 += diff * diff;

				}

				stats->addSample( sqrt( dist2 ) );

			}

		}

		stats->estimateDistribution();

	}

}

void ImageGroup::displayLinearTransforms() {

	for ( int i = this->numberOfFixedImages; i < this->images.size(); i++) {

		cout << "Image " << i << ", translation=";
		auto matrix = ( ( vtkMatrixToLinearTransform *) this->images[ i ].transform )->GetInput( );

		for ( int k = 0; k < 3; k++ ) {

			cout << matrix->GetElement( k, 3 );
			if ( k < 2 ) cout  << " ";

		}

		cout << endl << "scale=";

		for ( int k = 0; k < 3; k++ ) {

			cout << matrix->GetElement( k, k );
			if ( k < 2 ) cout  << " ";

		}

		cout << endl;

	}

}

void ImageGroup::RANSAC( int imageId ) {

	chrono::time_point< chrono::system_clock> start, end;
	start = chrono::system_clock::now();
	cout << "RANSAC registration for image " << imageId << ": "<<std::flush;

	Image *image = &this->images[ imageId ];
	int nBatches = omp_get_num_procs();
	int batchIterations = numberOfRANSACIterations / nBatches;
	vector < RANSACResult > results;

	#pragma omp parallel for
	for ( int i = 0; i < nBatches; i++ ) {

		RANSACResult result = this->RANSACBatch( imageId, batchIterations, i );
		#pragma omp critical
		results.push_back( result );

	}

	vtkMatrixToLinearTransform *transform = ( vtkMatrixToLinearTransform *) image->transform;
	auto matrix = transform->GetInput();
	int maxNumberOfInliers = 0;

	for ( auto res = results.begin(); res != results.end(); res++ ) {

		int nInliers = res->first;

		if ( nInliers > maxNumberOfInliers ) {

			matrix->DeepCopy( res->second );
			maxNumberOfInliers = nInliers;

		}

		res->second->Delete();

	}

	float transformed[ 3 ];
	auto &pts = image->points;
	vtkPoints *source = vtkPoints::New();
	vtkPoints *target = vtkPoints::New();
	source->SetDataTypeToFloat();
	target->SetDataTypeToFloat();
	vtkLandmarkTransform *trans = vtkLandmarkTransform::New();
	trans->SetModeToSimilarity();
	trans->SetSourceLandmarks( source );
	trans->SetTargetLandmarks( target );
	float maxDistance2 = pow( this->RANSACInlierDistance, 2 );

	for ( auto pointA = pts.begin(), end = pts.end(); pointA != end; pointA++ ) {

		transform->TransformPoint( pointA->xyz, transformed );

		for ( auto link = pointA->links.begin(), endL = pointA->links.end(); link != endL; link++ ) {

			Image *image2 = &this->images[ link->image ];
			Point *pointB = &image2->points[ link->point ];
			float *pB = pointB->xyz2;
			if ( vtkMath::Distance2BetweenPoints( transformed, pB ) < maxDistance2 ) {
				source->InsertNextPoint( pointA->xyz );
				target->InsertNextPoint( pB );
			}

		}

	}

	source->Modified();
	target->Modified();
	trans->Update();
	matrix->DeepCopy( trans->GetMatrix() );
	trans->Delete();
	source->Delete();
	target->Delete();
	end = chrono::system_clock::now();
	cout << maxNumberOfInliers << " inliers, computed in " << chrono::duration<float>(end - start).count() << "s" << endl;

}

ImageGroup::RANSACResult ImageGroup::RANSACBatch( int imageId, int nIterations, int batch ) {

	int numberOfRansacPoints = 4;
	vtkPoints *source = vtkPoints::New();
	vtkPoints *target = vtkPoints::New();
	source->SetDataTypeToFloat();
	target->SetDataTypeToFloat();
	vtkLandmarkTransform *trans = vtkLandmarkTransform::New();
	trans->SetModeToSimilarity();
	trans->SetSourceLandmarks( source );
	trans->SetTargetLandmarks( target );
	int maxNumberOfInliers = 0;
	float maxDistance2 = pow( this->RANSACInlierDistance, 2 );
	Image *image = &this->images[ imageId ];
	vtkMatrix4x4 *matrix = vtkMatrix4x4::New();
	auto matrix2 = trans->GetMatrix();

	auto &pts = image->points;
	int nPoints = pts.size();
	mt19937 rng( batch * 1000 );

	for ( int i = 0; i < nIterations; i++ ) {

		source->Reset();
		target->Reset();

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

			while ( true ) {

				int pt = rng() % nPoints;
				Point &point = pts[ pt ];
				auto &links = point.links;
				auto size = links.size();
				if ( size == 0 ) continue;
				source->InsertNextPoint( point.xyz );
				int linkId = rng() % size;
				auto &link = links[ linkId ];
				target->InsertNextPoint(
					this->images[ link.image ].points[ link.point ].xyz );
				break;

			}

		}

		source->Modified();
		target->Modified();
		trans->Update();

		int nInliers = 0;
		float transformed[ 3 ];

		for ( auto pointA = pts.begin(), end = pts.end(); pointA != end; pointA++ ) {

			trans->TransformPoint( pointA->xyz, transformed );

			for ( auto link = pointA->links.begin(), endL = pointA->links.end(); link != endL; link++ ) {

				Image *image2 = &this->images[ link->image ];
				Point *pointB = &image2->points[ link->point ];
				float *pB = pointB->xyz2;
				if ( vtkMath::Distance2BetweenPoints( transformed, pB ) < maxDistance2 )
					nInliers++;

			}

		}

		if ( maxNumberOfInliers < nInliers ) {

			maxNumberOfInliers = nInliers;
			matrix->DeepCopy( matrix2 );

		}

	}

	source->Delete();
	target->Delete();
	trans->Delete();
	return make_pair( maxNumberOfInliers, matrix );

}

void ImageGroup::setupLinearTransforms() {

	float box[ 6 ];
	float centers[ this->images.size() ][ 3 ];
	float average[ 3 ] = { 0, 0, 0 };

	for ( int i = 0; i < this->images.size(); i++ ) {

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

			box[ 2 * j ] = 1e9;
			box[ 2 * j + 1 ] = -1e9;

		}

		this->images[ i ].expandBoundingBox( box );

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

			float center = 0.5 * ( box[ 1 + 2 * j ] + box[ 2 * j ] );
			centers[ i ][ j ] = center;
			if ( i < this->images.size() - this->numberOfFixedImages )
				average[ j ] += center / ( float ) ( this->images.size() - this->numberOfFixedImages );

		}

	}

	// center volumes
	for ( int i = this->numberOfFixedImages; i < this->images.size() ; i++) {

		Image *image = &this->images[ i ];
		image->transform = vtkMatrixToLinearTransform::New();
		image->allTransforms = vtkGeneralTransform::New();
		image->allTransforms->PostMultiply();
		image->allTransforms->Concatenate( image->transform );

		vtkMatrix4x4 *matrix = vtkMatrix4x4::New();
		matrix->Identity();

		for ( int j = 0; j < 3; j++ )
			matrix->SetElement( j, 3, average[ j ] - centers[ i ][ j ] );

		( ( vtkMatrixToLinearTransform *) image->transform )->SetInput( matrix );

	}

}

void ImageGroup::saveDistanceHistograms( const char *file ) {

	std::fstream fs;
	fs.open( file, fstream::out | fstream::trunc );
	int maxSize = 0;

	for ( int i = 0; i < this->images.size(); i++) {

		Stats *stats = &this->images[ i ].stats;
		stats->getHistogram();
		maxSize = max( maxSize, ( int ) stats->histogram.size() );
		fs << "image " << i ;
		if ( i < this->images.size() - 1 ) fs << ",";
			else fs << endl;

	}

	for ( int d = 0; d < maxSize; d++ ) {

		for ( int i = 0; i < this->images.size(); i++) {

			Stats *stats = &this->images[ i ].stats;

			if ( stats->histogram.size() <= d ) fs << 0;
				else fs << stats->histogram[ d ];

			if ( i < this->images.size() - 1 ) fs << ",";
				else fs << endl;

		}

	}

	fs.close();

}

void ImageGroup::saveIndividualDistanceHistograms() {

	for ( int i = 0; i < this->images.size(); i++) {

		ostringstream file;
		file << "histogram" << i << ".csv";
		this->images[ i ].stats.saveHistogram( file.str().c_str() );

	}

}

void ImageGroup::displayStats() {

	for ( int i = 0; i < this->images.size() ; i++) {

		cout << "Stats " <<  i << ":";
		this->images[ i ].stats.displayParameters();

	}

}

void ImageGroup::transformPoints( bool apply ) {

	#pragma omp parallel for
	for ( int i = this->numberOfFixedImages; i < this->images.size() ; i++ ) {

		Image *image = &this->images[ i ];
		image->transformPoints();

		if ( apply ) {

			for ( auto point = image->points.begin(), end = image->points.end(); point != end; point++ ) {

				for ( int k = 0; k < 3; k++ )
					point->xyz[ k ] = point->xyz2[ k ];

			}

		}

	}

}

bool comparePairs( const vector< float > &p1, const vector< float > & p2) {

    return ( p1[ 4 ] < p2[ 4 ] );

}

void ImageGroup::writeLinksDistances() {

	vector < vector< float > > pairs;

	for ( int i1 = 0; i1 < this->images.size(); i1++ ) {

		Image &image = this->images[ i1 ];
		Stats *stats = &image.stats;

		for ( int p1 = 0; p1 != image.points.size(); p1++ ) {

			auto pointA = &image.points[ p1 ];
			float *pA = pointA->xyz2;

			for ( auto link = pointA->links.begin(), endL = pointA->links.end(); link != endL; link++ ) {

				int i2 = link->image;
				Image *image2 = &this->images[ i2 ];
				int p2 = link->point;
				Point *pointB = &image2->points[ p2 ];
				float *pB = pointB->xyz2;
				float dist = 0;

				for ( int k = 0; k < 3; k++ ) {

					float diff = pB[ k ] - pA[ k ];
					dist += diff * diff;

				}

				dist = sqrt( dist );
				vector< float > pair;
				pair.push_back( i1 );
				pair.push_back( p1 );
				pair.push_back( i2 );
				pair.push_back( p2 );
				pair.push_back( dist );
				pair.push_back( stats->getInlierProbability( dist ) );
				pairs.push_back( pair );

			}

		}

	}

    sort( pairs.begin(), pairs.end(), comparePairs );

	ofstream file( "pairs.csv.gz", ios_base::out | ios_base::binary);
	boost::iostreams::filtering_streambuf<boost::iostreams::output> outbuf;
    outbuf.push(boost::iostreams::gzip_compressor());
    outbuf.push(file);
    ostream out(&outbuf);

	for ( int i = 0; i < pairs.size(); i++ ) {

		vector< float > &pair = pairs[ i ];

		for ( int j = 0; j < pair.size(); j++ ) {

			out << pair[ j ];
			if ( j < pair.size() - 1 ) out << ",";

		}

		if ( i  < pairs.size() - 1 ) out << "\n";
	}

	boost::iostreams::close( outbuf );
	file.close();

}

void ImageGroup::countInliers() {

	int nPairs = 0, nInliers = 0, nOutliers = 0;

	#pragma omp parallel for reduction( +:nPairs, nInliers, nOutliers )
	for ( int image1 = this->numberOfFixedImages; image1 < this->images.size(); image1++ ) {

		Image &image = this->images[ image1 ];
		Stats *statsA = &image.stats;

		for ( auto pointA = image.points.begin(), end = image.points.end(); pointA != end; pointA++ ) {

			float *pA = pointA->xyz2;

			for ( auto link = pointA->links.begin(), endL = pointA->links.end(); link != endL; link++ ) {

				Image *image2 = &this->images[ link->image ];
				Point *pointB = &image2->points[ link->point ];
				float *pB = pointB->xyz2;
				float dist = 0;

				for ( int k = 0; k < 3; k++ ) {

					float diff = pB[ k ] - pA[ k ];
					dist += diff * diff;

				}

				dist = sqrt( dist );
				float probA = statsA->getInlierProbability( dist );
				float probB = image2->stats.getInlierProbability( dist );
				float weight = min( probA, probB );
				nPairs++;

				if ( weight < this->inlierThreshold )
					nOutliers++;
				else
					nInliers++;

			}

		}

	}

	cout << "Stats:" << endl;
	cout << nPairs << " half pairs" << endl;
	cout << nInliers << " inliers" << endl;
	cout << nOutliers << " outliers" << endl;
	cout << "Outlier ratio (%): " << ( float ) 100 * nOutliers / nPairs << endl;

}

double ImageGroup::updateLinearTransforms() {

	double sDistances = 0, sWeights = 0;

	#pragma omp parallel for reduction( +:sDistances, sWeights )
	for ( int image1 = this->numberOfFixedImages; image1 < this->images.size(); image1++ ) {

		float diff[3];
		double sDisp[ 3 ] = { 0, 0, 0 };
		double sPosA[ 3 ] = { 0, 0, 0 };
		double sPosA2[ 3 ] = { 0, 0, 0 };
		double sPosB[ 3 ] = { 0, 0, 0 };
		double sPosB2[ 3 ] = { 0, 0, 0 };
		double sWeight = 0;
		Image &image = this->images[ image1 ];
		Stats *statsA = &image.stats;

		for ( auto pointA = image.points.begin(), end = image.points.end(); pointA != end; pointA++ ) {

			float *pA = pointA->xyz2;

			for ( auto link = pointA->links.begin(), endL = pointA->links.end(); link != endL; link++ ) {

				Image *image2 = &this->images[ link->image ];
				Point *pointB = &image2->points[ link->point ];
				float *pB = pointB->xyz2;
				float dist = 0;

				for ( int k = 0; k < 3; k++ ) {

					diff[ k ] = pB[ k ] - pA[ k ];
					dist += diff[ k ] * diff[ k ];

				}

				dist = sqrt( dist );
				float probA = statsA->getInlierProbability( dist );
				float probB = image2->stats.getInlierProbability( dist );
				float weight = min( probA, probB );

				sDistances += weight * weight * dist * dist;
				sWeights += weight * weight;

				for ( int k = 0; k < 3; k++ ) {

					float posA = pA[ k ];
					float posB = pB[ k ];
					sDisp[ k ] += weight * diff[ k ];
					sPosA[ k ] += weight * posA;
					sPosB[ k ] += weight * posB;
					sPosA2[ k ] += weight * posA * posA;
					sPosB2[ k ] += weight * posB * posB;

				}

				sWeight += weight;

			}

		}

		auto matrix = ( ( vtkMatrixToLinearTransform *) image.transform )->GetInput();
		for ( int k = 0; k < 3; k++ ) {

			float sWeight2 = sWeight * sWeight;
			float scale = matrix->GetElement( k, k );
			float newScale = this->useScale ? pow(
				( sWeight * sPosB2[ k ] - sPosB[ k ] * sPosB[ k ] ) /
				( sWeight * sPosA2[ k ] - sPosA[ k ] * sPosA[ k ] ),
				0.5 * this->linearAlpha ) : 1.0;

			if ( isnan( newScale ) ) continue;
			matrix->SetElement( k, k, scale * newScale );
			float translation = matrix->GetElement( k, 3 );
			if ( isnan( translation ) ) continue;
			matrix->SetElement( k, 3, translation
				+ this->linearAlpha * sDisp[ k ] / sWeight
				+ sPosA[ k ] * ( 1 - newScale ) / sWeight );

		}

		image.transform->Update();

	}

	return sqrt( sDistances / sWeights );

}

void ImageGroup::setupStats() {

	// add samples to statistics
	for ( auto image = this->images.begin(); image != this->images.end(); image++ ) {

		Stats *stats = &image->stats;

		for ( auto point = image->points.begin(); point != image->points.end(); point++ ) {

			for ( auto link = point->links.begin(); link != point->links.end(); link++ ) {

				stats->addSlot();

			}

		}

	}

}

void ImageGroup::readLandmarks( const char *path ) {

	vector < string > files;

	for( const auto &p: experimental::filesystem::directory_iterator( path ) ) {

		files.push_back ( p.path() );

	}

	sort( files.begin(), files.end() );

	for ( int i = 0; i < files.size(); i++ ) {

		ifstream infile( files[ i ] );
		string line;

		while ( getline( infile, line ) ) {

			if ( line[ 0 ] == '#' ) continue;
			int pos = line.find( ',');
			string name = line.substr( 0, pos );
			line.erase( 0, pos + 1 );
			Landmark landmark;
			landmark.image = i;

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

				int pos = line.find( ',');
				string coord = line.substr( 0, pos );
				line.erase( 0, pos + 1 );
				landmark.xyz[ j ] = stof( coord );
				if ( j < 2 && this->invertLandmarksCoordinates )
					landmark.xyz[ j ] *= -1; // get opposite x and y coordinates!

			}

			auto arr = this->landmarks.find( name );
			Landmarks *landmarks;

			if ( arr == this->landmarks.end() ) {

				landmarks = this->landmarks[ name ] = new Landmarks;

			} else landmarks = arr->second;

			landmarks->push_back( landmark );

		}

	}

}

bool ImageGroup::computeLandmarkDistances( Measure &measure ) {

	if ( !this->landmarks.size() ) return false;
	vector< float > distances;
	vector < Landmarks* > landmarkV;

	for ( auto iter = this->landmarks.begin(); iter != landmarks.end(); iter++) {

		landmarkV.push_back( iter->second );

	}

	#pragma omp declare reduction (merge : std::vector<float> : omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))

	#pragma omp parallel for reduction( merge: distances )
	for ( int i = 0; i < landmarkV.size(); i++ ) {

		Landmarks *landmarks = landmarkV[ i ];
		float center[] = { 0, 0, 0 };
		int n = 0;

		for ( int i = 0; i < landmarks->size(); i++ ) {

			Landmark &landmark = landmarks[ 0 ][ i ];
			if ( landmark.image > ( this->images.size() - 1 ) ) continue;
			n++;
			float xyz2[ 3 ];
			this->images[ landmark.image ].allTransforms->TransformPoint(
				landmark.xyz, xyz2 );

			for ( int k = 0; k < 3; k++ ) center[ k ] += xyz2[ k ];

		}

		for ( int k = 0; k < 3; k++ ) center[ k ] /= n;

		for ( int i = 0; i < landmarks->size(); i++ ) {

			Landmark &landmark = landmarks[ 0 ][ i ];
			if ( landmark.image > ( this->images.size() - 1 ) ) continue;
			float distance2 = 0;
			float xyz2[ 3 ];
			this->images[ landmark.image ].allTransforms->TransformPoint(
				landmark.xyz, xyz2 );

			for ( int k = 0; k < 3; k++ ) {
				float diff = xyz2[ k ] - center[ k ];
				distance2 += diff * diff;
			}

			distances.push_back( sqrt( distance2 ) );

		}

	}

	double sum = accumulate( distances.begin(), distances.end(), 0.0 );
	double mean = sum / distances.size();

	double sq_sum = std::inner_product( distances.begin(), distances.end(),
		distances.begin(), 0.0 );

	auto max = max_element( distances.begin(), distances.end());

	double stdev = sqrt( sq_sum / distances.size() - mean * mean);
	cout << ", " << distances.size() << " landmarks:max=" << *max
		<< ",average="<< mean << ",stdev=" << stdev << endl;

	measure.landmarkAv = mean;
	measure.landmarkMax = *max;
	measure.landmarkSTD = stdev;
	return true;

}

bool ImageGroup::saveTransformedLandmarks() {

	if ( !this->landmarks.size() ) return false;
	picojson::object transformedLandmarks;

	for ( auto iter = this->landmarks.begin(); iter != landmarks.end(); iter++) {

		picojson::array landmarksArray;
		auto &arr = *iter->second;

		for ( auto landmark = arr.begin(); landmark != arr.end(); landmark++) {

			if ( landmark->image > ( this->images.size() - 1 ) ) continue;
			float xyz2[ 3 ];
			this->images[ landmark->image ].allTransforms
				->TransformPoint( landmark->xyz, xyz2 );

			picojson::object land;
			land[ "image" ] = picojson::value( ( double ) landmark->image );
			picojson::array coords;

			for ( int i = 0; i < 3; i++ )
				coords.push_back( picojson::value( xyz2[ i ] ) );

			land[ "xyz" ] = picojson::value( coords );
			landmarksArray.push_back( picojson::value( land ) );

		}

		transformedLandmarks[ iter->first ] = picojson::value( landmarksArray );

	}

	std::fstream fs;
	fs.open( "transformedLandmarks.json", fstream::out | fstream::trunc );
	fs << picojson::value( transformedLandmarks ).serialize();
	fs.close();
	return true;

}

void ImageGroup::saveLandmarkDistances() {

	if ( !this->landmarks.size() ) return;
	std::fstream fs;
	fs.open( "distances.txt", fstream::out | fstream::trunc );

	for ( auto iter = this->landmarks.begin(); iter != landmarks.end(); iter++) {

		Landmarks *landmarks = iter->second;
		float center[] = { 0, 0, 0 };
		int n = 0;

		for ( int i = 0; i < landmarks->size(); i++ ) {

			Landmark &landmark = landmarks[ 0 ][ i ];
			if ( landmark.image > ( this->images.size() - 1 ) ) continue;
			n++;
			float xyz2[ 3 ];
			this->images[ landmark.image ].allTransforms->TransformPoint(
				landmark.xyz, xyz2 );

			for ( int k = 0; k < 3; k++ ) center[ k ] += xyz2[ k ];

		}

		for ( int k = 0; k < 3; k++ ) center[ k ] /= n;

		for ( int i = 0; i < landmarks->size(); i++ ) {

			Landmark &landmark = landmarks[ 0 ][ i ];
			if ( landmark.image > ( this->images.size() - 1 ) ) continue;
			float distance2 = 0;
			float xyz2[ 3 ];
			this->images[ landmark.image ].allTransforms->TransformPoint(
				landmark.xyz, xyz2 );

			for ( int k = 0; k < 3; k++ ) {
				float diff = xyz2[ k ] - center[ k ];
				distance2 += diff * diff;
			}

			fs << sqrt( distance2 ) << "," << iter->first << "," << landmark.image << endl;

		}

	}

	fs.close();

}

void ImageGroup::readPairs( char *inputFile ) {

	FILE *file = fopen( inputFile ,"rb");
	unsigned short nImages;
	int unused;
	unused = fread(&nImages, sizeof(unsigned short), 1, file);
	this->images.resize( nImages );
	int numberOfPairs = 0;

	for ( int i = 0; i < nImages; i++) {

		unsigned short nameLength=18;
		unused = fread( &nameLength, sizeof(unsigned short), 1, file);
		char name[ nameLength + 1 ];
		unused = fread( name, sizeof(char), nameLength, file);
		name[ nameLength ] = 0;
		Image *image = &this->images[ i ];
		unused = fread( image->refTranslation, sizeof( double ), 3, file );
		pointIdType nPoints;
		unused = fread( &nPoints, sizeof( pointIdType ), 1, file );
		image->points.resize( nPoints );

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

			Point *pt = & image->points[ j ];
			unused = fread( pt->original_xyz, sizeof( float ), 3, file );
			for ( int k = 0; k < 3; k++ ) pt->xyz[ k ] = pt->original_xyz[ k ];
			unused = fread( pt->other, sizeof( float ), 3, file );

		}

	}

	unsigned short image1, image2;

	while ( unused = fread( &image1, sizeof( unsigned short ), 1, file ) ) {

		unused = fread( &image2, sizeof( unsigned short ), 1, file );
		unsigned int size;
		unused = fread(&size, sizeof(unsigned int), 1, file);

		if ( !size ) {

			cout << "Error : number of pairs is 0" << endl;
			exit( 1 );

		}

		for ( int i = 0; i < size; i++ ) {

			pointIdType point1, point2;
			unused = fread(&point1, sizeof(pointIdType), 1, file);
			unused = fread(&point2, sizeof(pointIdType), 1, file);
			Link link1;
			Link link2;
			link1.image = image1;
			link1.point = point1;
			link2.image = image2;
			link2.point = point2;
			this->images[ image1 ].points[ point1 ].links.push_back( link2 );
			this->images[ image2 ].points[ point2 ].links.push_back( link1 );

		}

		numberOfPairs += size;

	}

	fclose(file);
	cout << numberOfPairs << " pairs read : " << numberOfPairs * 2 << " half pairs " << endl;

}

void ImageGroup::readAndApplyFixedImagesTransforms() {

	if ( this->fixedTransformsDirectory )
		cout << "Reading transforms in directory " << this->fixedTransformsDirectory << endl;

	#pragma omp parallel for
	for ( int i = 0; i < this->numberOfFixedImages; i++ ) {

		Image *image = &this->images[ i ];

		if ( this->fixedTransformsDirectory ) {

			ostringstream file;
			file << this->fixedTransformsDirectory << "/" << i << ".json";
			image->allTransforms = readTransform( file.str().c_str() );

		} else {

			image->transform = vtkMatrixToLinearTransform::New();
			image->allTransforms = vtkGeneralTransform::New();
			image->allTransforms->PostMultiply();
			image->allTransforms->Concatenate( image->transform );
			vtkMatrix4x4 *matrix = vtkMatrix4x4::New();
			matrix->Identity();
			( ( vtkMatrixToLinearTransform *) image->transform )->SetInput( matrix );

		}

		for ( auto point = image->points.begin(); point != image->points.end(); point++ ) {

			image->allTransforms->TransformPoint( point->xyz, point->xyz2 );

			for ( int k = 0; k < 3; k++ )
				point->xyz[ k ] = point->xyz2[ k ];

		}

	}

}

void ImageGroup::saveTransforms() {

	experimental::filesystem::create_directory( "transforms" );

	// output to .tfm and .json
	#pragma omp parallel for
	for ( int image1 = this->numberOfFixedImages; image1 < this->images.size(); image1++) {

		vtkGeneralTransform *trans = this->images[ image1 ].allTransforms;
		ostringstream file;
		file << "transforms/" << image1 << ".json";
		writeFrogJSON( trans, file.str().c_str() );

	}

}

void ImageGroup::saveMeasures( const char *file ) {

	std::fstream fs;
	fs.open( file, fstream::out | fstream::trunc );
	fs << "Iteration, E, landmarkAv, landmarkMax, landmarkSTD" << endl;

	for ( auto i = this->measures.begin(); i != this->measures.end(); i++ ) {

		fs << i - this->measures.begin() << "," << i->E
			<< "," << i->landmarkAv << "," << i->landmarkMax
			<< ',' << i->landmarkSTD << endl;

	}

	fs.close();

}

void ImageGroup::saveBoundingBox() {

	fstream fs;
	fs.open( "bbox.json", fstream::out | fstream::trunc );
	float box[ 6 ];
	this->getBoundingBox( box, true );
	fs << "{ \"bbox\" : [ [";
	fs << box[ 0 ] << "," << box [ 2 ] << "," << box [ 4 ] << "], [";
	fs << box[ 1 ] << "," << box [ 3 ] << "," << box [ 5 ] << "] ] }";
	fs.close();

}

void ImageGroup::getBoundingBox( float *box, bool all ) {

	for ( int i = 0; i < 3; i++ ) {

		box[ 2 * i ] = 1e9;
		box[ 2 * i + 1 ] = -1e9;

	}

	for (int i = all ? 0 : this->numberOfFixedImages; i < this->images.size(); i++ ) {

		this->images[ i ].expandBoundingBox( box );

	}

}


#include "CsgLoader.hpp"

#include <Eigen/Geometry>

#include <iostream>

namespace {

  CsgNode* loadNode (const json11::Json theData, const Mat4f& theTransform);

  CsgNode* collectNodes (const CsgOperation theOp,
                         const json11::Json theData,
                         const int theStartIndex,
                         const Mat4f& theTransform) {

    auto& anItems = theData.array_items();

    size_t anActualSize = anItems.size() - theStartIndex;

    if (anActualSize == 0) {
      throw std::runtime_error ("The range should contain at least one element");
    }
    else if (anActualSize == 1) {
      return loadNode (anItems[theStartIndex], theTransform);
    }

    return new CsgOperationNode (theOp,
                                 loadNode (anItems[theStartIndex], theTransform),
                                 collectNodes (theOp, theData, theStartIndex + 1, theTransform));
  }

  CsgNode* loadNode (const json11::Json theData, const Mat4f& theTransform) {

    if (theData.is_null()) {
      throw std::runtime_error ("Unexpected NULL object");
    }

    if (theData.is_array()) {
      return loadNode (theData.array_items()[0], theTransform);
    }

    if (!theData.is_object()) {
      throw std::runtime_error ("Dictionary expected: " + theData.dump());
    }

    std::string aType = theData["type"].string_value();

    if (aType == "CSG file") {

      // theStream << theIndent << "# " << theData["version-name"].string_value() <<
      //                           " " << theData["version-major"].dump() <<
      //                           "." << theData["version-minor"].dump() << std::endl;

      return loadNode (theData["contents"], theTransform);
    }
    else if (aType == "group") {

      return collectNodes (CSG_OP_UNION, theData["objects"], 0, theTransform);
    }
    else if (aType == "multmatrix") {

      Mat4f aMatrix = Mat4f::Identity();

      // OpenScad compatibility matrix
      if (theData["properties"].is_array()) {

        for (int i = 0; i < 4; ++i) {
          auto anInputRow = theData["properties"][i];
          aMatrix.row (i) = Vec4f ((float)anInputRow[0].number_value(),
                                   (float)anInputRow[1].number_value(),
                                   (float)anInputRow[2].number_value(),
                                   (float)anInputRow[3].number_value());
        }
      }
      else {
        // TODO: fetch matrix
      }

      return collectNodes (CSG_OP_UNION, theData["objects"], 0, theTransform * aMatrix);
    }
    else if (aType == "union") {
      CsgNode* aFirstNode = loadNode (theData["objects"][0], theTransform);

      // OpenScad compatibility
      if (theData["objects"].array_items().size() == 1) {
        return aFirstNode;
      }

      CsgNode* aSecondNode = collectNodes (CSG_OP_UNION, theData["objects"], 1, theTransform);
      return new CsgOperationNode (CSG_OP_UNION, aFirstNode, aSecondNode);
    }
    else if (aType == "difference") {
      CsgNode* aFirstNode = loadNode (theData["objects"][0], theTransform);
      CsgNode* aSecondNode = collectNodes (CSG_OP_UNION, theData["objects"], 1, theTransform);
      return new CsgOperationNode (CSG_OP_MINUS, aFirstNode, aSecondNode);
    }
    else if (aType == "intersection") {
      CsgNode* aFirstNode = loadNode (theData["objects"][0], theTransform);
      CsgNode* aSecondNode = collectNodes (CSG_OP_INTER, theData["objects"], 1, theTransform);
      return new CsgOperationNode (CSG_OP_INTER, aFirstNode, aSecondNode);
    }
    // else if (aType == "smin") {
    // }
    else if (aType == "cube") {

      auto aCubeSize = theData["properties"]["size"];

      Eigen::Affine3f aBoxTransform;
      if (aCubeSize.is_null()) {
        aBoxTransform = Eigen::Scaling (1.0f, 1.0f, 1.0f);
      }
      else {
        aBoxTransform = Eigen::Scaling ((float)aCubeSize[0].number_value(),
                                        (float)aCubeSize[1].number_value(),
                                        (float)aCubeSize[2].number_value());
      }
      return new CsgPrimitiveNode (CSG_BOX, theTransform * aBoxTransform.matrix());
    }
    else if (aType == "sphere") {

      double aRadius = theData["properties"]["r"].number_value();

      Eigen::Affine3f aSphereTransform;
      aSphereTransform = Eigen::Scaling ((float)(aRadius > 0.0 ? aRadius : 1.0));
      return new CsgPrimitiveNode (CSG_SPHERE, theTransform * aSphereTransform.matrix());
    }
    // else if (aType == "cylinder") {
    // }
    // else if (aType == "cone") {
    // }
    else {
      throw std::runtime_error ("Unknown object type: " + theData["type"].dump());
    }
  }
}

CsgNode* CsgLoader::LoadTree (const json11::Json theSerializedTree)
{
  Mat4f theTransform = Mat4f::Identity();
  return loadNode (theSerializedTree, theTransform);
}


#ifndef GFX_HPP
#define GFX_HPP

#define _USE_MATH_DEFINES
#include <vector>
#include <random>
#include <SDL2/SDL.h>
#include <GL/glew.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace fox
{
	class counter;
}
class physics;

class gfx
{
public:

	gfx();

	void init();
	void deinit();
	void render();
	void resize(int w, int h);
	int main_loop();

private:
	void print_info();
	void load_shaders();

	fox::counter *fps_counter;
	fox::counter *update_counter;
	fox::counter *perf_counter;
	SDL_Window *window;
	SDL_GLContext context;
	int done;
	int win_w;
	int win_h;

	/**
	 * @brief A random generator that is initialized in the constructor
	 */
	std::mt19937_64 generator;

	double G = 6.67408e-11;
	uint16_t obj_count;

	// an empty vertex array object to bind to
	uint32_t default_vao;
	Eigen::Vector3f eye, target, up;
	Eigen::Affine3f V;
	Eigen::Affine3f M;
	Eigen::Projective3f P, MVP;

	GLuint point_render_program, shader_vert_id, shader_frag_id;
	GLuint x_vbo;
	std::vector<float> x_gfx;

	const static uint8_t perf_array_size = 8;
	double phys_times[perf_array_size];
	double render_times[perf_array_size];
	double phys_time;
	double render_time;
	uint8_t perf_index;
	double total_time;

	physics *p;
};

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!
  @ingroup group-arithmetic
    Function object implementing angle capabilities

    Computes the angular orientation of its parameter.

    This is a convenient alias of @ref arg
  **/
  const boost::dispatch::functor<tag::angle_> angle = {};
} }
#endif

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

#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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 NT2_TOOLBOX_BOOST_MATH_FUNCTION_ERFC_HPP_INCLUDED
#define NT2_TOOLBOX_BOOST_MATH_FUNCTION_ERFC_HPP_INCLUDED
#include <nt2/include/simd.hpp>
#include <nt2/include/functor.hpp>
#include <boost/math/special_functions/erf.hpp>

namespace nt2 { namespace boost_math { namespace tag
  {
    struct erfc_ {};
  }
  NT2_FUNCTION_IMPLEMENTATION(boost_math::tag::erfc_, erfc, 1)
  } }

#include <nt2/toolbox/boost_math/function/scalar/erfc.hpp>
// #include NT2_BOOST_MATH_INCLUDE(erfc.hpp)

#endif

// modified by jt the 29/12/2010


#include <algorithm>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <map>
#include <numeric>
#include <sstream>
#include <stdint.h>
#include <string>
#include <vector>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/bimap.hpp>
#include "boost/multi_array.hpp"
#include <cassert>
#include <regex>

using namespace std;

// Ideally use a bimap here for both turns, but it appears
// an initializer_list constructor doesn't exist yet in boost :(
const std::map<const char, const char> RIGHT_TURNS {
  {'N', 'E'},
  {'E', 'S'},
  {'S', 'W'},
  {'W', 'N'}};

const std::map<const char, const char> LEFT_TURNS {
  {'N', 'W'},
  {'W', 'S'},
  {'S', 'E'},
  {'E', 'N'}};

const std::map<const char, const tuple<int, int> > MOVE_MAP = {
  {'N', std::make_tuple(0, 1)},
  {'E', std::make_tuple(1, 0)},
  {'S', std::make_tuple(0, -1)},
  {'W', std::make_tuple(-1, 0)}
};

template <typename K, typename V>
V constmap_get(const K k, const std::map<const K, V>& m) {
  auto x = m.find(k);
  assert(x != m.end());
  return x->second;
}

enum class Direction {
  North, // 0
  East, // 1
  South, // 2
  West // 3
};

char direction_to_facing(Direction d) {
  const char compass_char[] = {'N', 'E', 'S', 'W'};
  return compass_char[static_cast<int>(d)];
}

std::ostream& operator<<(std::ostream& os, Direction d) {
  os << direction_to_facing(d);
  return os;
}

struct Rover {

  tuple<int, int> xy;
  char _facing;

  const char facing()const { return _facing; }

  Rover(int x, int y, char f):
    xy(x, y),
    _facing(f) {
    }

  void left() {
    _facing = constmap_get(facing(), LEFT_TURNS);
    cout << "turned left to " << facing() << endl;
  }
  void right() {
    _facing = constmap_get(facing(), RIGHT_TURNS);
    cout << "turned right to " << facing() << endl;
  }
  void move() {
    int dx, dy;
    std::tie(dx, dy) = constmap_get(facing(), MOVE_MAP);
    cout << "moving (" << dx << ", " << dy << ")\n";
  }
};

std::ostream& operator<<(std::ostream& os, const Rover& rhs) {
  os << std::get<0>(rhs.xy) << " " << std::get<1>(rhs.xy) <<
    " " << rhs.facing();
  return os;
}

typedef std::shared_ptr<Rover> RoverPtr;

RoverPtr make_rover(const std::string& position) {
  std::regex e("(\\d+) (\\d+) ([NESW])");

  std::smatch sm;
  if (std::regex_match(position, sm, e)) {
    cout << "regex matched" << endl;

    int x = std::stoi(sm[1]);
    int y = std::stoi(sm[2]);
    std::string facing_str = sm[3];

    return RoverPtr(new Rover(x, y, facing_str[0]));
  } else {
    cout << "didn't match" << endl;
    return RoverPtr();
  }
}

int main () {
  // Create a 3D array that is 3 x 4 x 2
  typedef boost::multi_array<bool, 2> array_type;
  typedef array_type::index index;

  array_type A(boost::extents[3][3]);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      A[i][j] = false;
    }
  }

  std::map<char, std::function<void(Rover&)>> command_map{
    {'M', &Rover::move},
      {'L', &Rover::left},
      {'R', &Rover::right}};

  std::string command_queue("LMLMRMMRCR");
  std::vector<std::function<void()> > commands;

  std::string rover_str = "23 14 N";

  Rover r(3,3, 'N');

  auto it = boost::find_if(command_queue,
      [&](char c) { return command_map.find(c) == command_map.end(); });

  //  boost::copy(it,
  //     std::ostream_iterator<char>(cout, "\n"));

  //    [](char c) { std::cout << c << "\n"; });

  auto it2 =
    find_if(command_queue.begin(), command_queue.end(),
        [&](char c) { return command_map.find(c) == command_map.end(); });

  std::for_each(command_queue.begin(), it2,
      [&](char c) { command_map[c](r); });

  for (Direction d : { Direction::North, Direction::South,
                        Direction::East, Direction::West}) {
  std::cout << d << endl;
  }

  cout << "Making rover " << endl;
  RoverPtr p = make_rover("-13 27 E");
  if (p) {
    cout << "rover " << *p << endl;
  }

  return 0;
}


/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/

#include <string>
#include <boost/detail/lightweight_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>

namespace phoenix = boost::phoenix;

int
main()
{
    using phoenix::ref;
    using phoenix::val;
    using phoenix::arg_names::arg1;
    using phoenix::arg_names::arg2;
    using std::string;
    {   // From Phoenix 1.1 binary tests

        int i2 = 2, i3 = 3, i = 5;
        const char* world = " world";

        BOOST_TEST((ref(i) = ref(i))() == 5);
        BOOST_TEST((ref(i) = 3)() == 3);
        BOOST_TEST(i == 3);
        i = 5;
        int x, y, z;
        (ref(x) = ref(y) = ref(z) = 10)();
        BOOST_TEST(x == 10 && y == 10 && z == 10);
        BOOST_TEST((val(world)[3])() == world[3]);

        BOOST_TEST((ref(i) += 5)() == 10);
        BOOST_TEST((ref(i) -= 5)() == 5);
        BOOST_TEST((ref(i) *= 5)() == 25);
        BOOST_TEST((ref(i) /= 5)() == 5);
        BOOST_TEST((ref(i) %= 2)() == 1);

        BOOST_TEST((ref(i) <<= 3)() == 8);
        BOOST_TEST((ref(i) >>= 1)() == 4);
        BOOST_TEST((ref(i) |= 0xFF)() == 0xFF);
        BOOST_TEST((ref(i) &= 0xF0)() == 0xF0);
        BOOST_TEST((ref(i) ^= 0xFFFFFFFF)() == int(0xFFFFFF0F));

        BOOST_TEST((val(5) == val(5))());
        BOOST_TEST((val(5) == 5)());

        BOOST_TEST((arg1 + arg2 )(i2, i3) == i2 + i3 );
        BOOST_TEST((arg1 - arg2)(i2, i3) == i2 - i3);
        BOOST_TEST((arg1 * arg2)(i2, i3) == i2 * i3);
        BOOST_TEST((arg1 / arg2)(i2, i3) == i2 / i3);
        BOOST_TEST((arg1 % arg2)(i2, i3) == i2 % i3);
        BOOST_TEST((arg1 & arg2)(i2, i3) == (i2 & i3));
        BOOST_TEST((arg1 | arg2)(i2, i3) == (i2 | i3));
        BOOST_TEST((arg1 ^ arg2)(i2, i3) == (i2 ^ i3));
        BOOST_TEST((arg1 << arg2)(i2, i3) == i2 << i3);
        BOOST_TEST((arg1 >> arg2)(i2, i3) == i2 >> i3);

        BOOST_TEST((val(5) != val(6))());
        BOOST_TEST((val(5) < val(6))());
        BOOST_TEST(!(val(5) > val(6))());
        BOOST_TEST((val(5) < val(6))());
        BOOST_TEST((val(5) <= val(6))());
        BOOST_TEST((val(5) <= val(5))());
        BOOST_TEST((val(7) >= val(6))());
        BOOST_TEST((val(7) >= val(7))());

        BOOST_TEST((val(false) && val(false))() == false);
        BOOST_TEST((val(true) && val(false))() == false);
        BOOST_TEST((val(false) && val(true))() == false);
        BOOST_TEST((val(true) && val(true))() == true);

        BOOST_TEST((val(false) || val(false))() == false);
        BOOST_TEST((val(true) || val(false))() == true);
        BOOST_TEST((val(false) || val(true))() == true);
        BOOST_TEST((val(true) || val(true))() == true);
    }

    {  // From Phoenix 1.1 mixed_binary tests

        int     i1 = 1, i2 = 2, i50 = 50, i100 = 100;
        double  d2_5 = 2.5;
        string hello = "hello";
        const char* world = " world";

        BOOST_TEST((arg1 + arg2)(i100, i50) == (i100 + i50));
        BOOST_TEST((arg1 + 3)(i100) == (3 + i100));
        BOOST_TEST((arg1 + arg2)(hello, world) == "hello world");
        BOOST_TEST((arg1 + arg2)(i1, d2_5) == (i1 + d2_5));

        BOOST_TEST((*(arg1 + arg2))(world, i2) == *(world + i2));
        BOOST_TEST((*(arg1 + arg2))(i2, world) == *(i2 + world));
        BOOST_TEST((*(val(world+i2) - arg1))(i2) == *world);
    }

    return boost::report_errors();
}